Skip to content

Commit b07ad8b

Browse files
zulfaqar-azmi-t4TomohitoAndo
authored andcommitted
fix(lane_change): consider max velocity during path planning (autowarefoundation#6615)
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 6f02335 commit b07ad8b

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
@@ -1171,9 +1171,9 @@ bool NormalLaneChange::getLaneChangePaths(
11711171
};
11721172

11731173
// get path on original lanes
1174-
const auto prepare_velocity = std::max(
1174+
const auto prepare_velocity = std::clamp(
11751175
current_velocity + sampled_longitudinal_acc * prepare_duration,
1176-
minimum_lane_changing_velocity);
1176+
minimum_lane_changing_velocity, getCommonParam().max_vel);
11771177

11781178
// compute actual longitudinal acceleration
11791179
const double longitudinal_acc_on_prepare =
@@ -1237,8 +1237,9 @@ bool NormalLaneChange::getLaneChangePaths(
12371237
const auto lane_changing_length =
12381238
initial_lane_changing_velocity * lane_changing_time +
12391239
0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
1240-
const auto terminal_lane_changing_velocity =
1241-
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
1240+
const auto terminal_lane_changing_velocity = std::min(
1241+
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
1242+
getCommonParam().max_vel);
12421243
utils::lane_change::setPrepareVelocity(
12431244
prepare_segment, current_velocity, terminal_lane_changing_velocity);
12441245

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
242242
p.min_acc = declare_parameter<double>("normal.min_acc");
243243
p.max_acc = declare_parameter<double>("normal.max_acc");
244244

245+
p.max_vel = declare_parameter<double>("max_vel");
245246
p.backward_length_buffer_for_end_of_pull_over =
246247
declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
247248
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
@@ -45,6 +45,7 @@ struct BehaviorPathPlannerParameters
4545
// common parameters
4646
double min_acc;
4747
double max_acc;
48+
double max_vel;
4849

4950
double minimum_pull_over_length;
5051
double minimum_pull_out_length;

0 commit comments

Comments
 (0)