From 783fbe540d633f1c6d4fae62cf888ce9404fe651 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 10 Oct 2024 21:00:29 +0900 Subject: [PATCH] feat(obstacle_cruise)!: improve obstacle_cruise (#625) * feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out (#1142) Signed-off-by: Berkay Karaman * revert(obstacle_cruise): reduce using predicted paths to maintain conventional behavior (#621) Signed-off-by: Yuki Takagi * revert(obstacle_cruisse): revert "fix(obstacle_cruise_planner): guarantee the stop margin (#1076)" (#1185) Signed-off-by: Yuki Takagi --------- Signed-off-by: Berkay Karaman Signed-off-by: Yuki Takagi Co-authored-by: Berkay Karaman --- .../obstacle_cruise_planner.param.yaml | 49 +++++++++++++------ 1 file changed, 34 insertions(+), 15 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 5f27fef6fb..23adc6829c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -26,18 +26,29 @@ enable_approaching: false additional_safe_distance_margin: 3.0 # [m] min_safe_distance_margin: 3.0 # [m] - suppress_sudden_obstacle_stop: false + suppress_sudden_obstacle_stop: true stop_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: true pointcloud: false + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true cruise_obstacle_type: inside: @@ -93,21 +104,29 @@ # if crossing vehicle is determined as target obstacles or not crossing_obstacle: - obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 0.523599 # [rad] = 30 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: max_lat_margin: 0.3 # lateral margin between the obstacles 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] + 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 : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: - obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] - max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + 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