fix(motion_utils): remove only first point whose orientation is invalid (autowarefoundation#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

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