fix(lane_change): consider max velocity during path planning (autowar…

…efoundation#6615) Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
philtell · Mar 15, 2024 · 152b11e · 152b11e
1 parent c653236
commit 152b11e
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -1247,9 +1247,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 =
@@ -1313,8 +1313,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);
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/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 =

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/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;