-
Notifications
You must be signed in to change notification settings - Fork 640
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feat(obstacle avoidance planner): fix out curve, make calculation cost low, make optimization stable, refactor, etc. #233
Changes from 6 commits
4f9afbc
23656a9
1a232e6
1dc853e
3b089b9
bf5b765
257d06d
6102d94
cc42e65
c2617d6
eab3635
6071830
7f8a8b4
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Large diffs are not rendered by default.
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,99 +1,164 @@ | ||
/**: | ||
ros__parameters: | ||
# trajectory total/fixing length | ||
trajectory_length: 200 # total trajectory length[m] | ||
forward_fixing_distance: 20.0 # forward fixing length from base_link[m] | ||
backward_fixing_distance: 5.0 # backward fixing length from base_link[m] | ||
|
||
# clearance(distance) when generating trajectory | ||
clearance_from_road: 0.2 # clearance from road boundary[m] | ||
clearance_from_object: 1.0 # clearance from object[m] | ||
min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range | ||
extra_desired_clearance_from_road: 0.0 # extra desired clearance from road | ||
|
||
# clearance for unique points | ||
clearance_for_straight_line: 0.05 # minimum optimizing range around straight points | ||
clearance_for_joint: 0.1 # minimum optimizing range around joint points | ||
clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing | ||
range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending | ||
clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line | ||
|
||
# avoiding param | ||
max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s] | ||
max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s] | ||
center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. center line width around path points used for judging that object is required to avoid or not | ||
acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range | ||
avoiding_object_type: # avoiding object class | ||
unknown: true | ||
car: true | ||
truck: true | ||
bus: true | ||
bicycle: true | ||
motorbike: true | ||
pedestrian: true | ||
animal: true | ||
|
||
# solving quadratic programming | ||
qp_max_iteration: 10000 # max iteration when solving QP | ||
qp_eps_abs: 1.0e-8 # eps abs when solving OSQP | ||
qp_eps_rel: 1.0e-11 # eps rel when solving OSQP | ||
qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending | ||
qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending | ||
qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing | ||
qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing | ||
|
||
# constrain space | ||
is_getting_constraints_close2path_points: true # generate trajectory closer to path points | ||
max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate | ||
coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction | ||
coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction | ||
keep_space_shape_x: 0.2 # keep space for x direction from base_link[m] | ||
keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. keep space for y direction from base_link[m] | ||
max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m] | ||
|
||
is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid | ||
is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid | ||
enable_avoidance: true # enable avoidance function | ||
is_using_vehicle_config: true # use vehicle config | ||
num_sampling_points: 100 # number of optimizing points | ||
num_joint_buffer_points: 3 # number of joint buffer points | ||
num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending | ||
num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx | ||
num_fix_points_for_extending: 50 # number of fixing points when extending | ||
delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m] | ||
delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m] | ||
delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m] | ||
delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0. delta yaw thres for closest point | ||
delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point | ||
option: | ||
# publish | ||
is_publishing_debug_visualization_marker: true | ||
is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid | ||
is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid | ||
is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid | ||
|
||
is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area | ||
|
||
# show | ||
is_showing_debug_info: false | ||
is_showing_calculation_time: false | ||
|
||
# check if planned trajectory is outside drivable area | ||
drivability_check: | ||
# true: vehicle shape is considered as a set of circles | ||
# false: vehicle shape is considered as footprint (= rectangle) | ||
use_vehicle_circles: false | ||
|
||
# parameters only when use_vehicle_circles is true | ||
vehicle_circles: | ||
use_manual_vehicle_circles: false | ||
num_for_constraints: 4 | ||
|
||
# parameters only when use_manual_vehicle_circles is true | ||
longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] | ||
radius: 0.95 | ||
|
||
# parameters only when use_manual_vehicle_circles is false | ||
num_for_radius: 4 | ||
radius_ratio: 0.9 | ||
|
||
# other | ||
enable_avoidance: false # enable avoidance function | ||
enable_pre_smoothing: true # enable EB | ||
skip_optimization: false # skip MPT and EB | ||
reset_prev_optimization: false | ||
|
||
common: | ||
# sampling | ||
num_sampling_points: 100 # number of optimizing points | ||
|
||
# trajectory total/fixing length | ||
trajectory_length: 300.0 # total trajectory length[m] | ||
|
||
forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] | ||
forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] | ||
|
||
backward_fixing_distance: 5.0 # backward fixing length from base_link [m] | ||
delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] | ||
|
||
delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] | ||
delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point | ||
delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point | ||
|
||
num_fix_points_for_extending: 50 # number of fixing points when extending | ||
max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] | ||
|
||
object: # avoiding object | ||
max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] | ||
max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] | ||
|
||
avoiding_object_type: | ||
unknown: true | ||
car: true | ||
truck: true | ||
bus: true | ||
bicycle: true | ||
motorbike: true | ||
pedestrian: true | ||
animal: true | ||
|
||
eb: | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. EB-related parameters can be moved to an advanced parameter zone as well. |
||
common: | ||
num_joint_buffer_points: 3 # number of joint buffer points | ||
num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx | ||
delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. | ||
num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points | ||
|
||
clearance: | ||
clearance_for_straight_line: 0.05 # minimum optimizing range around straight points | ||
clearance_for_joint: 0.1 # minimum optimizing range around joint points | ||
clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing | ||
|
||
qp: | ||
max_iteration: 10000 # max iteration when solving QP | ||
eps_abs: 1.0e-8 # eps abs when solving OSQP | ||
eps_rel: 1.0e-10 # eps rel when solving OSQP | ||
|
||
# mpt param | ||
# vehicle param for mpt | ||
max_steer_deg: 40.0 # max steering angle [deg] | ||
steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] | ||
|
||
# trajectory param for mpt | ||
num_curvature_sampling_points: 5 # number of sampling points when calculating curvature | ||
delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m] | ||
forward_fixing_mpt_distance: 10 # forward fixing distance for MPT | ||
|
||
# optimization param for mpt | ||
is_hard_fixing_terminal_point: false # hard fixing terminal point | ||
base_point_weight: 2000.0 # slack weight for lateral error around base_link | ||
top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point | ||
mid_point_weight: 1000.0 # slack weight for lateral error around the middle point | ||
lat_error_weight: 10.0 # weight for lateral error | ||
yaw_error_weight: 0.0 # weight for yaw error | ||
steer_input_weight: 0.01 # weight for steering input | ||
steer_rate_weight: 1.0 # weight for steering rate | ||
steer_acc_weight: 0.000001 # weight for steering acceleration | ||
terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point | ||
terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point | ||
terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point | ||
terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point | ||
zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero | ||
mpt: | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Move to some parameters to the advanced patameters. Some of them can stay here, for example, max_steer_deg. |
||
option: | ||
steer_limit_constraint: true | ||
fix_points_around_ego: true | ||
# plan_from_ego: false | ||
visualize_sampling_num: 1 | ||
enable_manual_warm_start: true | ||
enable_warm_start: true # false | ||
|
||
common: | ||
num_curvature_sampling_points: 5 # number of sampling points when calculating curvature | ||
|
||
delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] | ||
|
||
kinematics: | ||
max_steer_deg: 40.0 # max steering angle [deg] | ||
|
||
# By commenting this, we can run vehicle-centered optimization for MPT | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. what do you mean? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. typo: commenting this out. |
||
optimization_center_offset: 2.3 # optimization center offset from base link | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This is a vehicle-related parameter. Take an automatic calculation logic from the vehicle_length for this parameter, and use it by default. The logic could be
|
||
|
||
# check if planned trajectory is outside drivable area | ||
collision_free_constraints: | ||
option: | ||
l_inf_norm: true | ||
soft_constraint: true | ||
hard_constraint: false | ||
# two_step_soft_constraint: false | ||
|
||
vehicle_circles: | ||
use_manual_vehicle_circles: false | ||
num_for_constraints: 3 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This value can be kept as this it. |
||
|
||
# parameters only when use_manual_vehicle_circles is true | ||
longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95] | ||
radius: 0.95 | ||
|
||
# parameters only when use_manual_vehicle_circles is false | ||
num_for_radius: 4 | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The |
||
radius_ratio: 0.8 | ||
|
||
clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. some clearance parameters can stay here. |
||
hard_clearance_from_road: 0.0 # clearance from road boundary[m] | ||
soft_clearance_from_road: 0.1 # clearance from road boundary[m] | ||
soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] | ||
clearance_from_object: 1.0 # clearance from object[m] | ||
extra_desired_clearance_from_road: 0.0 # extra desired clearance from road | ||
|
||
weight: | ||
soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point | ||
soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point | ||
|
||
lat_error_weight: 100.0 # weight for lateral error | ||
yaw_error_weight: 0.0 # weight for yaw error | ||
yaw_error_rate_weight: 0.0 # weight for yaw error rate | ||
steer_input_weight: 10.0 # weight for steering input | ||
steer_rate_weight: 10.0 # weight for steering rate | ||
|
||
obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error | ||
obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error | ||
obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error | ||
near_objects_length: 30.0 # weight for yaw error | ||
|
||
terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point | ||
terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point | ||
terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point | ||
terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point | ||
|
||
# replanning & trimming trajectory param outside algorithm | ||
min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m] | ||
min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second] | ||
max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m] | ||
distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change | ||
replan: | ||
max_path_shape_change_dist: 2.0 # threshold of path shape change from behavior path [m] | ||
max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] | ||
max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Move the following
vehicle_circles
related parameters to the below of this file as an advanced parameters.Write like "default is false. Please set the advanced parameters under
drivability_check.vehicle_circle
namespace when you set this to true.