diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 17df98e7e0e6b..811e1652fcb4a 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -376,7 +376,7 @@ PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( const auto vehicle_footprints = createVehicleFootprints(path); size_t idx = 0; std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { - if (boost::geometry::within(footprint, fused_lanelets_polygon.value()) || idx > end_index) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { temp_path.points.push_back(path.points.at(idx)); } ++idx;