fix(lane_change): calculate distance to goal

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: rej55

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1513,9 +1513,16 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
   // lane changing start getEgoPose() is at the end of prepare segment
   const auto current_lane_terminal_point =
     lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back());
-  auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose(
+
+  double distance_to_terminal_from_goal = 0;
+  if (is_goal_in_route) {
+    distance_to_terminal_from_goal =
+      utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes);
+  }
+
+  const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose(
     prev_module_path_.points, current_lane_terminal_point,
-    -(lane_change_buffer + next_lane_change_buffer));
+    -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
 
   if (!lane_changing_start_pose) {
     RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!");

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -412,17 +412,17 @@ PathWithLaneId getReferencePathFromTargetLane(
     return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer);
   });
 
-  if (s_end - s_start < lane_changing_length) {
-    return PathWithLaneId();
-  }
-
   RCLCPP_DEBUG(
     rclcpp::get_logger("behavior_path_planner")
       .get_child("lane_change")
       .get_child("util")
       .get_child("getReferencePathFromTargetLane"),
     "start: %f, end: %f", s_start, s_end);
 
+  if (s_end - s_start < lane_changing_length) {
+    return PathWithLaneId();
+  }
+
   const auto lane_changing_reference_path =
     route_handler.getCenterLinePath(target_lanes, s_start, s_end);