Skip to content

Commit 152b11e

Browse files
fix(lane_change): consider max velocity during path planning (#6615)
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent c653236 commit 152b11e

File tree

3 files changed

+7
-4
lines changed

3 files changed

+7
-4
lines changed

planning/behavior_path_lane_change_module/src/scene.cpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -1247,9 +1247,9 @@ bool NormalLaneChange::getLaneChangePaths(
12471247
};
12481248

12491249
// get path on original lanes
1250-
const auto prepare_velocity = std::max(
1250+
const auto prepare_velocity = std::clamp(
12511251
current_velocity + sampled_longitudinal_acc * prepare_duration,
1252-
minimum_lane_changing_velocity);
1252+
minimum_lane_changing_velocity, getCommonParam().max_vel);
12531253

12541254
// compute actual longitudinal acceleration
12551255
const double longitudinal_acc_on_prepare =
@@ -1313,8 +1313,9 @@ bool NormalLaneChange::getLaneChangePaths(
13131313
const auto lane_changing_length =
13141314
initial_lane_changing_velocity * lane_changing_time +
13151315
0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
1316-
const auto terminal_lane_changing_velocity =
1317-
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
1316+
const auto terminal_lane_changing_velocity = std::min(
1317+
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
1318+
getCommonParam().max_vel);
13181319
utils::lane_change::setPrepareVelocity(
13191320
prepare_segment, current_velocity, terminal_lane_changing_velocity);
13201321

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -238,6 +238,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
238238
p.min_acc = declare_parameter<double>("normal.min_acc");
239239
p.max_acc = declare_parameter<double>("normal.max_acc");
240240

241+
p.max_vel = declare_parameter<double>("max_vel");
241242
p.backward_length_buffer_for_end_of_pull_over =
242243
declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
243244
p.backward_length_buffer_for_end_of_pull_out =

planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ struct BehaviorPathPlannerParameters
4242
// common parameters
4343
double min_acc;
4444
double max_acc;
45+
double max_vel;
4546

4647
double minimum_pull_over_length;
4748
double minimum_pull_out_length;

0 commit comments

Comments
 (0)