fix(behavior_path_planner): fix goal planner backward parking (#3601)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · May 2, 2023 · 8b2da4b · 8b2da4b
1 parent 66a01ff
commit 8b2da4b
Showing 1 changed file with 5 additions and 1 deletion.
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
@@ -345,7 +345,11 @@ bool GoalPlannerModule::isExecutionRequested() const
     route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
   const double request_length =
     allow_goal_modification ? calcModuleRequestLength() : parameters_->minimum_request_length;
-  if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) {
+  const double backward_goal_search_length =
+    allow_goal_modification ? parameters_->backward_goal_search_length : 0.0;
+  if (
+    self_to_goal_arc_length < -backward_goal_search_length ||
+    self_to_goal_arc_length > request_length) {
     return false;
   }