From 003ee0c9da3f341aefecb16fc216af8471b5238a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 22 May 2024 14:13:59 +0900 Subject: [PATCH] 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 --- .../motion_utils/trajectory/trajectory.hpp | 13 ++++++------ .../test/src/trajectory/test_trajectory.cpp | 21 +++++-------------- .../src/utils/path_shifter/path_shifter.cpp | 6 +++--- 3 files changed, 15 insertions(+), 25 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index d5413cdb8ac24..7e2d92c9434fb 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -1921,20 +1921,21 @@ insertOrientation * radians (default: M_PI_2) */ template -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++; } } } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 3fe6e6851b45e..8e84b111b0688 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/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) diff --git a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 38eafb2f38d7d..1ea555f4675ef 100644 --- a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/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