fix(goal_planner): set correct reference path (autowarefoundation#4809)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Nov 27, 2023 · 944d5e0 · 944d5e0
1 parent 7f51bbc
commit 944d5e0
Show file tree

Hide file tree

Showing 2 changed files with 4 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
@@ -556,6 +556,9 @@ void GoalPlannerModule::generateGoalCandidates()
 
 BehaviorModuleOutput GoalPlannerModule::plan()
 {
+  resetPathCandidate();
+  resetPathReference();
+
   generateGoalCandidates();
 
   path_reference_ = getPreviousModuleOutput().reference_path;

diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp
@@ -35,7 +35,7 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan(
   const PathWithLaneId smoothed_path =
     modifyPathForSmoothGoalConnection(*(output.path), planner_data);
   output.path = std::make_shared<PathWithLaneId>(smoothed_path);
-  output.reference_path = std::make_shared<PathWithLaneId>(smoothed_path);
+  output.reference_path = getPreviousModuleOutput().reference_path;
   return output;
 }