diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3c8ffe389aca..93c20fd25b1b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -93,6 +93,18 @@ bool AvoidanceModule::isExecutionRequested() const return true; } + // Check ego is in preferred lane + const auto current_lanes = util::getCurrentLanes(planner_data_); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet( + current_lanes, planner_data_->self_odometry->pose.pose, ¤t_lane); + const auto num = planner_data_->route_handler->getNumLaneToPreferredLane(current_lane); + + if (num != 0) { + return false; + } + + // Check avoidance targets exist const auto avoid_data = calcAvoidancePlanningData(debug_data_); if (parameters_->publish_debug_marker) {