fix(lane_departure_checker): fix trajectory resampling logic to keep …

…given interval (#10221) * fix(lane_departure_checker): fix trajectory resampling logic to keep given interval Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * test(lane_departure_checker): add test case for consecutive small distances followed by large distance Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
autowarefoundation · Mar 4, 2025 · 6ef2396 · 6ef2396
1 parent 9d61ff1
commit 6ef2396
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 3 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
@@ -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());
 

diff --git 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());