fix(goal_planner): add buffer to stop_pose for restarting (#3801)

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -161,7 +161,9 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // approximate distance from the start point to the end point of pull_over.
   // this is used as an assumed value to decelerate, etc., before generating the actual path.
-  double approximate_pull_over_distance_{20.0};
+  const double approximate_pull_over_distance_{20.0};
+  // ego may exceed the stop distance, so add a buffer
+  const double stop_distance_buffer_{2.0};
 
   // for parking policy
   bool left_side_parking_{true};
@@ -186,7 +188,8 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // stop or decelerate
   void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
-  void decelerateBeforeSearchStart(const Pose & search_start_pose, PathWithLaneId & path) const;
+  void decelerateBeforeSearchStart(
+    const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath();
   PathWithLaneId generateFeasibleStopPath();
   boost::optional<double> calcFeasibleDecelDistance(const double target_velocity) const;

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -539,13 +539,14 @@ void GoalPlannerModule::selectSafePullOverPath()
 
   // decelerate before the search area start
   if (status_.is_safe) {
-    const auto search_start_pose = calcLongitudinalOffsetPose(
+    const auto search_start_offset_pose = calcLongitudinalOffsetPose(
       status_.pull_over_path->getFullPath().points, refined_goal_pose_.position,
-      -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front);
+      -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front -
+        approximate_pull_over_distance_);
     auto & first_path = status_.pull_over_path->partial_paths.front();
 
-    if (search_start_pose) {
-      decelerateBeforeSearchStart(*search_start_pose, first_path);
+    if (search_start_offset_pose) {
+      decelerateBeforeSearchStart(*search_start_offset_pose, first_path);
     } else {
       // if already passed the search start pose, set pull_over_velocity to first_path.
       for (auto & p : first_path.points) {
@@ -577,8 +578,10 @@ void GoalPlannerModule::setLanes()
 void GoalPlannerModule::setOutput(BehaviorModuleOutput & output)
 {
   if (status_.is_safe) {
-    // safe: use pull over path
-    status_.stop_pose.reset();
+    // clear stop pose when the path is safe and activated
+    if (isActivated()) {
+      status_.stop_pose.reset();
+    }
 
     // keep stop if not enough time passed,
     // because it takes time for the trajectory to be reflected
@@ -904,24 +907,26 @@ PathWithLaneId GoalPlannerModule::generateStopPath()
   // stop point priority is
   // 1. actual start pose
   // 2. closest candidate start pose
-  // 3. search start pose
+  // 3. pose offset by approximate_pull_over_distance_ from search start pose.
   //     (In the case of the curve lane, the position is not aligned due to the
   //     difference between the outer and inner sides)
   // 4. feasible stop
-  const auto search_start_pose = calcLongitudinalOffsetPose(
+  const auto search_start_offset_pose = calcLongitudinalOffsetPose(
     reference_path.points, refined_goal_pose_.position,
-    -parameters_->backward_goal_search_length - common_parameters.base_link2front);
-  if (!status_.is_safe && !closest_start_pose_ && !search_start_pose) {
+    -parameters_->backward_goal_search_length - common_parameters.base_link2front -
+      approximate_pull_over_distance_);
+  if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) {
     return generateFeasibleStopPath();
   }
-  const Pose stop_pose =
-    status_.is_safe ? status_.pull_over_path->start_pose
-                    : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose);
+
+  const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose
+                                         : (closest_start_pose_ ? closest_start_pose_.value()
+                                                                : *search_start_offset_pose);
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
   const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose);
   const auto min_stop_distance = calcFeasibleDecelDistance(0.0);
-  if (min_stop_distance && ego_to_stop_distance < *min_stop_distance) {
+  if (min_stop_distance && ego_to_stop_distance + stop_distance_buffer_ < *min_stop_distance) {
     return generateFeasibleStopPath();
   }
 
@@ -930,8 +935,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath()
   status_.stop_pose = stop_pose;
 
   // slow down before the search area.
-  if (search_start_pose) {
-    decelerateBeforeSearchStart(*search_start_pose, reference_path);
+  if (search_start_offset_pose) {
+    decelerateBeforeSearchStart(*search_start_offset_pose, reference_path);
   } else {
     // if already passed the search start pose, set pull_over_velocity to reference_path.
     for (auto & p : reference_path.points) {
@@ -1132,15 +1137,19 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const double current_vel = planner_data_->self_odometry->twist.twist.linear.x;
 
-  // once stopped, the vehicle cannot start again if start_pose is close.
+  // when the path is separated and start_pose is close,
+  // once stopped, the vehicle cannot start again.
   // so need enough distance to restart.
   // distance to restart should be less than decide_path_distance.
   // otherwise, the goal would change immediately after departure.
+  const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1;
   constexpr double eps_vel = 0.01;
   const double distance_to_start = calcSignedArcLength(
     pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position);
   const double distance_to_restart = parameters_->decide_path_distance / 2;
-  if (std::abs(current_vel) < eps_vel && distance_to_start < distance_to_restart) {
+  if (
+    is_separated_path && std::abs(current_vel) < eps_vel &&
+    distance_to_start < distance_to_restart) {
     return false;
   }
 
@@ -1149,7 +1158,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c
     return false;
   }
 
-  if (distance_to_start < *current_to_stop_distance) {
+  if (distance_to_start + stop_distance_buffer_ < *current_to_stop_distance) {
     return false;
   }
 
@@ -1250,15 +1259,16 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith
 }
 
 void GoalPlannerModule::decelerateBeforeSearchStart(
-  const Pose & search_start_pose, PathWithLaneId & path) const
+  const Pose & search_start_offset_pose, PathWithLaneId & path) const
 {
   const double pull_over_velocity = parameters_->pull_over_velocity;
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
 
   // slow down before the search area.
   const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity);
   if (min_decel_distance) {
-    const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_pose);
+    const double distance_to_search_start =
+      calcSignedArcLengthFromEgo(path, search_start_offset_pose);
     const double distance_to_decel =
       std::max(*min_decel_distance, distance_to_search_start - approximate_pull_over_distance_);
     insertDecelPoint(current_pose.position, distance_to_decel, pull_over_velocity, path.points);