fix(lane_change): fix terminal stop distance

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 24, 2023 · 2c5d290 · 2c5d290
1 parent 0f9eb8a
commit 2c5d290
Showing 1 changed file with 3 additions and 9 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -207,15 +207,9 @@ void NormalLaneChange::insertStopPoint(
     return utils::getSignedDistance(path.points.front().point.pose, target, lanelets);
   };
 
-  // If lanelets.back() is in goal route section, get distance to goal.
-  // Otherwise, get distance to end of lane.
-  double distance_to_terminal = 0.0;
-  if (route_handler->isInGoalRouteSection(lanelets.back())) {
-    const auto goal = route_handler->getGoalPose();
-    distance_to_terminal = getDistanceAlongLanelet(goal);
-  } else {
-    distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets);
-  }
+  // get distance to end of lane.
+  const double distance_to_terminal =
+    utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets);
 
   const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane;
   const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);