From dd02979b4386730fa5c742d8db2bccb879d19797 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 28 Jul 2021 18:30:40 +0900 Subject: [PATCH] Feature/porting motion velocity smoother (#302) * Launch motion_velocity_smoother (#215) * Launch motion_velocity_smoother * Change params * Fix parameter files * Fix Signed-off-by: wep21 * Fix/smoother trajectory ds (#249) * Add parameter * Fix * Fix * change launcher parameter (#265) * Feature/smoother resampling (#269) * change launcher parameter * add new parameter Co-authored-by: Fumiya Watanabe Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> --- .../JerkFiltered.param.yaml | 7 +++ .../motion_velocity_smoother/L2.param.yaml | 5 ++ .../motion_velocity_smoother/Linf.param.yaml | 5 ++ .../motion_velocity_smoother.param.yaml | 54 +++++++++++++++++++ .../lane_change_planner.param.yaml | 2 +- .../obstacle_avoidance_planner.param.yaml | 2 +- .../scenario_planning.launch.xml | 11 ++-- 7 files changed, 80 insertions(+), 6 deletions(-) create mode 100644 planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml create mode 100644 planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml create mode 100644 planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml create mode 100644 planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml new file mode 100644 index 0000000000000..a6906b7117fd5 --- /dev/null +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + jerk_weight: 0.1 # weight for "smoothness" cost for jerk + over_v_weight: 10000.0 # weight for "over speed limit" cost + over_a_weight: 500.0 # weight for "over accel limit" cost + over_j_weight: 200.0 # weight for "over jerk limit" cost + jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml new file mode 100644 index 0000000000000..c993978204f10 --- /dev/null +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml new file mode 100644 index 0000000000000..ec38010621cc9 --- /dev/null +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost + over_v_weight: 100000.0 # weight for "over speed limit" cost + over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml new file mode 100644 index 0000000000000..4231eb1ef80bf --- /dev/null +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -0,0 +1,54 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] + max_accel: 1.0 # max acceleration limit [m/ss] + min_decel: -0.5 # min deceleration limit [m/ss] + max_jerk: 1.0 # default maximum jerk limit + min_jerk: -0.5 # default minimum jerk limit + + # external velocity limit parameter + margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] + + # curve parameters + max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit + decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit + + # engage & replan parameters + replan_vel_deviation: 5.53 # 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] + + # stop velocity + stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] + stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. + + # path extraction parameters + extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] + extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] + delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] + + # resampling parameters for optimization + max_trajectory_length: 200.0 # max trajectory length for resampling [m] + min_trajectory_length: 150.0 # min trajectory length for resampling [m] + resample_time: 5.0 # resample total time for dense sampling [s] + dense_dt: 0.1 # resample time interval for dense sampling [s] + dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + sparse_dt: 0.5 # resample time interval for sparse sampling [s] + sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] + + # resampling parameters for post process + post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] + post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] + post_resample_time: 10.0 # resample total time for dense sampling [s] + post_dense_dt: 0.1 # resample time interval for dense sampling [s] + post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] + post_sparse_dt: 0.1 # resample time interval for sparse sampling [s] + post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + + # system + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml index 3e899ca586e28..0bf42387a6621 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml @@ -4,7 +4,7 @@ stop_time: 2.0 hysteresis_buffer_distance: 2.0 backward_path_length: 5.0 - forward_path_length: 200.0 + forward_path_length: 300.0 max_accel: -5.0 lane_change_prepare_duration: 4.0 lane_changing_duration: 8.0 diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 52620f1d3c38e..8150af7436212 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: # trajectory total/fixing length - trajectory_length: 200 # total trajectory length[m] + trajectory_length: 300 # total trajectory length[m] forward_fixing_distance: 5.0 # forward fixing length from base_link[m] backward_fixing_distance: 3.0 # backward fixing length from base_link[m] diff --git a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 0d80c87faf3e0..639cc9b6b1738 100644 --- a/planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -14,10 +14,13 @@ - - - - + + + + + + +