From 2a9ef1c02b75082f50c92d8c38d262cc17bf7a87 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 10 Apr 2023 13:00:36 +0900 Subject: [PATCH] feat(behavior_velocity_planner): add out of lane module (#269) * Add initial param file for new out_of_lane module Signed-off-by: Maxime CLEMENT * Add params for extending the ego footprint Signed-off-by: Maxime CLEMENT * Add more parameters Signed-off-by: Maxime CLEMENT * add a few more params Signed-off-by: Maxime CLEMENT * Add/rename parameters for new version with 3 methods (thr, inter, ttc) Signed-off-by: Maxime CLEMENT * Add parameters for "skip_if_*", "strict", and "use_predicted_path" Signed-off-by: Maxime CLEMENT * Update default parameters Signed-off-by: Maxime CLEMENT * style(pre-commit): autofix * Fix typo Signed-off-by: Maxime CLEMENT * Change param ego.extra_front_offset 1.0 -> 0.0 Signed-off-by: Maxime CLEMENT * Update rviz config with "out_of_lane" virtual wall and debug markers Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_velocity_planner.param.yaml | 1 + .../out_of_lane.param.yaml | 39 +++++++++++++++++++ .../tier4_planning_component.launch.xml | 1 + autoware_launch/rviz/autoware.rviz | 24 ++++++++++++ 4 files changed, 65 insertions(+) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index f774d11d2a..6f1341cb02 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -11,6 +11,7 @@ launch_no_stopping_area: true launch_run_out: false launch_speed_bump: false + launch_out_of_lane: true forward_path_length: 1000.0 backward_path_length: 5.0 stop_line_extend_length: 5.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml new file mode 100644 index 0000000000..c501599b4a --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + out_of_lane: # module to stop or slowdown before overlapping another lane with other objects + mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" + skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + + threshold: + time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time + intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego + ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer + objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer + ttc: + threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap + + objects: + minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored + use_predicted_paths: true # if true, use the predicted paths to estimate future positions. + # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + + overlap: + minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered + extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) + + action: # action to insert in the path if an object causes a conflict at an overlap + skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed + strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego + # if false, ego stops just before entering a lane but may then be overlapping another lane. + distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + slowdown: + distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap + velocity: 2.0 # [m/s] slowdown velocity + stop: + distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap + + ego: + extra_front_offset: 0.0 # [m] extra front distance + extra_rear_offset: 0.0 # [m] extra rear distance + extra_right_offset: 0.0 # [m] extra right distance + extra_left_offset: 0.0 # [m] extra left distance diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index a520ef4bc5..a126783e47 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -89,6 +89,7 @@ /> +