From 4044f17a21e8975eff721c17bdf65bc43391964c Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 4 Mar 2025 11:41:03 +0900 Subject: [PATCH 1/2] fix(lane_departure_checker): fix trajectory resampling logic to keep given interval Signed-off-by: kyoichi-sugahara --- .../src/lane_departure_checker_node/utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp index 2df7a69de90bc..d6f3d5e26d4e3 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp @@ -120,9 +120,8 @@ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double if (boost::geometry::distance(prev_point.to_2d(), next_point.to_2d()) >= interval) { resampled.push_back(traj_point); + prev_point = next_point; } - - prev_point = next_point; } resampled.push_back(trajectory.points.back()); From 9562288a4a8f274baf2749e2fc871a54a8534792 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 4 Mar 2025 12:07:36 +0900 Subject: [PATCH 2/2] test(lane_departure_checker): add test case for consecutive small distances followed by large distance Signed-off-by: kyoichi-sugahara --- .../test/test_resample_trajectory.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp index 214a165660225..10550051a1df6 100644 --- a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp +++ b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp @@ -92,5 +92,10 @@ INSTANTIATE_TEST_SUITE_P( "IntervalIsGreaterThanDistance", {{1.0, 0.0, 0.0}, {2.0, 0.0, 0.0}, {3.0, 0.0, 0.0}}, 1.5, - {{1.0, 0.0, 0.0}, {3.0, 0.0, 0.0}}}), + {{1.0, 0.0, 0.0}, {3.0, 0.0, 0.0}}}, + ResampleTrajectoryTestParam{ + "ConsecutiveSmallDistancesFollowedByLargeDistance", + {{0.0, 0.0, 0.0}, {0.7, 0.0, 0.0}, {1.3, 0.0, 0.0}, {3.0, 0.0, 0.0}}, + 1.0, + {{0.0, 0.0, 0.0}, {1.3, 0.0, 0.0}, {3.0, 0.0, 0.0}}}), ::testing::PrintToStringParamName());