fix(motion_utils): remove only first point whose orientation is invalid (#7084)

fix(motion_utils): don't continue when it removes invalid orientation point

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

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

@@ -1921,20 +1921,21 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
  * radians (default: M_PI_2)
  */
 template <class T>
-void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
+void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
-  for (size_t i = 1; i < points.size();) {
-    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
-    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+  for (auto itr = points.begin(); std::next(itr) != points.end();) {
+    const auto p1 = tier4_autoware_utils::getPose(*itr);
+    const auto p2 = tier4_autoware_utils::getPose(*std::next(itr));
     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);
+      points.erase(std::next(itr));
+      return;
     } else {
-      ++i;
+      itr++;
     }
   }
 }

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -5337,10 +5337,10 @@ TEST(trajectory, cropPoints)
   }
 }
 
-TEST(Trajectory, removeInvalidOrientationPoints)
+TEST(Trajectory, removeFirstInvalidOrientationPoints)
 {
   using motion_utils::insertOrientation;
-  using motion_utils::removeInvalidOrientationPoints;
+  using motion_utils::removeFirstInvalidOrientationPoints;
 
   const double max_yaw_diff = M_PI_2;
 
@@ -5351,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     auto modified_traj = traj;
     insertOrientation(modified_traj.points, true);
     modifyTrajectory(modified_traj);
-    removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
+    removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
     EXPECT_EQ(modified_traj.points.size(), expected_size);
     for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) {
       EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i));
@@ -5374,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points.back();
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.push_back(invalid_point);
     },
     traj.points.size());
@@ -5385,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points[4];
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.insert(t.points.begin() + 4, invalid_point);
     },
     traj.points.size());
-
-  // invalid point at the beginning
-  testRemoveInvalidOrientationPoints(
-    traj,
-    [&](Trajectory & t) {
-      auto invalid_point = t.points.front();
-      invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
-      t.points.insert(t.points.begin(), invalid_point);
-    },
-    1);  // expected size is 1 since only the first point remains
 }
 
 TEST(trajectory, calcYawDeviation)

planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp

@@ -53,7 +53,7 @@ namespace behavior_path_planner
 
 using motion_utils::findNearestIndex;
 using motion_utils::insertOrientation;
-using motion_utils::removeInvalidOrientationPoints;
+using motion_utils::removeFirstInvalidOrientationPoints;
 using motion_utils::removeOverlapPoints;
 
 void PathShifter::setPath(const PathWithLaneId & path)
@@ -155,14 +155,14 @@ bool PathShifter::generate(
   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);
+  removeFirstInvalidOrientationPoints(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);
+    removeFirstInvalidOrientationPoints(shifted_path->path.points);
   } while (previous_size != shifted_path->path.points.size());
 
   // DEBUG