fix(start_planner): set stop path drivable area (autowarefoundation#4297

) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1222-takeshi · Jul 17, 2023 · 361fbaf · 361fbaf
1 parent 76c61de
commit 361fbaf
Showing 1 changed file with 6 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -298,27 +298,26 @@ PathWithLaneId StartPlannerModule::getFullPath() const
 BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
 {
   updatePullOutStatus();
-  waitApproval();
 
   if (IsGoalBehindOfEgoInSameRouteSegment()) {
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
+    path_reference_ = getPreviousModuleOutput().reference_path;
+    clearWaitingApproval();
     return generateStopOutput();
   }
 
   BehaviorModuleOutput output;
   if (!status_.is_safe) {
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path");
-    // the path of getCurrent() is generated by generateStopPath()
-    const PathWithLaneId stop_path = getCurrentPath();
-    output.path = std::make_shared<PathWithLaneId>(stop_path);
-    output.reference_path = getPreviousModuleOutput().reference_path;
-    path_candidate_ = std::make_shared<PathWithLaneId>(stop_path);
     path_reference_ = getPreviousModuleOutput().reference_path;
-    return output;
+    clearWaitingApproval();
+    return generateStopOutput();
   }
 
+  waitApproval();
+
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto current_lanes =