From 75695cd68c583114703f6a81b045090c5a13467a Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 11 Dec 2023 10:55:20 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): add jerk and acc limits for slow-down (#5810) Add jerk and acc limits for slow-down Signed-off-by: Daniel Sanchez Signed-off-by: karishma --- .../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 27b2945f614d2..6c44c4744e96c 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 @@ -172,6 +172,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"); @@ -197,6 +199,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( @@ -220,6 +225,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 2dc1247630eaf..a7dac94e1c38a 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -763,8 +763,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) {