Revert "fix(goal_planner): fix zero velocity in middle of path (autowarefoundation#8563)"

This reverts commit 0c9046b.

Author: TetsuKawa

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

@@ -82,31 +82,12 @@ 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 & 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
- */
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const double extend_length);
 PathWithLaneId extendPath(
-  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose, const bool remove_connected_zero_velocity);
+  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose);
 
 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,10 +283,6 @@ 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) {
@@ -1672,12 +1668,8 @@ 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, true);
+    getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length);
 
   // 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,12 +148,8 @@ 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, true);
+        prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose);
     } 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

@@ -388,7 +388,7 @@ PathWithLaneId cropForwardPoints(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const double extend_length, const bool remove_connected_zero_velocity)
+  const double extend_length)
 {
   const auto & target_terminal_pose = target_path.points.back().point.pose;
 
@@ -408,11 +408,6 @@ 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 =
@@ -431,15 +426,15 @@ PathWithLaneId extendPath(
 
 PathWithLaneId extendPath(
   const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose, const bool remove_connected_zero_velocity)
+  const Pose & extend_pose)
 {
   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, remove_connected_zero_velocity);
+  return extendPath(target_path, reference_path, extend_distance);
 }
 
 std::vector<Polygon2d> createPathFootPrints(