fix(behavior_path_planner): disable avoidance if ego is not on prefer…

…red lane (#2781) * fix(behavior_path_planner): disable avoidance in preferred lane Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): use odometry Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> --------- Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Jan 31, 2023 · cd546b5 · cd546b5
1 parent 202f283
commit cd546b5
Showing 1 changed file with 12 additions and 0 deletions.
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
@@ -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, &current_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) {