fix(behavior_path_planner): overwrite goal lane id when pull over fun…

…ction is on (#451) Signed-off-by: kyoichi sugahara <[email protected]>
tier4 · Mar 3, 2022 · 6668c0e · 6668c0e
1 parent 53e4163
commit 6668c0e
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -669,8 +669,8 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   const PathWithLaneId & path) const
 {
   const auto goal = planner_data_->route_handler->getGoalPose();
-  const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
   const auto is_approved = planner_data_->approval.is_approved.data;
+  auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
 
   Pose refined_goal{};
   {
@@ -681,6 +681,7 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
       is_approved && planner_data_->route_handler->getPullOverTarget(
                        planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) {
       refined_goal = planner_data_->route_handler->getPullOverGoalPose();
+      goal_lane_id = pull_over_lane.id();
     } else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
       refined_goal = util::refineGoal(goal, goal_lanelet);
     } else {