update

Signed-off-by: Yuki Takagi <[email protected]>

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.

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<uint8_t, std::string> 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<double>(param_prefix + type_str + ".limit_min_acc"),
-              longitudinal_info.min_accel),
-            node.declare_parameter<double>(param_prefix + type_str + ".sudden_definition_dist"),
-            node.declare_parameter<double>(param_prefix + type_str + ".sudden_definition_acc"),
+          ObstacleSpecificParams param{
+            node.declare_parameter<double>(param_prefix + type_str + ".limit_min_acc"),
+            node.declare_parameter<double>(
+              param_prefix + type_str + ".sudden_object_acc_threshold"),
+            node.declare_parameter<double>(
+              param_prefix + type_str + ".sudden_object_dist_threshold"),
             node.declare_parameter<bool>(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<rclcpp::Parameter> & parameters)
+    void onParam(
+      const std::vector<rclcpp::Parameter> & 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<double>(
           parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc);
         tier4_autoware_utils::updateParam<double>(
-          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<double>(
-          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<bool>(
           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)

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -308,8 +308,8 @@ std::vector<TrajectoryPoint> 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;
         }