fix(behavior_path_planner): fix lane change speed

planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp

@@ -54,10 +54,9 @@ bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
 
-std::pair<double, double> calcLaneChangingSpeedAndDistance(
-  const double velocity, const double shift_length, const double deceleration,
-  const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param,
-  const LaneChangeParameters & lc_param);
+double calcLaneChangingDistance(
+  const double lane_changing_velocity, const double shift_length, const double min_total_lc_len,
+  const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param);
 
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
@@ -113,8 +112,8 @@ ShiftLine getLaneChangingShiftLine(
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double target_lane_length,
-  const double lane_changing_distance, const double min_total_lane_changing_distance,
-  const double forward_path_length, const double resample_interval, const bool is_goal_in_route);
+  const double lane_changing_distance, const double forward_path_length,
+  const double resample_interval, const bool is_goal_in_route);
 
 PathWithLaneId getPrepareSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
@@ -128,8 +127,8 @@ PathWithLaneId getPrepareSegment(
 
 PathWithLaneId getLaneChangingSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
-  const double forward_path_length, const double arc_length_from_target,
-  const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end,
+  const double forward_path_length, const Pose & lane_changing_start_pose,
+  const double target_lane_length, const double lane_changing_distance,
   const double lane_changing_speed, const double total_required_min_dist);
 
 bool isEgoWithinOriginalLane(

planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp

@@ -134,8 +134,7 @@ class PathShifter
   static double calcLongitudinalDistFromJerk(
     const double lateral, const double jerk, const double velocity);
 
-  static double calcShiftTimeFromJerkAndJerk(
-    const double lateral, const double jerk, const double acc);
+  static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc);
 
   static double calcJerkFromLatLonDistance(
     const double lateral, const double longitudinal, const double velocity);

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -22,6 +22,7 @@
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <algorithm>
@@ -640,12 +641,11 @@ bool LaneChangeModule::isAbortState() const
 bool LaneChangeModule::hasFinishedLaneChange() const
 {
   const auto & current_pose = getEgoPose();
-  const auto arclength_current =
-    lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose);
-  const double travel_distance = arclength_current.length - status_.start_distance;
-  const double finish_distance =
-    status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer;
-  return travel_distance > finish_distance;
+  const auto & lane_change_path = status_.lane_change_path.path;
+  const auto & lane_change_end = status_.lane_change_path.shift_line.end;
+  const double dist_to_lane_change_end = motion_utils::calcSignedArcLength(
+    lane_change_path.points, current_pose.position, lane_change_end.position);
+  return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0;
 }
 
 void LaneChangeModule::setObjectDebugVisualization() const

planning/behavior_path_planner/src/util/lane_change/util.cpp

@@ -24,6 +24,7 @@
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -329,7 +330,7 @@ std::pair<bool, bool> getLaneChangePaths(
 
   const auto sorted_lane_ids = getSortedLaneIds(
     route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance);
-  constexpr auto ignore_unknown{true};
+  constexpr auto ignore_unknown{false};
   const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5);
 
   LaneChangeTargetObjectIndices dynamic_object_indices;
@@ -363,35 +364,52 @@ std::pair<bool, bool> getLaneChangePaths(
       std::max(prepare_speed, minimum_lane_change_velocity));
 #endif
 
-    const auto estimated_shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(
-      target_lanelets, prepare_segment.points.front().point.pose);
+    if (prepare_segment.points.empty()) {
+      RCLCPP_ERROR_STREAM(
+        rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"),
+        "prepare segment is empty!! something wrong...");
+      continue;
+    }
 
-    const auto [lane_changing_speed, lane_changing_distance] = calcLaneChangingSpeedAndDistance(
-      prepare_speed, estimated_shift_length, acceleration, end_of_lane_dist, common_parameter,
-      parameter);
+    // lane changing start pose is at the end of prepare segment
+    const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
 
-    const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance};
+    const auto estimated_shift_length =
+      lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose);
+
+    // we assume constant speed during lane change
+    const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity);
+    const auto lane_changing_distance = calcLaneChangingDistance(
+      lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter);
+
+    if (is_goal_in_route) {
+      const double s_start =
+        lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length;
+      const double s_goal =
+        lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length;
+      if (s_start + lane_changing_distance + parameter.lane_change_finish_judge_buffer > s_goal) {
+        continue;
+      }
+    }
 
     const auto lane_changing_segment = getLaneChangingSegment(
-      route_handler, target_lanelets, forward_path_length, arc_position_from_target.length,
-      target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance);
+      route_handler, target_lanelets, forward_path_length, lane_changing_start_pose,
+      target_lane_length, lane_changing_distance, lane_changing_speed, required_total_min_distance);
 
-    if (prepare_segment.points.empty() || lane_changing_segment.points.empty()) {
+    if (lane_changing_segment.points.empty()) {
       RCLCPP_ERROR_STREAM(
         rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"),
-        "reference path is empty!! something wrong...");
+        "lane changing segment is empty!! something wrong...");
       continue;
     }
 
-    const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
-
     const auto resample_interval =
       calcLaneChangeResampleInterval(lane_changing_distance, lane_changing_speed);
 
+    const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance};
     const auto target_lane_reference_path = getReferencePathFromTargetLane(
       route_handler, target_lanelets, lane_changing_start_pose, target_lane_length,
-      lc_dist.lane_changing, required_total_min_distance, forward_path_length, resample_interval,
-      is_goal_in_route);
+      lc_dist.lane_changing, forward_path_length, resample_interval, is_goal_in_route);
 
     if (target_lane_reference_path.points.empty()) {
       continue;
@@ -482,9 +500,7 @@ bool hasEnoughDistance(
   const double lane_change_required_distance =
     static_cast<double>(num) * minimum_lane_change_length;
 
-  if (
-    lane_change_total_distance + lane_change_required_distance >
-    util::getDistanceToEndOfLane(current_pose, current_lanes)) {
+  if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) {
     return false;
   }
 
@@ -710,17 +726,14 @@ PathWithLaneId getPrepareSegment(
   return prepare_segment;
 }
 
-std::pair<double, double> calcLaneChangingSpeedAndDistance(
-  const double velocity, const double shift_length, const double deceleration,
-  const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param,
-  const LaneChangeParameters & lc_param)
+double calcLaneChangingDistance(
+  const double lane_changing_speed, const double shift_length, const double min_total_lc_len,
+  const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param)
 {
-  const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk(
+  const auto required_time = PathShifter::calcShiftTimeFromJerk(
     shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc);
 
-  const auto lane_changing_average_speed =
-    std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity);
-  const auto expected_dist = lane_changing_average_speed * required_time;
+  const auto expected_dist = lane_changing_speed * required_time;
   const auto lane_changing_distance =
     (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length;
 
@@ -730,22 +743,25 @@ std::pair<double, double> calcLaneChangingSpeedAndDistance(
       .get_child("util")
       .get_child("calcLaneChangingSpeedAndDistance"),
     "required_time: %f [s] average_speed: %f [m/s], lane_changing_distance : %f [m]", required_time,
-    lane_changing_average_speed, lane_changing_distance);
+    lane_changing_speed, lane_changing_distance);
 
-  return {lane_changing_average_speed, lane_changing_distance};
+  return lane_changing_distance;
 }
 
 PathWithLaneId getLaneChangingSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
-  const double forward_path_length, const double arc_length_from_target,
-  const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end,
+  const double forward_path_length, const Pose & lane_changing_start_pose,
+  const double target_lane_length, const double lane_changing_distance,
   const double lane_changing_speed, const double total_required_min_dist)
 {
-  const double s_start = std::invoke([&arc_length_from_target, &dist_prepare_to_lc_end,
-                                      &target_lane_length, &total_required_min_dist]() {
-    const double dist_from_current_pose = arc_length_from_target + dist_prepare_to_lc_end.sum();
+  const double s_start = std::invoke([&lane_changing_start_pose, &target_lanelets,
+                                      &lane_changing_distance, &target_lane_length,
+                                      &total_required_min_dist]() {
+    const auto arc_to_start_pose =
+      lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose);
+    const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_distance;
     const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist;
-    return std::min(dist_from_current_pose, end_of_lane_dist_without_buffer);
+    return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer);
   });
 
   const double s_end =
@@ -776,8 +792,8 @@ PathWithLaneId getLaneChangingSegment(
 PathWithLaneId getReferencePathFromTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & lane_changing_start_pose, const double target_lane_length,
-  const double lane_changing_distance, const double min_total_lane_changing_distance,
-  const double forward_path_length, const double resample_interval, const bool is_goal_in_route)
+  const double lane_changing_distance, const double forward_path_length,
+  const double resample_interval, const bool is_goal_in_route)
 {
   const ArcCoordinates lane_change_start_arc_position =
     lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose);
@@ -786,13 +802,11 @@ PathWithLaneId getReferencePathFromTargetLane(
   const double s_end = std::invoke([&]() {
     const auto dist_from_lc_start = s_start + lane_changing_distance + forward_path_length;
     if (is_goal_in_route) {
-      const auto goal_arc_coordinates =
-        lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
-      const auto dist_to_goal = goal_arc_coordinates.length - min_total_lane_changing_distance;
-      return std::min(dist_from_lc_start, dist_to_goal);
+      const double s_goal =
+        lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
+      return std::min(dist_from_lc_start, s_goal);
     }
-    const auto dist_from_end = target_lane_length - min_total_lane_changing_distance;
-    return std::min(dist_from_lc_start, dist_from_end);
+    return std::min(dist_from_lc_start, target_lane_length);
   });
 
   RCLCPP_DEBUG(

planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp

@@ -473,8 +473,7 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) const
   }
 }
 
-double PathShifter::calcShiftTimeFromJerkAndJerk(
-  const double lateral, const double jerk, const double acc)
+double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc)
 {
   const double j = std::abs(jerk);
   const double a = std::abs(acc);