feat(obstacle_stop_planner): add hold stop margin distance (backport #…

…1434) (#210)

* feat(obstacle_stop_planner): add hold stop margin distance (#1434)

* feat(obstacle_stop_planner): add hold stop margin distance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(obstacle_stop_planner): output distance info

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(obstacle_stop_planner): add param into config

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix: replace motion_utils to tier4_autoware_utils

* ci(pre-commit): autofix

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -37,9 +37,13 @@ namespace motion_planning
 {
 using tier4_autoware_utils::calcAzimuthAngle;
 using tier4_autoware_utils::calcDistance2d;
+using tier4_autoware_utils::calcLongitudinalOffsetPose;
 using tier4_autoware_utils::calcSignedArcLength;
 using tier4_autoware_utils::createPoint;
 using tier4_autoware_utils::findNearestIndex;
+using tier4_autoware_utils::findNearestSegmentIndex;
+using tier4_autoware_utils::getPoint;
+using tier4_autoware_utils::getPose;
 using tier4_autoware_utils::getRPY;
 
 namespace
@@ -463,6 +467,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);
   }
 
   {
@@ -753,6 +758,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;
@@ -765,10 +772,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);
+      }
     }
   }
 
@@ -788,8 +826,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,
@@ -918,7 +956,6 @@ void ObstacleStopPlannerNode::insertStopPoint(
   }
 
   stop_reason_diag = makeStopReasonDiag("obstacle", p_insert.pose);
-  debug_ptr_->pushPose(p_insert.pose, PoseType::Stop);
 }
 
 StopPoint ObstacleStopPlannerNode::searchInsertPoint(