diff --git a/autoware_launch/config/map/pointcloud_map_loader.param.yaml b/autoware_launch/config/map/pointcloud_map_loader.param.yaml index 61b02c490c..ba4c032d3e 100644 --- a/autoware_launch/config/map/pointcloud_map_loader.param.yaml +++ b/autoware_launch/config/map/pointcloud_map_loader.param.yaml @@ -4,6 +4,7 @@ enable_downsampled_whole_load: false enable_partial_load: true enable_differential_load: true + enable_selected_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index ca1eb740be..83a548bb46 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: pull_over: - request_length: 100.0 + minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -9,7 +9,8 @@ pull_over_minimum_velocity: 1.38 margin_from_boundary: 0.5 decide_path_distance: 10.0 - maximum_deceleration: 0.5 + maximum_deceleration: 1.0 + maximum_jerk: 1.0 # goal research enable_goal_research: true search_priority: "efficient_path" # "efficient_path" or "close_goal" 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 dfc2b2e6dd..f5621257ae 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 @@ -3,8 +3,10 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: false - disable_stop_planning: false # true + enable_debug_info: false + enable_calculation_time_info: false + + enable_slow_down_planning: false # longitudinal info idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] @@ -13,15 +15,11 @@ safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] - lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] - obstacle_velocity_threshold_from_cruise_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_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - cruise_obstacle_type: + stop_obstacle_type: unknown: true car: true truck: true @@ -29,9 +27,30 @@ trailer: true motorcycle: true bicycle: true - pedestrian: false + pedestrian: true - stop_obstacle_type: + cruise_obstacle_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 + + slow_down_obstacle_type: unknown: true car: true truck: true @@ -41,98 +60,111 @@ bicycle: true pedestrian: true - obstacle_filtering: - rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] - detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion + behavior_determination: decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - # if crossing vehicle is decided as target obstacles or not - crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop - collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] - - outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] - outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.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 + goal_extension_length: 5.0 # extension length for collision check around the goal + goal_extension_interval: 1.0 # extension interval for collision check around the goal prediction_resampling_time_interval: 0.1 prediction_resampling_time_horizon: 10.0 - goal_extension_length: 5.0 # extension length for collision check around the goal - goal_extension_interval: 1.0 # extension interval for collision check around the goal - stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle - ignored_outside_obstacle_type: - unknown: true - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: true - pedestrian: true - - 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] + # hysteresis for cruise and stop + obstacle_velocity_threshold_from_cruise_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_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - # 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 + # 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 + + stop: + max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + crossing_obstacle: + collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + cruise: + max_lat_margin: 0.5 # lateral margin between obstacle and trajectory band with ego's width + outside_obstacle: + obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 1.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 + + slow_down: + max_lat_margin: 10.0 # lateral margin between obstacle and trajectory band with ego's width + + cruise: + 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 + + slow_down: + # parameters to calculate slow down velocity + max_lateral_margin: 5.0 + min_lateral_margin: 3.0 + max_ego_velocity: 10.0 + min_ego_velocity: 3.0 + + # parameter for where to insert slow down point + max_deceleration: -2.0