From f95a0b73d02aff427510d1fe0e65ff3483a89039 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 6 Dec 2023 08:34:45 +0900 Subject: [PATCH] Add jerk and acc limits for slow-down Signed-off-by: Daniel Sanchez --- .../config/default_common.param.yaml | 4 ++++ .../include/obstacle_cruise_planner/common_structs.hpp | 7 +++++++ planning/obstacle_cruise_planner/src/planner_interface.cpp | 4 ++-- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/default_common.param.yaml b/planning/obstacle_cruise_planner/config/default_common.param.yaml index 6bb130e8052b0..f900d309123cf 100644 --- a/planning/obstacle_cruise_planner/config/default_common.param.yaml +++ b/planning/obstacle_cruise_planner/config/default_common.param.yaml @@ -7,6 +7,10 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] + slow_down: + min_acc: -1.0 # min slowdown deceleration [m/ss] + min_jerk: -1.0 # min slowdown jerk [m/sss] + # constraints to be observed limit: min_acc: -2.5 # min deceleration limit [m/ss] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index c6cb73a680e5a..8adb95618b232 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -166,6 +166,8 @@ struct LongitudinalInfo limit_min_accel = node.declare_parameter("limit.min_acc"); limit_max_jerk = node.declare_parameter("limit.max_jerk"); limit_min_jerk = node.declare_parameter("limit.min_jerk"); + slow_down_min_accel = node.declare_parameter("slow_down.min_acc"); + slow_down_min_jerk = node.declare_parameter("slow_down.min_jerk"); idling_time = node.declare_parameter("common.idling_time"); min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); @@ -191,6 +193,9 @@ struct LongitudinalInfo tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + tier4_autoware_utils::updateParam( + parameters, "slow_down.min_accel", slow_down_min_accel); + tier4_autoware_utils::updateParam(parameters, "slow_down.min_jerk", slow_down_min_jerk); tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); tier4_autoware_utils::updateParam( @@ -214,6 +219,8 @@ struct LongitudinalInfo double min_accel; double max_jerk; double min_jerk; + double slow_down_min_jerk; + double slow_down_min_accel; double limit_max_accel; double limit_min_accel; double limit_max_jerk; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 47f62016df53b..a97e3ae7d5c4f 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -790,8 +790,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( } // TODO(murooka) Calculate more precisely. Final acceleration should be zero. const double min_feasible_slow_down_vel = calcDecelerationVelocityFromDistanceToTarget( - longitudinal_info_.min_jerk, longitudinal_info_.min_accel, planner_data.ego_acc, - planner_data.ego_vel, deceleration_dist); + longitudinal_info_.slow_down_min_jerk, longitudinal_info_.slow_down_min_accel, + planner_data.ego_acc, planner_data.ego_vel, deceleration_dist); return min_feasible_slow_down_vel; }(); if (prev_output) {