fix(lane_change): consider max velocity during path planning

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1247,74 +1247,75 @@
       };
 
       // get path on original lanes
-      const auto prepare_velocity = std::max(
+      const auto prepare_velocity = std::clamp(
         current_velocity + sampled_longitudinal_acc * prepare_duration,
-        minimum_lane_changing_velocity);
+        minimum_lane_changing_velocity, getCommonParam().max_vel);
 
       // compute actual longitudinal acceleration
       const double longitudinal_acc_on_prepare =
         (prepare_duration < 1e-3) ? 0.0
                                   : ((prepare_velocity - current_velocity) / prepare_duration);
 
       const double prepare_length =
         current_velocity * prepare_duration +
         0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
 
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
       if (prepare_segment.points.empty()) {
         debug_print("prepare segment is empty...? Unexpected.");
         continue;
       }
 
       // lane changing start getEgoPose() is at the end of prepare segment
       const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
       const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
         current_lanes, target_lanes.front(), lane_changing_start_pose);
 
       // Check if the lane changing start point is not on the lanes next to target lanes,
       if (target_length_from_lane_change_start_pose > 0.0) {
         debug_print("lane change start getEgoPose() is behind target lanelet!");
         break;
       }
 
       const auto shift_length =
         lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
 
       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] =
         lane_change_parameters_->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;
 
       std::vector<double> sample_lat_acc;
       constexpr double eps = 0.01;
       for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) {
         sample_lat_acc.push_back(a);
       }
       RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
 
       for (const auto & lateral_acc : sample_lat_acc) {
         const auto debug_print = [&](const auto & s) {
           RCLCPP_DEBUG_STREAM(
             logger_, "    -  " << s << " : prep_time = " << prepare_duration << ", lon_acc = "
                                << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc);
         };
 
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc);
         const double longitudinal_acc_on_lane_changing =
           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 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
-        const auto terminal_lane_changing_velocity =
-          initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
+        const auto terminal_lane_changing_velocity = std::min(
+          initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
+          getCommonParam().max_vel);
         utils::lane_change::setPrepareVelocity(
           prepare_segment, current_velocity, terminal_lane_changing_velocity);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -238,6 +238,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.min_acc = declare_parameter<double>("normal.min_acc");
   p.max_acc = declare_parameter<double>("normal.max_acc");
 
+  p.max_vel = declare_parameter<double>("max_vel");
   p.backward_length_buffer_for_end_of_pull_over =
     declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
   p.backward_length_buffer_for_end_of_pull_out =

planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp

@@ -42,6 +42,7 @@ struct BehaviorPathPlannerParameters
   // common parameters
   double min_acc;
   double max_acc;
+  double max_vel;
 
   double minimum_pull_over_length;
   double minimum_pull_out_length;