fix(obstacle_stop_planner): use common ego nearest search

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_stop_planner/README.md

@@ -48,13 +48,12 @@ When the deceleration section is inserted, the start point of the section is ins
 
 ### Common Parameter
 
-| Parameter               | Type   | Description                                                                              |
-| ----------------------- | ------ | ---------------------------------------------------------------------------------------- |
-| `enable_slow_down`      | bool   | enable slow down planner [-]                                                             |
-| `max_velocity`          | double | max velocity [m/s]                                                                       |
-| `hunting_threshold`     | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
-| `lowpass_gain`          | double | low pass gain for calculating acceleration [-]                                           |
-| `max_yaw_deviation_deg` | double | # maximum ego yaw deviation from trajectory [deg] (measures against overlapping lanes)   |
+| Parameter           | Type   | Description                                                                              |
+| ------------------- | ------ | ---------------------------------------------------------------------------------------- |
+| `enable_slow_down`  | bool   | enable slow down planner [-]                                                             |
+| `max_velocity`      | double | max velocity [m/s]                                                                       |
+| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
+| `lowpass_gain`      | double | low pass gain for calculating acceleration [-]                                           |
 
 ### Obstacle Stop Planner
 

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -8,7 +8,6 @@
       step_length: 1.0 # step length for pointcloud search range [m]
       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:

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -98,12 +98,12 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   struct NodeParam
   {
-    bool enable_slow_down;         // set True, slow down for obstacle beside the path
-    double max_velocity;           // max velocity [m/s]
-    double lowpass_gain;           // smoothing calculated current acceleration [-]
-    double hunting_threshold;      // keep slow down or stop state if obstacle vanished [s]
-    double max_yaw_deviation_rad;  // maximum ego yaw deviation from trajectory [rad] (measures
-                                   // against overlapping lanes)
+    bool enable_slow_down;              // set True, slow down for obstacle beside the path
+    double max_velocity;                // max velocity [m/s]
+    double lowpass_gain;                // smoothing calculated current acceleration [-]
+    double hunting_threshold;           // keep slow down or stop state if obstacle vanished [s]
+    double ego_nearest_dist_threshold;  // dist threshold for ego's nearest index
+    double ego_nearest_yaw_threshold;   // yaw threshold for ego's nearest index
   };
 
   struct StopParam

planning/obstacle_stop_planner/src/node.cpp

@@ -44,8 +44,8 @@ namespace motion_planning
 {
 using motion_utils::calcLongitudinalOffsetPose;
 using motion_utils::calcSignedArcLength;
-using motion_utils::findNearestIndex;
-using motion_utils::findNearestSegmentIndex;
+using motion_utils::findFirstNearestIndexWithSoftConstraints;
+using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints;
 using tier4_autoware_utils::calcAzimuthAngle;
 using tier4_autoware_utils::calcDistance2d;
 using tier4_autoware_utils::createPoint;
@@ -457,8 +457,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     p.hunting_threshold = declare_parameter("hunting_threshold", 0.5);
     p.lowpass_gain = declare_parameter("lowpass_gain", 0.9);
     lpf_acc_ = std::make_shared<LowpassFilter1d>(p.lowpass_gain);
-    const double max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 90.0);
-    p.max_yaw_deviation_rad = tier4_autoware_utils::deg2rad(max_yaw_deviation_deg);
+    p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
+    p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
   }
 
   {
@@ -817,6 +817,7 @@ void ObstacleStopPlannerNode::insertVelocity(
         index_with_dist_remain.get().first, output, index_with_dist_remain.get().second,
         stop_param);
 
+      const auto & ego_pose = planner_data.current_pose;
       const auto & ego_pos = planner_data.current_pose.position;
       const auto stop_point_distance =
         calcSignedArcLength(output, ego_pos, getPoint(stop_point.point));
@@ -827,7 +828,9 @@ void ObstacleStopPlannerNode::insertVelocity(
 
         if (ego_pos_on_path) {
           StopPoint current_stop_pos{};
-          current_stop_pos.index = findNearestSegmentIndex(output, ego_pos);
+          current_stop_pos.index = findFirstNearestSegmentIndexWithSoftConstraints(
+            output, ego_pose, node_param_.ego_nearest_dist_threshold,
+            node_param_.ego_nearest_yaw_threshold);
           current_stop_pos.point.pose = ego_pos_on_path.get();
 
           insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);
@@ -1316,14 +1319,10 @@ TrajectoryPoints ObstacleStopPlannerNode::trimTrajectoryWithIndexFromSelfPose(
 {
   TrajectoryPoints output{};
 
-  size_t min_distance_index = 0;
-  const auto nearest_index =
-    motion_utils::findNearestIndex(input, self_pose, 10.0, node_param_.max_yaw_deviation_rad);
-  if (!nearest_index) {
-    min_distance_index = motion_utils::findNearestIndex(input, self_pose.position);
-  } else {
-    min_distance_index = nearest_index.value();
-  }
+  const size_t min_distance_index = findFirstNearestIndexWithSoftConstraints(
+    input, self_pose, node_param_.ego_nearest_dist_threshold,
+    node_param_.ego_nearest_yaw_threshold);
+
   for (size_t i = min_distance_index; i < input.size(); ++i) {
     output.push_back(input.at(i));
   }