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

…ation#4950) fix(start_planner): fix inverse order path points Signed-off-by: kosuke55 <[email protected]>
tier4 · Sep 12, 2023 · f02176d · f02176d
1 parent 5129805
commit f02176d
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 0 deletions.
diff --git a/...vior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/...vior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp
@@ -56,6 +56,21 @@ void updatePathProperty(
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const PullOutPath & pull_out_path, 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_
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp
@@ -16,6 +16,8 @@
 
 #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>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -170,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;

diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
@@ -84,4 +84,19 @@ std::pair<double, double> getPairsTerminalVelocityAndAccel(
   return pull_out_path.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
diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/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"
 
@@ -297,6 +298,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);