fix(lane_change): consider max velocity during path planning (autowarefoundation#1206)

* fix(lane_change): consider max velocity during path planning (autowarefoundation#6615) (autowarefoundation#1203)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix the max_velocity not subscribed from motion velocity smoother

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -214,6 +214,8 @@
       <param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
       <param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
       <param from="$(var behavior_path_planner_common_param_path)"/>
+
+      <param from="$(var motion_velocity_smoother_param_path)"/>
       <!-- composable node config -->
       <extra_arg name="use_intra_process_comms" value="false"/>
     </composable_node>

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1175,9 +1175,9 @@ bool NormalLaneChange::getLaneChangePaths(
       };
 
       // 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 =
@@ -1241,8 +1241,9 @@ bool NormalLaneChange::getLaneChangePaths(
         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/launch/behavior_path_planner.launch.xml

@@ -21,6 +21,8 @@
   <arg name="behavior_path_planner_start_planner_module_param_path"/>
   <arg name="behavior_path_planner_drivable_area_expansion_param_path"/>
 
+  <arg name="motion_velocity_smoother_param_path"/>
+
   <node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" output="screen">
     <!-- topic remap -->
     <remap from="~/input/route" to="/planning/scenario_planning/scenario"/>
@@ -53,5 +55,7 @@
     <param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
     <param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
     <param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
+
+    <param from="$(var motion_velocity_smoother_param_path)"/>
   </node>
 </launch>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -242,6 +242,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_velocity");
   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

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