From f4c9941e7e3f247a300d0772ae7ee5d939fdb79c Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 31 Jan 2023 11:58:08 +0900 Subject: [PATCH] fix(behavior_path_planner): fix pull out module when enable_back is false (#2779) Signed-off-by: tomoya.kimura --- .../src/scene_module/pull_out/pull_out_module.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 9e96a502561c2..ac8241d691675 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -389,13 +389,13 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } // plan with each planner for (const auto & planner : pull_out_planners_) { - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); planner->setPlannerData(planner_data_); @@ -412,6 +412,9 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); @@ -439,11 +442,11 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); // plan with each planner @@ -462,6 +465,9 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose);