From 61a75e8a7e6adba41a1679fec95578d881f67fe7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 15 Mar 2024 11:29:07 +0900 Subject: [PATCH] fix(lane_change): consider max velocity during path planning (#6615) Signed-off-by: Muhammad Zulfaqar Azmi --- planning/behavior_path_lane_change_module/src/scene.cpp | 9 +++++---- .../src/behavior_path_planner_node.cpp | 1 + .../include/behavior_path_planner_common/parameters.hpp | 1 + 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f0972d66416c8..ce44ee27b6638 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1171,9 +1171,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 = @@ -1237,8 +1237,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 index 85abf774d159e..298111e1c27b2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -242,6 +242,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); + p.max_vel = declare_parameter("max_vel"); p.backward_length_buffer_for_end_of_pull_over = declare_parameter("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 index 5bdd0a2f3f88d..6decd8fd2a131 100644 --- 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 @@ -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;