From 20e44929708fc5a4f1c7a775b6d99a5427ec8fe6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 20 Nov 2023 21:54:04 +0900 Subject: [PATCH] feat(goal_planner): prevent auto approval for unsafe path (#5621) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 index 9712faae5712a..f39d706eb7ceb 100644 --- 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(