fix(start/goal_planner): fix inverse order path points

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp

@@ -98,6 +98,21 @@ std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx);
 
+/**
+ * @brief removeInverseOrderPathPoints function
+ *
+ * This function is designed to handle a situation that can arise when shifting paths on a curve,
+ * where the positions of the path points may become inverted (i.e., a point further along the path
+ * comes to be located before an earlier point). It corrects for this by using the function
+ * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in
+ * the correct order (with the second point being 'forward' of the first). Any points which fail
+ * this test are omitted from the returned PathWithLaneId.
+ *
+ * @param path A path with points possibly in incorrect order.
+ * @return Returns a new PathWithLaneId that has all points in the correct order.
+ */
+PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path);
+
 }  // namespace behavior_path_planner::utils::start_goal_planner_common
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_

planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/goal_planner/util.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "motion_utils/trajectory/path_with_lane_id.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
@@ -171,6 +172,9 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     shifted_path.path.points.push_back(p);
   }
 
+  shifted_path.path =
+    utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path);
+
   // set the same z as the goal
   for (auto & p : shifted_path.path.points) {
     p.point.pose.position.z = goal_pose.position.z;

planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp

@@ -169,4 +169,19 @@ std::pair<double, double> getPairsTerminalVelocityAndAccel(
   return pairs_terminal_velocity_and_accel.at(current_path_idx);
 }
 
+PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path)
+{
+  PathWithLaneId fixed_path;
+  fixed_path.header = path.header;
+  fixed_path.points.push_back(path.points.at(0));
+  for (size_t i = 1; i < path.points.size(); i++) {
+    const auto p1 = path.points.at(i - 1).point.pose;
+    const auto p2 = path.points.at(i).point.pose;
+    if (tier4_autoware_utils::isDrivingForward(p1, p2)) {
+      fixed_path.points.push_back(path.points.at(i));
+    }
+  }
+  return fixed_path;
+}
+
 }  // namespace behavior_path_planner::utils::start_goal_planner_common

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "behavior_path_planner/utils/start_planner/util.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 #include "motion_utils/trajectory/path_with_lane_id.hpp"
@@ -298,6 +299,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       continue;
     }
 
+    shifted_path.path =
+      utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path);
+
     // set velocity
     const size_t pull_out_end_idx =
       findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position);