feat(obstacle_cruise_planner)!: relax unknown stop

perception/multi_object_tracker/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>eigen</depend>
   <depend>kalman_filter</depend>
   <depend>mussp</depend>

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -222,3 +222,5 @@
           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.
+      limit_margin_for_unknown: 3.0
+      preferred_acc_for_unknown: -3.0

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -110,6 +110,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   double additional_safe_distance_margin_on_curve_;
   double min_safe_distance_margin_on_curve_;
   bool suppress_sudden_obstacle_stop_;
+  double limit_margin_for_unknown_;
+  double preferred_acc_for_unknown_;
 
   std::vector<int> stop_obstacle_types_;
   std::vector<int> inside_cruise_obstacle_types_;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -65,7 +65,8 @@ class PlannerInterface
     const bool enable_debug_info, const bool enable_calculation_time_info,
     const double min_behavior_stop_margin, const double enable_approaching_on_curve,
     const double additional_safe_distance_margin_on_curve,
-    const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop)
+    const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop,
+    const double limit_margin_for_unknown, const double preferred_acc_for_unknown)
   {
     enable_debug_info_ = enable_debug_info;
     enable_calculation_time_info_ = enable_calculation_time_info;
@@ -74,6 +75,8 @@ class PlannerInterface
     additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve;
     min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve;
     suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop;
+    limit_margin_for_unknown_ = limit_margin_for_unknown;
+    preferred_acc_for_unknown_ = preferred_acc_for_unknown;
   }
 
   std::vector<TrajectoryPoint> generateStopTrajectory(
@@ -123,6 +126,8 @@ class PlannerInterface
   double additional_safe_distance_margin_on_curve_;
   double min_safe_distance_margin_on_curve_;
   bool suppress_sudden_obstacle_stop_;
+  double limit_margin_for_unknown_;
+  double preferred_acc_for_unknown_;
 
   // stop watch
   tier4_autoware_utils::StopWatch<

planning/obstacle_cruise_planner/src/node.cpp

@@ -445,10 +445,13 @@
       declare_parameter<double>("common.stop_on_curve.min_safe_distance_margin");
     suppress_sudden_obstacle_stop_ =
       declare_parameter<bool>("common.suppress_sudden_obstacle_stop");
+    limit_margin_for_unknown_ = declare_parameter<double>("stop.limit_margin_for_unknown");
+    preferred_acc_for_unknown_ = declare_parameter<double>("stop.preferred_acc_for_unknown");
     planner_ptr_->setParam(
       enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
       enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
-      min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
+      min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_,
+      preferred_acc_for_unknown_);
   }
 
   {  // stop/cruise/slow down obstacle type
@@ -496,11 +499,16 @@
   tier4_autoware_utils::updateParam<double>(
     parameters, "common.stop_on_curve.min_safe_distance_margin",
     min_safe_distance_margin_on_curve_);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "stop.limit_margin_for_unknown", limit_margin_for_unknown_);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "stop.preferred_acc_for_unknown", preferred_acc_for_unknown_);
 
   planner_ptr_->setParam(
     enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_,
     enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_,
-    min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_);
+    min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_,
+    preferred_acc_for_unknown_);
 
   tier4_autoware_utils::updateParam<bool>(
     parameters, "common.enable_slow_down_planning", enable_slow_down_planning_);
@@ -1185,7 +1193,7 @@
       if (!route_handler_->getLaneletMapPtr()->polygonLayer.exists(id)) {
         RCLCPP_DEBUG(
           rclcpp::get_logger("ObstacleCruisePlanner"),
           "Specified Lanelet polygon id [%ld] is not exsit in the map", id);
         continue;
       }
 

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -300,6 +300,19 @@
 
     // calc stop point against the obstacle
     double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin);
+    if (stop_obstacle.classification.label == ObjectClassification::UNKNOWN) {
+      const double pref_acc_stop_dist =
+        motion_utils::calcSignedArcLength(
+          planner_data.traj_points, 0, planner_data.ego_pose.position) +
+        calcMinimumDistanceToStop(
+          planner_data.ego_vel, longitudinal_info_.limit_max_accel, preferred_acc_for_unknown_);
+      const double liimit_margin_stop_dist =
+        std::max(0.0, dist_to_collide_on_ref_traj - limit_margin_for_unknown_);
+      if (pref_acc_stop_dist > candidate_zero_vel_dist) {
+        candidate_zero_vel_dist = std::min(pref_acc_stop_dist, liimit_margin_stop_dist);
+      }
+    }
+
     if (suppress_sudden_obstacle_stop_) {
       const auto acceptable_stop_acc = [&]() -> std::optional<double> {
         if (stop_param_.getParamType(stop_obstacle.classification) == "default") {