feat(lane_change): enable lane change to accelerate in the lane changing segment

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -15,6 +15,10 @@
       longitudinal_acceleration_sampling_num: 5
       lateral_acceleration_sampling_num: 3
 
+      # longitudinal acceleration
+      min_longitudinal_acc: -1.0
+      max_longitudinal_acc: 1.0
+
       # lateral acceleration map
       lateral_acceleration:
         velocity: [0.0, 4.0, 10.0]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -182,8 +182,7 @@ class LaneChangeBase
 
   virtual PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-    const double backward_path_length, const double prepare_length, const double current_velocity,
-    const double prepare_velocity) const = 0;
+    const double backward_path_length, const double prepare_length) const = 0;
 
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -91,8 +91,7 @@ class NormalLaneChange : public LaneChangeBase
 
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-    const double backward_path_length, const double prepare_length, const double current_velocity,
-    const double prepare_velocity) const override;
+    const double backward_path_length, const double prepare_length) const override;
 
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
@@ -128,8 +127,7 @@ class NormalLaneChangeBT : public NormalLaneChange
 
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-    const double backward_path_length, const double prepare_length, const double current_velocity,
-    const double prepare_velocity) const override;
+    const double backward_path_length, const double prepare_length) const override;
 
   std::vector<DrivableLanes> getDrivableLanes() const override;
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -35,6 +35,10 @@ struct LaneChangeParameters
   int longitudinal_acc_sampling_num{10};
   int lateral_acc_sampling_num{10};
 
+  // acceleration data
+  double min_longitudinal_acc{-1.0};
+  double max_longitudinal_acc{1.0};
+
   // collision check
   bool enable_prepare_segment_collision_check{true};
   double prepare_segment_ignore_object_velocity_thresh{0.1};

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -55,7 +55,11 @@ double calcLaneChangeResampleInterval(
 
 double calcMaximumAcceleration(
   const PathWithLaneId & path, const Pose & current_pose, const double current_velocity,
-  const BehaviorPathPlannerParameters & params);
+  const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params);
+
+double calcLaneChangingAcceleration(
+  const double initial_lane_changing_velocity, const double max_path_velocity,
+  const double lane_changing_time, const double prepare_longitudinal_acc);
 
 void setPrepareVelocity(
   PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity);
@@ -77,28 +81,23 @@ bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);
 
-double calcLaneChangingLength(
-  const double lane_changing_velocity, const double shift_length, const double lateral_acc,
-  const double lateral_jerk);
-
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
   const std::vector<std::vector<int64_t>> & sorted_lane_ids, const double longitudinal_acceleration,
   const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length,
-  const LaneChangePhaseInfo lane_change_velocity,
-  const BehaviorPathPlannerParameters & common_parameter,
-  const LaneChangeParameters & lane_change_param);
+  const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity,
+  const LaneChangePhaseInfo lane_change_time);
 
 PathSafetyStatus isLaneChangePathSafe(
   const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects,
   const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose,
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter,
   const behavior_path_planner::LaneChangeParameters & lane_change_parameter,
   const double front_decel, const double rear_decel,
-  std::unordered_map<std::string, CollisionCheckDebug> & debug_data,
-  const double acceleration = 0.0);
+  std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const double prepare_acc = 0.0,
+  const double lane_changing_acc = 0.0);
 
 bool isObjectIndexIncluded(
   const size_t & index, const std::vector<size_t> & dynamic_objects_indices);
@@ -172,5 +171,10 @@ CandidateOutput assignToCandidate(
 boost::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
   const LaneChangeModuleType type, const Direction & direction);
+
+PredictedPath convertToPredictedPath(
+  const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose,
+  const size_t nearest_seg_idx, const double duration, const double resolution,
+  const double prepare_time, const double prepare_acc, const double lane_changing_acc);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -107,11 +107,6 @@ struct FrenetPoint
 };
 
 // data conversions
-PredictedPath convertToPredictedPath(
-  const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose,
-  const size_t nearest_seg_idx, const double duration, const double resolution,
-  const double prepare_time, const double acceleration);
-
 template <class T>
 FrenetPoint convertToFrenetPoint(
   const T & points, const Point & search_point_geom, const size_t seg_idx)

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -749,6 +749,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lateral_acc_sampling_num =
     declare_parameter<int>(parameter("lateral_acceleration_sampling_num"));
 
+  // acceleration
+  p.min_longitudinal_acc = declare_parameter<double>(parameter("min_longitudinal_acc"));
+  p.max_longitudinal_acc = declare_parameter<double>(parameter("max_longitudinal_acc"));
+
   // collision check
   p.enable_prepare_segment_collision_check =
     declare_parameter<bool>(parameter("enable_prepare_segment_collision_check"));

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

@@ -371,7 +371,7 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane)
 PathWithLaneId NormalLaneChange::getPrepareSegment(
   const lanelet::ConstLanelets & current_lanes,
   [[maybe_unused]] const double arc_length_from_current, const double backward_path_length,
-  const double prepare_length, const double current_velocity, const double prepare_velocity) const
+  const double prepare_length) const
 {
   if (current_lanes.empty()) {
     return PathWithLaneId();
@@ -382,8 +382,6 @@ PathWithLaneId NormalLaneChange::getPrepareSegment(
     prepare_segment.points, getEgoPose(), 3.0, 1.0);
   utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length);
 
-  utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity);
-
   return prepare_segment;
 }
 
@@ -406,20 +404,24 @@ bool NormalLaneChange::getLaneChangePaths(
   const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity;
   const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num;
   const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num;
+  const auto min_longitudinal_acc =
+    std::max(common_parameter.min_acc, lane_change_parameters_->min_longitudinal_acc);
+  const auto max_longitudinal_acc =
+    std::min(common_parameter.max_acc, lane_change_parameters_->max_longitudinal_acc);
 
   // get velocity
   const auto current_velocity = getEgoTwist().linear.x;
 
   // compute maximum longitudinal deceleration and acceleration
-  const auto maximum_deceleration =
-    std::invoke([&minimum_lane_changing_velocity, &current_velocity, &common_parameter, this]() {
-      const double min_a = (minimum_lane_changing_velocity - current_velocity) /
-                           common_parameter.lane_change_prepare_duration;
-      return std::clamp(
-        min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits<double>::epsilon());
-    });
+  const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, &current_velocity,
+                                                 &min_longitudinal_acc, &common_parameter, this]() {
+    const double min_a = (minimum_lane_changing_velocity - current_velocity) /
+                         common_parameter.lane_change_prepare_duration;
+    return std::clamp(
+      min_a, -std::abs(min_longitudinal_acc), -std::numeric_limits<double>::epsilon());
+  });
   const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration(
-    prev_module_path_, getEgoPose(), current_velocity, common_parameter);
+    prev_module_path_, getEgoPose(), current_velocity, max_longitudinal_acc, common_parameter);
 
   // get sampling acceleration values
   const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues(
@@ -473,9 +475,8 @@ bool NormalLaneChange::getLaneChangePaths(
       break;
     }
 
-    const auto prepare_segment = getPrepareSegment(
-      original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length,
-      current_velocity, prepare_velocity);
+    auto prepare_segment = getPrepareSegment(
+      original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length);
 
     if (prepare_segment.points.empty()) {
       RCLCPP_DEBUG(
@@ -502,21 +503,30 @@ bool NormalLaneChange::getLaneChangePaths(
     const auto shift_length =
       lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose);
 
-    // we assume constant velocity during lane change
-    const auto lane_changing_velocity = prepare_velocity;
+    const auto initial_lane_changing_velocity = prepare_velocity;
+    const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps;
 
     // get lateral acceleration range
     const auto [min_lateral_acc, max_lateral_acc] =
-      common_parameter.lane_change_lat_acc_map.find(lane_changing_velocity);
+      common_parameter.lane_change_lat_acc_map.find(initial_lane_changing_velocity);
     const auto lateral_acc_resolution =
       std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num;
     constexpr double lateral_acc_epsilon = 0.01;
 
     for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon;
          lateral_acc += lateral_acc_resolution) {
-      const auto lane_changing_length = utils::lane_change::calcLaneChangingLength(
-        lane_changing_velocity, shift_length, lateral_acc,
-        common_parameter.lane_changing_lateral_jerk);
+      const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
+        shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc);
+      const double lane_changing_lon_acc = utils::lane_change::calcLaneChangingAcceleration(
+        initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
+        sampled_longitudinal_acc);
+      const auto lane_changing_length =
+        initial_lane_changing_velocity * lane_changing_time +
+        0.5 * lane_changing_lon_acc * lane_changing_time * lane_changing_time;
+      const auto terminal_lane_changing_velocity =
+        initial_lane_changing_velocity + lane_changing_lon_acc * lane_changing_time;
+      utils::lane_change::setPrepareVelocity(
+        prepare_segment, current_velocity, terminal_lane_changing_velocity);
 
       if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
         RCLCPP_DEBUG(
@@ -543,7 +553,8 @@ bool NormalLaneChange::getLaneChangePaths(
 
       const auto target_segment = utils::lane_change::getTargetSegment(
         route_handler, target_lanelets, forward_path_length, lane_changing_start_pose,
-        target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer);
+        target_lane_length, lane_changing_length, initial_lane_changing_velocity,
+        next_lane_change_buffer);
 
       if (target_segment.points.empty()) {
         RCLCPP_DEBUG(
@@ -553,7 +564,7 @@ bool NormalLaneChange::getLaneChangePaths(
       }
 
       const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(
-        lane_changing_length, lane_changing_velocity);
+        lane_changing_length, initial_lane_changing_velocity);
 
       const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
       const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane(
@@ -571,12 +582,15 @@ bool NormalLaneChange::getLaneChangePaths(
       const auto shift_line = utils::lane_change::getLaneChangingShiftLine(
         prepare_segment, target_segment, target_lane_reference_path, shift_length);
 
-      const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity};
+      const auto lc_velocity =
+        LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
+
+      const auto lc_time = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
 
       const auto candidate_path = utils::lane_change::constructCandidatePath(
         prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets,
-        target_lanelets, sorted_lane_ids, longitudinal_acc, lateral_acc, lc_length, lc_velocity,
-        common_parameter, *lane_change_parameters_);
+        target_lanelets, sorted_lane_ids, lane_changing_lon_acc, lateral_acc, lc_length,
+        lc_velocity, terminal_lane_changing_velocity, lc_time);
 
       if (!candidate_path) {
         RCLCPP_DEBUG(
@@ -612,7 +626,8 @@ bool NormalLaneChange::getLaneChangePaths(
       const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe(
         *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(),
         common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration,
-        common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc);
+        common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc,
+        lane_changing_lon_acc);
 
       if (is_safe) {
         return true;
@@ -960,8 +975,7 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane
 
 PathWithLaneId NormalLaneChangeBT::getPrepareSegment(
   const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-  const double backward_path_length, const double prepare_length, const double current_velocity,
-  const double prepare_velocity) const
+  const double backward_path_length, const double prepare_length) const
 {
   if (current_lanes.empty()) {
     return PathWithLaneId();
@@ -977,8 +991,6 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment(
   PathWithLaneId prepare_segment =
     getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end);
 
-  utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity);
-
   return prepare_segment;
 }