Skip to content

Commit c6a8911

Browse files
use a better function to get the previous lanelets
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 7b40ad5 commit c6a8911

File tree

1 file changed

+3
-1
lines changed

1 file changed

+3
-1
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -374,7 +374,9 @@ bool StartPlannerModule::isPreventingRearVehicleFromCrossing() const
374374
if (!is_closest_lanelet) return std::nullopt;
375375
lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData());
376376
// Check backwards just in case the Vehicle behind ego is in a different lanelet
377-
const auto prev_lanes = route_handler->getPreviousLanelets(closest_lanelet);
377+
constexpr double backwards_length = 200.0;
378+
const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets(
379+
*route_handler, target_lanes, current_pose, backwards_length);
378380
// return all the relevant lanelets
379381
lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const};
380382
relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end());

0 commit comments

Comments
 (0)