feat(obstacle_stop_planner): add hold stop margin distance

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -7,6 +7,7 @@
       extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
       expand_stop_range: 0.0 # margin of vehicle footprint [m]
       max_yaw_deviation_deg: 90.0 # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes)
+      hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m]
 
     slow_down_planner:
       # slow down planner parameters

planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp

@@ -51,11 +51,12 @@ class DebugValues
     CURRENT_VEL = 0,
     CURRENT_ACC = 1,
     CURRENT_FORWARD_MARGIN = 2,
-    OBSTACLE_DISTANCE = 3,
-    FLAG_FIND_COLLISION_OBSTACLE = 4,
-    FLAG_FIND_SLOW_DOWN_OBSTACLE = 5,
-    FLAG_ADAPTIVE_CRUISE = 6,
-    FLAG_EXTERNAL = 7,
+    SLOWDOWN_OBSTACLE_DISTANCE = 3,
+    COLLISION_OBSTACLE_DISTANCE = 4,
+    FLAG_FIND_COLLISION_OBSTACLE = 5,
+    FLAG_FIND_SLOW_DOWN_OBSTACLE = 6,
+    FLAG_ADAPTIVE_CRUISE = 7,
+    FLAG_EXTERNAL = 8,
     SIZE
   };
 

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -108,12 +108,13 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   struct StopParam
   {
-    double stop_margin;               // stop margin distance from obstacle on the path [m]
-    double min_behavior_stop_margin;  // margin distance, any other stop point is inserted [m]
-    double expand_stop_range;         // margin of vehicle footprint [m]
-    double extend_distance;           // trajectory extend_distance [m]
-    double step_length;               // step length for pointcloud search range [m]
-    double stop_search_radius;        // search radius for obstacle point cloud [m]
+    double stop_margin;                // stop margin distance from obstacle on the path [m]
+    double min_behavior_stop_margin;   // margin distance, any other stop point is inserted [m]
+    double expand_stop_range;          // margin of vehicle footprint [m]
+    double extend_distance;            // trajectory extend_distance [m]
+    double step_length;                // step length for pointcloud search range [m]
+    double stop_search_radius;         // search radius for obstacle point cloud [m]
+    double hold_stop_margin_distance;  // keep stopping if the ego is in this margin [m]
   };
 
   struct SlowDownParam

planning/obstacle_stop_planner/src/node.cpp

@@ -42,11 +42,15 @@
 
 namespace motion_planning
 {
+using motion_utils::calcLongitudinalOffsetPose;
 using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using motion_utils::findNearestSegmentIndex;
 using tier4_autoware_utils::calcAzimuthAngle;
 using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::createPoint;
+using tier4_autoware_utils::getPoint;
+using tier4_autoware_utils::getPose;
 using tier4_autoware_utils::getRPY;
 
 namespace
@@ -470,6 +474,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     p.stop_search_radius =
       p.step_length +
       std::hypot(i.vehicle_width_m / 2.0 + p.expand_stop_range, i.vehicle_length_m / 2.0);
+    p.hold_stop_margin_distance = declare_parameter(ns + "hold_stop_margin_distance", 0.0);
   }
 
   {
@@ -773,6 +778,8 @@ void ObstacleStopPlannerNode::insertVelocity(
   const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
   const double current_acc, const double current_vel, const StopParam & stop_param)
 {
+  const auto & base_link2front = vehicle_info.max_longitudinal_offset_m;
+
   if (planner_data.stop_require) {
     // insert stop point
     const auto traj_end_idx = output.size() - 1;
@@ -785,10 +792,41 @@ void ObstacleStopPlannerNode::insertVelocity(
         planner_data.nearest_collision_point.x, planner_data.nearest_collision_point.y, 0));
 
     if (index_with_dist_remain) {
+      const auto vehicle_idx = std::min(planner_data.trajectory_trim_index, traj_end_idx);
+      const auto dist_baselink_to_obstacle =
+        calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first);
+
+      debug_ptr_->setDebugValues(
+        DebugValues::TYPE::COLLISION_OBSTACLE_DISTANCE,
+        dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front);
+
       const auto stop_point = searchInsertPoint(
         index_with_dist_remain.get().first, output, index_with_dist_remain.get().second,
         stop_param);
-      insertStopPoint(stop_point, output, planner_data.stop_reason_diag);
+
+      const auto & ego_pos = planner_data.current_pose.position;
+      const auto stop_point_distance =
+        calcSignedArcLength(output, ego_pos, getPoint(stop_point.point));
+      const auto is_stopped = current_vel < 0.01;
+
+      if (stop_point_distance < stop_param_.hold_stop_margin_distance && is_stopped) {
+        const auto ego_pos_on_path = calcLongitudinalOffsetPose(output, ego_pos, 0.0);
+
+        if (ego_pos_on_path) {
+          StopPoint current_stop_pos{};
+          current_stop_pos.index = findNearestSegmentIndex(output, ego_pos);
+          current_stop_pos.point.pose = ego_pos_on_path.get();
+
+          insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);
+
+          debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
+        }
+
+      } else {
+        insertStopPoint(stop_point, output, planner_data.stop_reason_diag);
+
+        debug_ptr_->pushPose(getPose(stop_point.point), PoseType::Stop);
+      }
     }
   }
 
@@ -808,8 +846,8 @@ void ObstacleStopPlannerNode::insertVelocity(
         calcSignedArcLength(output, vehicle_idx, index_with_dist_remain.get().first);
 
       debug_ptr_->setDebugValues(
-        DebugValues::TYPE::OBSTACLE_DISTANCE,
-        dist_baselink_to_obstacle + index_with_dist_remain.get().second);
+        DebugValues::TYPE::SLOWDOWN_OBSTACLE_DISTANCE,
+        dist_baselink_to_obstacle + index_with_dist_remain.get().second - base_link2front);
       const auto slow_down_section = createSlowDownSection(
         index_with_dist_remain.get().first, output, planner_data.lateral_deviation,
         index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc,
@@ -940,7 +978,6 @@ void ObstacleStopPlannerNode::insertStopPoint(
   }
 
   stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose);
-  debug_ptr_->pushPose(p_insert.pose, PoseType::Stop);
 }
 
 StopPoint ObstacleStopPlannerNode::searchInsertPoint(