feat(obstacle_cruise_planner): add goal safe distance (autowarefounda…

…tion#2031)
tier4 · Oct 18, 2022 · 7fbd3df · 7fbd3df
1 parent 5853b52
commit 7fbd3df
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 8 deletions.
diff --git a/...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/...g/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]
 
       lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
 

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/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]
 
       lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-]
 

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/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

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -282,6 +282,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,
@@ -295,7 +297,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");

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -123,16 +123,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) {