Skip to content

Commit 06ec361

Browse files
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>
1 parent d6aa2c7 commit 06ec361

File tree

5 files changed

+13
-4
lines changed

5 files changed

+13
-4
lines changed

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

+2
Original file line numberDiff line numberDiff line change
@@ -214,6 +214,8 @@
214214
<param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
215215
<param from="$(var behavior_path_planner_scene_module_manager_param_path)"/>
216216
<param from="$(var behavior_path_planner_common_param_path)"/>
217+
218+
<param from="$(var motion_velocity_smoother_param_path)"/>
217219
<!-- composable node config -->
218220
<extra_arg name="use_intra_process_comms" value="false"/>
219221
</composable_node>

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/launch/behavior_path_planner.launch.xml

+4
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121
<arg name="behavior_path_planner_start_planner_module_param_path"/>
2222
<arg name="behavior_path_planner_drivable_area_expansion_param_path"/>
2323

24+
<arg name="motion_velocity_smoother_param_path"/>
25+
2426
<node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" output="screen">
2527
<!-- topic remap -->
2628
<remap from="~/input/route" to="/planning/scenario_planning/scenario"/>
@@ -53,5 +55,7 @@
5355
<param from="$(var behavior_path_planner_goal_planner_module_param_path)"/>
5456
<param from="$(var behavior_path_planner_start_planner_module_param_path)"/>
5557
<param from="$(var behavior_path_planner_drivable_area_expansion_param_path)"/>
58+
59+
<param from="$(var motion_velocity_smoother_param_path)"/>
5660
</node>
5761
</launch>

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_velocity");
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)