diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 099c33e6757ea..4a9a9c9e9aee3 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -222,8 +222,10 @@ labels: # For the listed types, the node try to read the following type specified values - "default" - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc unknown: limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. - sudden_definition_acc: -1.0 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". - sudden_definition_dist: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 80e3d29dadf23..72610e1219d58 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -93,7 +93,7 @@ class PlannerInterface updateCommonParam(parameters); updateCruiseParam(parameters); slow_down_param_.onParam(parameters); - stop_param_.onParam(parameters); + stop_param_.onParam(parameters, longitudinal_info_); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -341,8 +341,8 @@ class PlannerInterface struct ObstacleSpecificParams { double limit_min_acc; - double sudden_definition_dist; - double sudden_definition_acc; + double sudden_object_acc_threshold; + double sudden_object_dist_threshold; bool abandon_to_stop; }; const std::unordered_map types_maps = { @@ -360,18 +360,24 @@ class PlannerInterface for (const auto & type_str : obstacle_labels) { if (type_str != "default") { - const ObstacleSpecificParams param{ - std::min( - node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), - longitudinal_info.min_accel), - node.declare_parameter(param_prefix + type_str + ".sudden_definition_dist"), - node.declare_parameter(param_prefix + type_str + ".sudden_definition_acc"), + ObstacleSpecificParams param{ + node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_acc_threshold"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_dist_threshold"), node.declare_parameter(param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + type_specified_param_list.emplace(type_str, param); } } } - void onParam(const std::vector & parameters) + void onParam( + const std::vector & parameters, const LongitudinalInfo & longitudinal_info) { const std::string param_prefix = "stop.type_specified_params."; for (auto & [type_str, param] : type_specified_param_list) { @@ -381,13 +387,17 @@ class PlannerInterface tier4_autoware_utils::updateParam( parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); tier4_autoware_utils::updateParam( - parameters, param_prefix + type_str + ".sudden_definition_dist", - param.sudden_definition_dist); + parameters, param_prefix + type_str + ".sudden_object_acc_threshold", + param.sudden_object_acc_threshold); tier4_autoware_utils::updateParam( - parameters, param_prefix + type_str + ".sudden_definition_acc", - param.sudden_definition_acc); + parameters, param_prefix + type_str + ".sudden_object_dist_threshold", + param.sudden_object_dist_threshold); tier4_autoware_utils::updateParam( parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); } } std::string getParamType(const ObjectClassification label) diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 0f9ec0d32b3dc..6e1de97378d16 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -308,8 +308,8 @@ std::vector PlannerInterface::generateStopTrajectory( const double distance_to_judge_suddenness = std::min( calcMinimumDistanceToStop( planner_data.ego_vel, longitudinal_info_.limit_max_accel, - stop_param_.getParam(stop_obstacle.classification).sudden_definition_acc), - stop_param_.getParam(stop_obstacle.classification).sudden_definition_dist); + stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), + stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); if (candidate_zero_vel_dist > distance_to_judge_suddenness) { return longitudinal_info_.limit_min_accel; }