feat(goal_planner): prevent auto approval for unsafe path (#5621)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Nov 20, 2023 · 20e4492 · 20e4492
1 parent a395e72
commit 20e4492
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -862,6 +862,12 @@ bool GoalPlannerModule::hasDecidedPath() const
     return false;
   }
 
+  // If it is dangerous before approval, do not determine the path.
+  // This eliminates a unsafe path to be approved
+  if (!isSafePath() && !isActivated()) {
+    return false;
+  }
+
   // if ego is sufficiently close to the start of the nearest candidate path, the path is decided
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex(