fix(goal_planner): fix zero velocity in middle of path (autowarefound…

…ation#8563)

* fix(goal_planner): fix zero velocity in middle of path

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add comment

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -84,12 +84,31 @@ bool isReferencePath(
 std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose);
 PathWithLaneId cropForwardPoints(
   const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length);
+
+/**
+ * @brief extend target_path by extend_length
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_length length to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const double extend_length);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const double extend_length, const bool remove_connected_zero_velocity);
+/**
+ * @brief extend target_path to extend_pose
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_pose pose to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose, const bool remove_connected_zero_velocity);
 
 std::vector<Polygon2d> createPathFootPrints(
   const PathWithLaneId & path, const double base_to_front, const double base_to_rear,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -283,6 +283,10 @@ void GoalPlannerModule::onTimer()
       RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path");
       return true;
     }
+    // TODO(someone): The generated path inherits the velocity of the path of the previous module.
+    // Therefore, if the velocity of the path of the previous module changes (e.g. stop points are
+    // inserted, deleted), the path should be regenerated.
+
     return false;
   });
   if (!need_update) {
@@ -1628,8 +1632,12 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
     const double s_end = s_current + common_parameters.forward_path_length;
     return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
   });
+
+  // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it by
+  // setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is the
+  // role of the goal planner, and the intermediate zero velocity after extension is unnecessary.
   const auto extended_prev_path = goal_planner_utils::extendPath(
-    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length);
+    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length, true);
 
   // calculate search start offset pose from the closest goal candidate pose with
   // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp

@@ -148,8 +148,12 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
       lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
       lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
     if (extend_previous_module_path) {  // case1
+      // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it
+      // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is
+      // the role of the goal planner, and the intermediate zero velocity after extension is
+      // unnecessary.
       return goal_planner_utils::extendPath(
-        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose);
+        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true);
     } else {  // case2
       return goal_planner_utils::cropPath(prev_module_path, shift_end_pose);
     }

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -389,7 +389,7 @@ PathWithLaneId cropForwardPoints(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const double extend_length)
+  const double extend_length, const bool remove_connected_zero_velocity)
 {
   const auto & target_terminal_pose = target_path.points.back().point.pose;
 
@@ -409,6 +409,11 @@ PathWithLaneId extendPath(
   }
 
   auto extended_path = target_path;
+  auto & target_terminal_vel = extended_path.points.back().point.longitudinal_velocity_mps;
+  if (remove_connected_zero_velocity && target_terminal_vel < 0.01) {
+    target_terminal_vel = clipped_path.points.front().point.longitudinal_velocity_mps;
+  }
+
   const auto start_point =
     std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) {
       const bool is_forward =
@@ -427,15 +432,15 @@ PathWithLaneId extendPath(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose)
+  const Pose & extend_pose, const bool remove_connected_zero_velocity)
 {
   const auto & target_terminal_pose = target_path.points.back().point.pose;
   const size_t target_path_terminal_idx = autoware::motion_utils::findNearestSegmentIndex(
     reference_path.points, target_terminal_pose.position);
   const double extend_distance = autoware::motion_utils::calcSignedArcLength(
     reference_path.points, target_path_terminal_idx, extend_pose.position);
 
-  return extendPath(target_path, reference_path, extend_distance);
+  return extendPath(target_path, reference_path, extend_distance, remove_connected_zero_velocity);
 }
 
 std::vector<Polygon2d> createPathFootPrints(