From 95c13d55ff333e601951af8c995a68cb01a93820 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 18 Feb 2021 02:00:23 +0900 Subject: [PATCH] Ros2 v0.8.0 planning launch (#59) * [planning_launch] restore file name for ros2 porting Signed-off-by: Takamasa Horibe * Add obstacle_stop_planner.yaml (#82) Signed-off-by: Kenji Miyake * add surround obstacle checker options (#86) * fix slow down param related to https://github.com/tier4/autoware.iv/commit/a9cdcb2e9c4e7afbe50bfc15a86f6c525b8294bd (#91) * fix parame max_steer_deg (#92) * add refine_goal_search_radius_range (#93) * Change default evaluation in motion velocity optimizer (#97) * Use Linf * Add new line * Add maximum_deceleration parameter (#98) * Add maximum_deceleration parameter * Change default value * Unable abort lane change (#102) * add param stoping velocity and fix typo (#106) * Add a parameter for minimum velocity for lane change (#109) * Add parameters for collision check for lane change (#110) * Add a parameter for disable collision check at prepare phase * Add parameters for collision check with predicted_path * Add a parameter for backward buffer for end of lane (#114) * Add parameters for backward buffer for end of lane * Remove comment out * add extend_distance param (#107) * add parameter of acc (#129) Signed-off-by: Yuma Nihei * fix typo & change static object velocity thres in lane_change_planner (#104) * change static object velocity thres Signed-off-by: Kosuke Murakami * fix typo Signed-off-by: Kosuke Murakami * Change minimum_lane_change_velocity (#131) * Feature/update avoidance param (#140) * update avoidance param Signed-off-by: Kosuke Murakami * disable unnecesarry marker Signed-off-by: Kosuke Murakami * modify min_behavior_stop_margin (#127) * modify min_behavior_stop_margin Signed-off-by: Yamato Ando * Update obstacle_stop_planner.yaml Co-authored-by: Yukihiro Saito * Add expand_stop_range to obstacle_stop_planner.yaml (#152) Signed-off-by: Kenji Miyake * Update obstacle_stop_planner.yaml (#153) * Visualize echo back goal_pose instead of 2D Nav Goal (#150) * Visualize echo back goal_pose instead of 2D Nav Goal Signed-off-by: Kenji Miyake * Fix mission_planning.launch Signed-off-by: Kenji Miyake * Add surround_obstacle_checker.yaml (#157) * Add parameter (#158) * Revert "[planning_launch] restore file name for ros2 porting" This reverts commit 275f0df232323bf24627adea9cb08888c250625e. * fix namespace Signed-off-by: Takamasa Horibe * fix relay Signed-off-by: Takamasa Horibe * [planning_launch]: Add vehicle_param_file for turn signal decider Signed-off-by: wep21 * [planning_launch]: Change topic type of lane change approval Signed-off-by: wep21 Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Taichi Higashide Co-authored-by: Satoshi Tanaka Co-authored-by: Fumiya Watanabe Co-authored-by: Yuma Nihei Co-authored-by: Kosuke Murakami Co-authored-by: YamatoAndo Co-authored-by: Yukihiro Saito Co-authored-by: hiroaki-ishikawa-t4 <57431939+hiroaki-ishikawa-t4@users.noreply.github.com> Co-authored-by: wep21 --- .../config/adaptive_cruise_control.yaml | 37 +++++++++++++++++++ .../config/obstacle_stop_planner.yaml | 15 ++++++++ .../motion_velocity_optimizer.yaml | 12 ++++-- .../obstacle_avoidance_planner.yaml | 8 ++-- .../surround_obstacle_checker.yaml | 12 ++++++ .../mission_planning.launch.xml | 19 +++------- .../behavior_planning.launch.xml | 16 ++++++-- .../motion_planning.launch.xml | 16 +++++++- 8 files changed, 108 insertions(+), 27 deletions(-) create mode 100644 planning_launch/config/adaptive_cruise_control.yaml create mode 100644 planning_launch/config/obstacle_stop_planner.yaml create mode 100644 planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml diff --git a/planning_launch/config/adaptive_cruise_control.yaml b/planning_launch/config/adaptive_cruise_control.yaml new file mode 100644 index 0000000000000..8734776b344b4 --- /dev/null +++ b/planning_launch/config/adaptive_cruise_control.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + adaptive_cruise_control: + # Adaptive Cruise Control (ACC) config + use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not + use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not + consider_obj_velocity: true # consider forward vehicle velocity to ACC or not + + # general parameter for ACC + obstacle_stop_velocity_thresh: 1.0 # threshold of forward obstacle velocity to insert stop line (to stop acc) [m/s] + emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] + min_dist_stop: 4.0 # minimum distance of emergency stop [m] + max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] + min_dist_standard: 4.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + + # pid parameter for ACC + p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] + d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + + # parameter for object velocity estimation + object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] + valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] + valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] + valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] + thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) + rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/planning_launch/config/obstacle_stop_planner.yaml b/planning_launch/config/obstacle_stop_planner.yaml new file mode 100644 index 0000000000000..91beb1e1ca1b1 --- /dev/null +++ b/planning_launch/config/obstacle_stop_planner.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + stop_planner: + stop_margin: 5.0 # stop margin distance from obstacle on the path [m] + min_behavior_stop_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] + step_length: 1.0 # step length for pointcloud search range [m] + extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance + expand_stop_range: 0.0 # margin of vehicle footprint [m] + + slow_down_planner: + slow_down_margin: 5.0 # margin distance from slow down point [m] + expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] + max_slow_down_vel: 1.38 # max slow down velocity [m/s] + min_slow_down_vel: 0.28 # min slow down velocity [m/s] + max_deceleration: 2.0 # max slow down deceleration [m/s2] diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml index c94f2dbffe0d4..5acef77dfb5ac 100644 --- a/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml +++ b/planning_launch/config/scenario_planning/common/motion_velocity_optimizer/motion_velocity_optimizer.yaml @@ -6,7 +6,7 @@ # curve parameters max_lateral_accel: 0.5 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/s] 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 @@ -17,6 +17,10 @@ engage_exit_ratio: 0.5 # exit engage sequence to normal velocity plannning 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. + 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 trajecotory pose [radian] @@ -31,8 +35,8 @@ # L2: jerk = 100.0, v_weight = 100000.0, a_weight = 1000.0 # Linf: jerk = 200.0, v_weight = 100000.0, a_weight = 5000.0 - pseudo_jerk_weight: 100.0 # weight for "smoothness" cost + pseudo_jerk_weight: 200.0 # weight for "smoothness" cost over_v_weight: 100000.0 # weight for "overspeed limit" cost - over_a_weight: 1000.0 # weight for "overaccel limit" cost + over_a_weight: 5000.0 # weight for "overaccel limit" cost - algorithm_type: "L2" # Option : L2, Linf + algorithm_type: "Linf" # Option : L2, Linf diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml index 7b37f0aedd2c6..111b83ce524d2 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.yaml @@ -13,7 +13,7 @@ # clearance for unique points clearance_for_straight_line_: 0.05 # minimum optimizing range around straight points - clearance_for_joint_: 3.2 # minimum optimizing range around joint points + clearance_for_joint_: 0.1 # minimum optimizing range around joint points clearance_for_only_smoothing: 0.1 # minimum optimizing range when applygin 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 @@ -59,16 +59,16 @@ # mpt param # vehicle param for mpt - max_steer_deg: 30.0 # max steering angle [deg] + max_steer_deg: 40.0 # max steering angle [deg] steer_tau: 0.1 # steering dynamics time constant (1d approzimation) [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] - num_fix_points_for_mpt: 8 # number of fixing points around ego vehicle + forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle # optimization param for mpt - is_hard_fixing_terminal_point: true # hard fixing terminal point + 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 diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml new file mode 100644 index 0000000000000..8354ea60beca8 --- /dev/null +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + + # obstacle check + use_pointcloud: true # use pointcloud as obstacle check + use_dynamic_object: true # use dynamic object as obstacle check + surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + state_clear_time: 2.0 + + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] diff --git a/planning_launch/launch/mission_planning/mission_planning.launch.xml b/planning_launch/launch/mission_planning/mission_planning.launch.xml index 9bbc44e98a6b1..712840021ba96 100644 --- a/planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,16 +1,7 @@ - - - - - - - - - - - - - - + + + + + diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 60fa2e2afae07..5292611be1a16 100644 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -23,17 +23,26 @@ + + + - - + + + + + + + - + @@ -41,6 +50,7 @@ + diff --git a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index da8f22823634e..88adb9d467155 100644 --- a/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -1,6 +1,7 @@ + @@ -14,23 +15,34 @@ - + + + + + + + + + + - + + +