feat(goal_planner): prevent auto approval for unsafe path (autowarefo…

…undation#5621)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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(