From b1679e60933e1012d25b7e9bb4c1b7d16d482850 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 2 May 2024 16:15:04 +0900 Subject: [PATCH 1/6] add motion_velocity_planner parameter file Signed-off-by: Maxime CLEMENT --- .../motion_velocity_planner.param.yaml | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml new file mode 100644 index 0000000000..14ce26a15c --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + stop_line_extend_length: 5.0 + system_delay: 0.5 + delay_response_time: 0.5 From 5367be5de90a9dc14c4e7bc1fb230ef5f6d3c6d9 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 2 May 2024 17:12:36 +0900 Subject: [PATCH 2/6] update planning component launch with motion_velocity_planner params Signed-off-by: Maxime CLEMENT --- .../launch/components/tier4_planning_component.launch.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index f093ce8133..7688cdbc64 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -69,6 +69,10 @@ + + + + From 8ba205602e713b07b467b2bb2a622750ee507a15 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 7 May 2024 06:45:57 +0900 Subject: [PATCH 3/6] update presets to launch out_of_lane in the motion instead of behavior Signed-off-by: Maxime CLEMENT --- autoware_launch/config/planning/preset/default_preset.yaml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index 674b3e07e7..fb50fb0e08 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -76,7 +76,7 @@ launch: default: "false" - arg: name: launch_out_of_lane_module - default: "true" + default: "false" - arg: name: launch_no_drivable_lane_module default: "false" @@ -98,6 +98,11 @@ launch: # path_sampler # none + # motion velocity planner modules + - arg: + name: launch_motion_out_of_lane_module + default: "true" + - arg: name: motion_stop_planner_type default: obstacle_cruise_planner From b320d916c51c975ab0db0698c0326e400cbc1fcb Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 28 May 2024 16:48:11 +0900 Subject: [PATCH 4/6] add parameter to enable velocity smoothing Signed-off-by: Maxime CLEMENT --- .../motion_velocity_planner.param.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml index 14ce26a15c..5b2fea537d 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml @@ -1,5 +1,3 @@ /**: ros__parameters: - stop_line_extend_length: 5.0 - system_delay: 0.5 - delay_response_time: 0.5 + smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning From 85a0932b5b8c0fa900c472fa6a7c9c9cf1431ba2 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 23 May 2024 13:21:23 +0900 Subject: [PATCH 5/6] add virtual wall and debug markers to rviz config Signed-off-by: Maxime CLEMENT --- autoware_launch/rviz/autoware.rviz | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 9005a0fdba..e0c4315f42 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2128,6 +2128,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/motion_velocity_smoother/virtual_wall Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OutOfLane) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls + Value: true Enabled: true Name: VirtualWall - Class: rviz_common/Group @@ -2251,6 +2263,22 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: OutOfLane + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers + Value: true + Enabled: false + Name: MotionVelocityPlanner Enabled: false Name: DebugMarker Enabled: true From fbf6cf159a19a5a9c7aac40dbdd1a9cd44bfdd30 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 27 May 2024 11:41:01 +0900 Subject: [PATCH 6/6] fix parameter files and paths Signed-off-by: Maxime CLEMENT --- .../out_of_lane.param.yaml | 44 +++++++++++++++++++ .../tier4_planning_component.launch.xml | 3 +- 2 files changed, 45 insertions(+), 2 deletions(-) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane.param.yaml diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane.param.yaml new file mode 100644 index 0000000000..b13df72409 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane.param.yaml @@ -0,0 +1,44 @@ +/**: + 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: false # do not run this module when ego already overlaps another lane + ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored + + 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. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights + + 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 + precision: 0.1 # [m] precision when inserting a stop pose in the path + distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled + 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: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of 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 7688cdbc64..9f59a2191e 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -70,8 +70,7 @@ - - +