From 0a0a4ab1d0e5f9808f0a45a0039100074a98e3b7 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 30 Jan 2025 09:06:57 +0900 Subject: [PATCH] feat(rviz): update autoware.rviz for motion_velocity_obstacle__module (#1314) * feat: add motion_velocity_obstacle_stop/slow_down/cruise_module Signed-off-by: Takayuki Murooka * update autoware.rviz Signed-off-by: Takayuki Murooka * update rviz Signed-off-by: Takayuki Murooka * disable obstacle_cruise_planner Signed-off-by: Takayuki Murooka * update motion velocity planner params Signed-off-by: Takayuki Murooka * add module.param.yaml Signed-off-by: Takayuki Murooka * enable obstacle_cruise_planner Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../planning/preset/default_preset.yaml | 9 ++ .../motion_velocity_planner.param.yaml | 11 ++ .../obstacle_cruise.param.yaml | 120 ++++++++++++++++++ .../obstacle_slow_down.param.yaml | 50 ++++++++ .../obstacle_stop.param.yaml | 74 +++++++++++ .../tier4_planning_component.launch.xml | 3 + autoware_launch/rviz/autoware.rviz | 72 +++++++++++ 7 files changed, 339 insertions(+) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index 5a3139e4c5..78965615ab 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -93,6 +93,15 @@ launch: # none # motion velocity planner modules + - arg: + name: launch_obstacle_stop_module + default: "false" + - arg: + name: launch_obstacle_slow_down_module + default: "false" + - arg: + name: launch_obstacle_cruise_module + default: "false" - arg: name: launch_dynamic_obstacle_stop_module default: "true" 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 5b2fea537d..4d5409cef2 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,3 +1,14 @@ /**: ros__parameters: smooth_velocity_before_planning: true # [-] if true, smooth the velocity profile of the input trajectory before planning + + trajectory_polygon_collision_check: + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + goal_extended_trajectory_length: 6.0 + + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: true + time_to_convergence: 1.5 #[s] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml new file mode 100644 index 0000000000..214f7bb35a --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml @@ -0,0 +1,120 @@ +/**: + ros__parameters: + obstacle_cruise: + option: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + cruise_planning: + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] + min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] + min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] + safe_distance_margin : 5.0 # This is also used as a stop margin [m] + + pid_based_planner: + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic + + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 + + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false + + disable_target_acceleration: true + + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 + + lpf_normalized_error_cruise_dist_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) + engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 + + obstacle_filtering: + object_type: + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: false + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + + # if crossing vehicle is determined as target obstacles or not + crossing_obstacle: + obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + + outside_obstacle: + obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 0.2 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + yield: + enable_yield: true + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 + + # hysteresis for cruise and stop + obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml new file mode 100644 index 0000000000..f826e4ccd6 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + obstacle_slow_down: + slow_down_planning: + slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] + slow_down_min_jerk: -1.0 # slow down min jerk [m/sss] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + + time_margin_on_target_velocity: 1.5 # [s] + + # parameters to calculate slow down velocity by linear interpolation + object_type_specified_params: + types: + - "default" + default: + moving: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 4.0 + max_ego_velocity: 8.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold + + obstacle_filtering: + object_type: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + pointcloud: false + + min_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop + max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 + + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml new file mode 100644 index 0000000000..21dbf5f5bc --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + obstacle_stop: + option: + ignore_crossing_obstacle: true + suppress_sudden_stop: true + + stop_planning: + stop_margin : 5.0 # longitudinal margin to obstacle [m] + terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m] + min_behavior_stop_margin: 3.0 # [m] + + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + + stop_on_curve: + enable_approaching: false + additional_stop_margin: 3.0 # [m] + min_stop_margin: 3.0 # [m] + + object_type_specified_params: + types: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as "sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as "sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. + + obstacle_filtering: + object_type: + pointcloud: false + + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + # hysteresis for velocity + obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + outside_obstacle: + max_lateral_time_margin: 4.5 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] + + crossing_obstacle: + collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index a01a5fca17..5754d17ab5 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -74,6 +74,9 @@ + + + diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 3f029ff75d..5a2d8c9140 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2114,6 +2114,42 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/velocity_smoother/virtual_wall Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + 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/obstacle_stop/virtual_walls + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleSlowDown) + 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/obstacle_slow_down/virtual_walls + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleCruise) + 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/obstacle_cruise/virtual_walls + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: VirtualWall (OutOfLane) @@ -2251,6 +2287,42 @@ Visualization Manager: Value: false - Class: rviz_common/Group Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleStop + 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/obstacle_stop/debug_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleSlowDown + 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/obstacle_slow_down/debug_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleCruise + 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/obstacle_cruise/debug_markers + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: OutOfLane