Skip to content

Commit

Permalink
refactor(obstacle_cruise_planner): clean up a part of the code (#287)
Browse files Browse the repository at this point in the history 
* modify parameters

* use cruise planner temporarily

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* use obstacle_stop_planner by default

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Apr 11, 2023
1 parent a462edf commit b1ed636
Showing 1 changed file with 127 additions and 95 deletions.
222 changes: 127 additions & 95 deletions ...g/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -13,25 +15,42 @@
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
bus: true
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
Expand All @@ -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

0 comments on commit b1ed636

Please sign in to comment.