feat(obstacle_cruise_planner): add goal safe distance (#2031)

Author: purewater0901

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml

@@ -10,6 +10,7 @@
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
+      terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -10,6 +10,7 @@
       min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
       min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
+      terminal_safe_distance_margin : 3.0 # Stop margin at the goal [m]
 
       nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
       nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -109,6 +109,7 @@ struct LongitudinalInfo
   double min_ego_accel_for_rss;
   double min_object_accel_for_rss;
   double safe_distance_margin;
+  double terminal_safe_distance_margin;
 };
 
 struct DebugData

planning/obstacle_cruise_planner/src/node.cpp

@@ -245,6 +245,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
       declare_parameter<double>("common.min_object_accel_for_rss");
     const double idling_time = declare_parameter<double>("common.idling_time");
     const double safe_distance_margin = declare_parameter<double>("common.safe_distance_margin");
+    const double terminal_safe_distance_margin =
+      declare_parameter<double>("common.terminal_safe_distance_margin");
 
     return LongitudinalInfo{
       max_accel,
@@ -258,7 +260,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
       idling_time,
       min_ego_accel_for_rss,
       min_object_accel_for_rss,
-      safe_distance_margin};
+      safe_distance_margin,
+      terminal_safe_distance_margin};
   }();
 
   is_showing_debug_info_ = declare_parameter<bool>("common.is_showing_debug_info");

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -140,16 +140,29 @@ Trajectory PlannerInterface::generateStopTrajectory(
     const auto closest_behavior_stop_idx =
       motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1);
 
-    if (
-      closest_behavior_stop_idx &&
-      closest_behavior_stop_idx != planner_data.traj.points.size() - 1) {
-      const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
-        planner_data.traj.points, planner_data.current_pose.position, nearest_segment_idx,
-        *closest_behavior_stop_idx);
+    if (!closest_behavior_stop_idx) {
+      return longitudinal_info_.safe_distance_margin;
+    }
+
+    const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength(
+      planner_data.traj.points, planner_data.current_pose.position, nearest_segment_idx,
+      *closest_behavior_stop_idx);
+
+    if (*closest_behavior_stop_idx == planner_data.traj.points.size() - 1) {
+      // Closest behavior stop point is the end point
+      const double closest_obstacle_stop_dist_from_ego =
+        closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin -
+        abs_ego_offset;
+      const double stop_dist_diff =
+        closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
+      if (stop_dist_diff < 0.5) {
+        // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
+        return longitudinal_info_.terminal_safe_distance_margin;
+      }
+    } else {
       const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego -
                                                          longitudinal_info_.safe_distance_margin -
                                                          abs_ego_offset;
-
       const double stop_dist_diff =
         closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego;
       if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) {