update

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -341,7 +341,8 @@ class PlannerInterface
     struct ObstacleSpecificParams
     {
       double limit_min_acc;
-      double limit_max_dist;
+      double sudden_dist;
+      double sudden_acc;
       bool abandon_to_stop;
     };
     const std::unordered_map<uint8_t, std::string> types_maps = {
@@ -358,16 +359,13 @@ class PlannerInterface
         node.declare_parameter<std::vector<std::string>>(param_prefix + "labels", obstacle_labels);
 
       for (const auto & type_str : obstacle_labels) {
-        if (type_str == "default") {
-          const ObstacleSpecificParams default_param{
-            longitudinal_info.limit_min_accel, std::numeric_limits<double>::max(), false};
-          type_specified_param_list.emplace(type_str, default_param);
-        } else {
+        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 + ".limit_max_dist"),
+            node.declare_parameter<double>(param_prefix + type_str + ".sudden_dist"),
+            node.declare_parameter<double>(param_prefix + type_str + ".sudden_acc"),
             node.declare_parameter<bool>(param_prefix + type_str + ".abandon_to_stop")};
           type_specified_param_list.emplace(type_str, param);
         }
@@ -383,7 +381,9 @@ 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 + ".limit_max_dist", param.limit_max_dist);
+          parameters, param_prefix + type_str + ".sudden__dist", param.sudden_dist);
+        tier4_autoware_utils::updateParam<double>(
+          parameters, param_prefix + type_str + ".sudden__acc", param.sudden_acc);
         tier4_autoware_utils::updateParam<bool>(
           parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop);
       }

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -301,27 +301,38 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     // calc stop point against the obstacle
     double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin);
     if (suppress_sudden_obstacle_stop_) {
-      const double acceptable_stop_dist = std::min(
-        calcMinimumDistanceToStop(
-          planner_data.ego_vel, longitudinal_info_.limit_max_accel,
-          stop_param_.getParam(stop_obstacle.classification).limit_min_acc),
-        stop_param_.getParam(stop_obstacle.classification).limit_max_dist);
-
-      const double acceptable_stop_pos =
-        motion_utils::calcSignedArcLength(
-          planner_data.traj_points, 0, planner_data.ego_pose.position) +
-        acceptable_stop_dist;
-
-      if (acceptable_stop_pos > candidate_zero_vel_dist) {
-        if (
-          stop_param_.getParam(stop_obstacle.classification).abandon_to_stop &&
-          acceptable_stop_pos > dist_to_collide_on_ref_traj) {
+      const auto acceptable_stop_acc = [&]() -> std::optional<double> {
+        if (stop_param_.getParamType(stop_obstacle.classification) == "default") {
+          return longitudinal_info_.limit_min_accel;
+        }
+        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_acc),
+          stop_param_.getParam(stop_obstacle.classification).sudden_dist);
+        if (candidate_zero_vel_dist > distance_to_judge_suddenness) {
+          return longitudinal_info_.limit_min_accel;
+        }
+        if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) {
           RCLCPP_WARN(
             rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"),
             "[Cruise] abandon to stop against %s object",
             stop_param_.types_maps.at(stop_obstacle.classification.label).c_str());
-          continue;
+          return std::nullopt;
+        } else {
+          return stop_param_.getParam(stop_obstacle.classification).limit_min_acc;
         }
+      }();
+      if (!acceptable_stop_acc) {
+        continue;
+      }
+
+      const double acceptable_stop_pos =
+        motion_utils::calcSignedArcLength(
+          planner_data.traj_points, 0, planner_data.ego_pose.position) +
+        calcMinimumDistanceToStop(
+          planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value());
+      if (acceptable_stop_pos > candidate_zero_vel_dist) {
         candidate_zero_vel_dist = acceptable_stop_pos;
       }
     }