feat(obstacle_cruise_planner): add a hold stop distance feature (auto…

…warefoundation#4720)

* feat(obstacle_cruise_planner): add a hold stop distance feature

Signed-off-by: Takayuki Murooka <[email protected]>

* add param

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -14,6 +14,8 @@
       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. This value cannot exceed safe distance margin. [m]
+      hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
+      hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [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

@@ -165,6 +165,11 @@ struct LongitudinalInfo
     safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
     terminal_safe_distance_margin =
       node.declare_parameter<double>("common.terminal_safe_distance_margin");
+
+    hold_stop_velocity_threshold =
+      node.declare_parameter<double>("common.hold_stop_velocity_threshold");
+    hold_stop_distance_threshold =
+      node.declare_parameter<double>("common.hold_stop_distance_threshold");
   }
 
   void onParam(const std::vector<rclcpp::Parameter> & parameters)
@@ -188,6 +193,11 @@ struct LongitudinalInfo
       parameters, "common.safe_distance_margin", safe_distance_margin);
     tier4_autoware_utils::updateParam<double>(
       parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
+
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold);
   }
 
   // common parameter
@@ -208,6 +218,10 @@ struct LongitudinalInfo
   // distance margin
   double safe_distance_margin;
   double terminal_safe_distance_margin;
+
+  // hold stop
+  double hold_stop_velocity_threshold;
+  double hold_stop_distance_threshold;
 };
 
 struct DebugData

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -254,6 +254,11 @@ class PlannerInterface
   SlowDownParam slow_down_param_;
 
   std::vector<SlowDownOutput> prev_slow_down_output_;
+  // previous trajectory and distance to stop
+  // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or
+  // crossing lanes.
+  std::optional<std::pair<std::vector<TrajectoryPoint>, double>> prev_stop_distance_info_{
+    std::nullopt};
 };
 
 #endif  // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -228,6 +228,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
       motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
 
+    prev_stop_distance_info_ = std::nullopt;
     return planner_data.traj_points;
   }
 
@@ -244,6 +245,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
       motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
     tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
 
+    prev_stop_distance_info_ = std::nullopt;
     return planner_data.traj_points;
   }
 
@@ -315,9 +317,36 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
   }
 
   // Generate Output Trajectory
+  const double zero_vel_dist = [&]() {
+    const double current_zero_vel_dist =
+      std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle);
+
+    // Hold previous stop distance if necessary
+    if (
+      std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
+      prev_stop_distance_info_) {
+      // NOTE: We assume that the current trajectory's front point is ahead of the previous
+      // trajectory's front point.
+      const size_t traj_front_point_prev_seg_idx =
+        motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+          prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
+      const double diff_dist_front_points = motion_utils::calcSignedArcLength(
+        prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
+        traj_front_point_prev_seg_idx);
+
+      const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
+      if (
+        std::abs(prev_zero_vel_dist - current_zero_vel_dist) <
+        longitudinal_info_.hold_stop_distance_threshold) {
+        return prev_zero_vel_dist;
+      }
+    }
+
+    return current_zero_vel_dist;
+  }();
+
+  // Insert stop point
   auto output_traj_points = planner_data.traj_points;
-  const double zero_vel_dist =
-    std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle);
   const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points);
   if (zero_vel_idx) {
     // virtual wall marker for stop obstacle
@@ -338,6 +367,8 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     const auto stop_speed_exceeded_msg =
       createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
     stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);
+
+    prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist);
   }
 
   stop_planning_debug_info_.set(