feat(obstacle_cruise_planner)!: relax unknown stop (#1612)

yuki-takagi-66 · saka1-s · saka1-s · commit c87784d22fd3 · 2024-11-09T13:16:37.000+09:00
* chore: fix dependency of mult object tracker (#1567) fix dependency of mult object tracker * add relax logic Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * define params Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * add universe params Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * delete null line Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * fix spell Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * fix Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * change universe params Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> Co-authored-by: Shohei Sakai <saka1s.jp@gmail.com>
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/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: 6.0
+      preferred_acc_for_unknown: -6.0
diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/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_;
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
@@ -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<
diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -445,10 +445,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
       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 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam(
   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_);
diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -300,6 +300,19 @@ 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 (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 limit_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, limit_margin_stop_dist);
+      }
+    }
+
     if (suppress_sudden_obstacle_stop_) {
       const auto acceptable_stop_acc = [&]() -> std::optional<double> {
         if (stop_param_.getParamType(stop_obstacle.classification) == "default") {
