fix(start_planner): remove inverse orientation points (#5100)

* fix(start_planner): remove inverse orientation points

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

* use loop

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

---------

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

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1850,7 +1850,8 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
 
 /**
  * @brief Remove points with invalid orientation differences from a given points container
- * (trajectory, path, ...)
+ * (trajectory, path, ...). Check the difference between the angles of two points and the difference
+ * between the azimuth angle between the two points and the angle of the next point.
  * @param points Points of trajectory, path, or other point container (input / output)
  * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in
  * radians (default: M_PI_2)
@@ -1859,10 +1860,14 @@ template <class T>
 void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
   for (size_t i = 1; i < points.size();) {
-    const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation);
-    const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation);
-
-    if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) {
+    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+    const double yaw1 = tf2::getYaw(p1.orientation);
+    const double yaw2 = tf2::getYaw(p2.orientation);
+
+    if (
+      max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
+      !tier4_autoware_utils::isDrivingForward(p1, p2)) {
       points.erase(points.begin() + i);
     } else {
       ++i;

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

@@ -98,21 +98,6 @@ 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);
-
 std::optional<PathWithLaneId> generateFeasibleStopPath(
   PathWithLaneId & current_path, std::shared_ptr<const PlannerData> planner_data,
   geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration,

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

@@ -172,9 +172,6 @@ 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/path_shifter/path_shifter.cpp

@@ -56,6 +56,7 @@ namespace behavior_path_planner
 using motion_utils::findNearestIndex;
 using motion_utils::insertOrientation;
 using motion_utils::removeInvalidOrientationPoints;
+using motion_utils::removeOverlapPoints;
 
 void PathShifter::setPath(const PathWithLaneId & path)
 {
@@ -147,9 +148,18 @@ bool PathShifter::generate(
   type == SHIFT_TYPE::SPLINE ? applySplineShifter(shifted_path, offset_back)
                              : applyLinearShifter(shifted_path);
 
-  const bool is_driving_forward = true;
-  insertOrientation(shifted_path->path.points, is_driving_forward);
+  shifted_path->path.points = removeOverlapPoints(shifted_path->path.points);
+  // Use orientation before shift to remove points in reverse order
+  // before setting wrong azimuth orientation
   removeInvalidOrientationPoints(shifted_path->path.points);
+  size_t previous_size{shifted_path->path.points.size()};
+  do {
+    previous_size = shifted_path->path.points.size();
+    // Set the azimuth orientation to the next point at each point
+    insertOrientation(shifted_path->path.points, true);
+    // Use azimuth orientation to remove points in reverse order
+    removeInvalidOrientationPoints(shifted_path->path.points);
+  } while (previous_size != shifted_path->path.points.size());
 
   // DEBUG
   RCLCPP_DEBUG_STREAM_THROTTLE(

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

@@ -167,21 +167,6 @@ 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;
-}
-
 std::optional<PathWithLaneId> generateFeasibleStopPath(
   PathWithLaneId & current_path, std::shared_ptr<const PlannerData> planner_data,
   geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration,

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

@@ -303,9 +303,6 @@ 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);