From 04a956053bf2e2b8c16b0806ca9bc9d850d6bbcc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 23 Dec 2022 22:11:57 +0900 Subject: [PATCH 01/11] feat(planning_launch): new lane change path generation parameters (#629) * feat(planning_launch): new lane change path generation parameters Signed-off-by: Muhammad Zulfaqar Azmi * rearrange parameters Signed-off-by: Muhammad Zulfaqar Azmi * fix parameter for stable result Signed-off-by: Muhammad Zulfaqar Azmi * Preparing for #652 Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner.param.yaml | 8 +++----- .../lane_change/lane_change.param.yaml | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 3423bfa221..e34fa49cba 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -2,10 +2,8 @@ ros__parameters: backward_path_length: 5.0 forward_path_length: 300.0 - backward_length_buffer_for_end_of_lane: 5.0 backward_length_buffer_for_end_of_pull_over: 5.0 backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_lane_change_length: 12.0 minimum_pull_over_length: 16.0 refine_goal_search_radius_range: 7.5 @@ -23,11 +21,11 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 - expected_front_deceleration: -0.5 + expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -2.0 - expected_rear_deceleration_for_abort: -2.5 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 2.0 diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index fc8b314b09..66d66fe780 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -2,16 +2,26 @@ ros__parameters: lane_change: lane_change_prepare_duration: 4.0 # [s] - lane_changing_duration: 8.0 # [s] + lane_changing_safety_check_duration: 8.0 # [s] + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 + backward_length_buffer_for_end_of_lane: 2.0 lane_change_finish_judge_buffer: 3.0 # [m] - minimum_lane_change_velocity: 5.6 # [m/s] + + double lane_changing_lateral_jerk: 0.5 + double lane_changing_lateral_acc: 0.5 + + minimum_lane_change_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 + abort_lane_change_velocity_thresh: 0.5 abort_lane_change_angle_thresh: 10.0 # [deg] abort_lane_change_distance_thresh: 0.3 # [m] + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] + enable_abort_lane_change: true enable_collision_check_at_prepare_phase: true use_predicted_path_outside_lanelet: true From 7ae682487fff95ecf0297cb90a1db3aedc941c8e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sat, 24 Dec 2022 14:11:52 +0900 Subject: [PATCH 02/11] feat(planning_launch): add new param for avoidance safety check (#637) Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index e698819fc9..e6461fa2b1 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -9,6 +9,8 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false + enable_safety_check: false # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] @@ -22,11 +24,20 @@ object_check_shiftable_ratio: 0.6 # [-] object_check_min_road_shoulder_width: 0.5 # [m] + # For safety check + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + # For lateral margin object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] + # For longitudinal margin + longitudinal_collision_margin_buffer: 0.0 # [m] + prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_avoidance_distance: 10.0 # [m] @@ -72,3 +83,9 @@ # ---------- advanced parameters ---------- avoidance_execution_lateral_threshold: 0.499 + + target_velocity_matrix: + col_size: 4 + matrix: + [2.78, 5.56, 11.1, 13.9, # velocity [m/s] + 0.20, 0.90, 1.10, 1.20] # margin [m] From 0302028899094351752960972e8e2ac6fa06818b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 26 Dec 2022 13:05:34 +0900 Subject: [PATCH 03/11] feat(control_launch): organize controller param yaml (#658) * feat(control_launch): organize controller param yaml Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * organize param files Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- autoware_launch/launch/autoware.launch.xml | 9 ++++++--- .../mpc.param.yaml} | 0 .../{ => lateral}/pure_pursuit.param.yaml | 0 .../pid.param.yaml} | 0 control_launch/launch/control.launch.py | 12 ++++++------ control_launch/package.xml | 3 +-- 6 files changed, 13 insertions(+), 11 deletions(-) rename control_launch/config/trajectory_follower/{mpc_follower.param.yaml => lateral/mpc.param.yaml} (100%) rename control_launch/config/trajectory_follower/{ => lateral}/pure_pursuit.param.yaml (100%) rename control_launch/config/trajectory_follower/{longitudinal_controller.param.yaml => longitudinal/pid.param.yaml} (100%) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index a8bd3f4796..e33089a5fd 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -119,9 +119,12 @@ - - - + + + + + + diff --git a/control_launch/config/trajectory_follower/mpc_follower.param.yaml b/control_launch/config/trajectory_follower/lateral/mpc.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/mpc_follower.param.yaml rename to control_launch/config/trajectory_follower/lateral/mpc.param.yaml diff --git a/control_launch/config/trajectory_follower/pure_pursuit.param.yaml b/control_launch/config/trajectory_follower/lateral/pure_pursuit.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/pure_pursuit.param.yaml rename to control_launch/config/trajectory_follower/lateral/pure_pursuit.param.yaml diff --git a/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/longitudinal/pid.param.yaml similarity index 100% rename from control_launch/config/trajectory_follower/longitudinal_controller.param.yaml rename to control_launch/config/trajectory_follower/longitudinal/pid.param.yaml diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index e2c69f208d..1962c8d520 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -61,8 +61,8 @@ def launch_setup(context, *args, **kwargs): shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + package="trajectory_follower_node", + plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", remappings=[ @@ -260,8 +260,8 @@ def add_launch_arg(name: str, default_value=None, description=None): # lateral controller mode add_launch_arg( "lateral_controller_mode", - "mpc_follower", - "lateral controller mode: `mpc_follower` or `pure_pursuit`", + "mpc", + "lateral controller mode: `mpc` or `pure_pursuit`", ) # longitudinal controller mode @@ -284,7 +284,7 @@ def add_launch_arg(name: str, default_value=None, description=None): "lat_controller_param_path", [ FindPackageShare("control_launch"), - "/config/trajectory_follower/mpc_follower.param.yaml", + "/config/mpc_lateral_controller/mpc_lateral_controller.param.yaml", ], "path to the parameter file of lateral controller. default is `mpc_follower`", ) @@ -292,7 +292,7 @@ def add_launch_arg(name: str, default_value=None, description=None): "lon_controller_param_path", [ FindPackageShare("control_launch"), - "/config/trajectory_follower/longitudinal_controller.param.yaml", + "/config/pid_longitudinal_controller/pid_longitudinal_controller.param.yaml", ], "path to the parameter file of longitudinal controller", ) diff --git a/control_launch/package.xml b/control_launch/package.xml index ed1180a06e..ebf76a49f8 100644 --- a/control_launch/package.xml +++ b/control_launch/package.xml @@ -14,8 +14,7 @@ external_cmd_converter lane_departure_checker shift_decider - trajectory_follower - trajectory_follower_nodes + trajectory_follower_node vehicle_cmd_gate ament_lint_auto From 094e11d3b10578cdd66d3e44c342a2924b8433b4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 28 Dec 2022 12:11:52 +0900 Subject: [PATCH 04/11] refactor(control_launch): organize trajectory_follower_node's param (#661) * refactor(control_launch): organize trajectory_follower_node's param Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../trajectory_follower_node.param.yaml | 4 +++ control_launch/launch/control.launch.py | 32 +++++++++++++++++-- 2 files changed, 33 insertions(+), 3 deletions(-) create mode 100644 control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml diff --git a/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml new file mode 100644 index 0000000000..dcd985bb4a --- /dev/null +++ b/control_launch/config/trajectory_follower/trajectory_follower_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + ctrl_period: 0.03 + timeout_thr_sec: 0.5 diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index 1962c8d520..0dc4d1e836 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -39,6 +39,11 @@ def launch_setup(context, *args, **kwargs): lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + trajectory_follower_node_param_path = LaunchConfiguration( + "trajectory_follower_node_param_path" + ).perform(context) + with open(trajectory_follower_node_param_path, "r") as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -83,6 +88,7 @@ def launch_setup(context, *args, **kwargs): "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"), }, nearest_search_param, + trajectory_follower_node_param, lon_controller_param, lat_controller_param, ], @@ -208,7 +214,12 @@ def launch_setup(context, *args, **kwargs): launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), ("target_container", "/control/control_container"), - ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + # This is temporary uncomment. I will replace control_launch with tier4_control_launch soon. + # ("initial_selector_mode", LaunchConfiguration("initial_selector_mode")), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), ], ) @@ -284,7 +295,7 @@ def add_launch_arg(name: str, default_value=None, description=None): "lat_controller_param_path", [ FindPackageShare("control_launch"), - "/config/mpc_lateral_controller/mpc_lateral_controller.param.yaml", + "/config/trajectory_follower/lateral/mpc.param.yaml", ], "path to the parameter file of lateral controller. default is `mpc_follower`", ) @@ -292,7 +303,15 @@ def add_launch_arg(name: str, default_value=None, description=None): "lon_controller_param_path", [ FindPackageShare("control_launch"), - "/config/pid_longitudinal_controller/pid_longitudinal_controller.param.yaml", + "/config/trajectory_follower/longitudinal/pid.param.yaml", + ], + "path to the parameter file of longitudinal controller", + ) + add_launch_arg( + "trajectory_follower_node_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/trajectory_follower_node.param.yaml", ], "path to the parameter file of longitudinal controller", ) @@ -332,6 +351,13 @@ def add_launch_arg(name: str, default_value=None, description=None): # external cmd selector add_launch_arg("initial_selector_mode", "remote", "local or remote") + add_launch_arg( + "external_cmd_selector_param_path", + [ + FindPackageShare("external_cmd_selector"), + "/config/external_cmd_selector.param.yaml", + ], + ) # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") From 9078519ec52b6111d85986e96d77d29c5c4fa73a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 28 Dec 2022 13:17:34 +0900 Subject: [PATCH 05/11] feat(behavior_path_planner): update pull over params (#666) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner/pull_over/pull_over.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 75b5facaaa..5f6ebd2eec 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/planning_launch/config/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: 200.0 + 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. @@ -34,7 +34,7 @@ maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - after_pull_over_straight_distance: 5.0 + after_pull_over_straight_distance: 1.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true From 6a107493e1daf17fa37384117c3adcebaa443350 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 28 Dec 2022 13:22:17 +0900 Subject: [PATCH 06/11] fix(control_launch): add missin param in yaml (#665) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 4876699351..351ad877e1 100644 --- a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -12,9 +12,11 @@ lon_jerk_lim: 5.0 lat_acc_lim: 5.0 lat_jerk_lim: 5.0 + actual_steer_diff_lim: 1.0 on_transition: vel_lim: 50.0 lon_acc_lim: 1.0 lon_jerk_lim: 0.5 lat_acc_lim: 1.2 lat_jerk_lim: 0.75 + actual_steer_diff_lim: 0.05 From 5895271713b1ce04233e4ea72f4ddfad1a02c917 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 29 Dec 2022 15:38:26 +0900 Subject: [PATCH 07/11] feat(behavior_path_planner): change pull over max_lateral_offset (#670) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner/pull_over/pull_over.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml index 5f6ebd2eec..4c9f4fd41b 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -17,7 +17,7 @@ backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 3.0 - max_lateral_offset: 1.0 + max_lateral_offset: 0.5 lateral_offset_interval: 0.25 # occupancy grid map use_occupancy_grid: true From 3342ed48b764bb00bc87bfcc5dc8ed7a7061e59b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 5 Jan 2023 14:15:52 +0900 Subject: [PATCH 08/11] feat(planning_launch): lane change abort parameters (#583) * feat(planning_launch): lane change abort parameters Signed-off-by: Muhammad Zulfaqar Azmi * allow very high lateral jerk for now Signed-off-by: Muhammad Zulfaqar Azmi * remove unnecessary parameter Signed-off-by: Muhammad Zulfaqar Azmi * set abort to false Signed-off-by: Muhammad Zulfaqar Azmi * fix parameter Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change.param.yaml | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index 66d66fe780..891e54a544 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -9,21 +9,26 @@ backward_length_buffer_for_end_of_lane: 2.0 lane_change_finish_judge_buffer: 3.0 # [m] - double lane_changing_lateral_jerk: 0.5 - double lane_changing_lateral_acc: 0.5 + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] minimum_lane_change_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 10 - abort_lane_change_velocity_thresh: 0.5 - abort_lane_change_angle_thresh: 10.0 # [deg] - abort_lane_change_distance_thresh: 0.3 # [m] - prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] - - enable_abort_lane_change: true + # collision check enable_collision_check_at_prepare_phase: true + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] use_predicted_path_outside_lanelet: true use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug publish_debug_marker: false From f7c7bad092e09f421e16890e3d4301734c082daa Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Jan 2023 18:22:16 +0900 Subject: [PATCH 09/11] fix(planning_launch): ignore unavoidable objects around the goal (#676) * fix(planning_launch): ignore unavoidable objects around the goal Signed-off-by: satoshi-ota * Update planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml Signed-off-by: satoshi-ota Co-authored-by: Kosuke Takeuchi --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index e6461fa2b1..3af3a26836 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -18,6 +18,7 @@ object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] threshold_distance_object_is_on_center: 1.0 # [m] From 620625bd95187c369679dc3f9430c159dbcb70b3 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Thu, 5 Jan 2023 19:24:57 +0900 Subject: [PATCH 10/11] fix(behavior_velocity_planner): fix blind spot over-detection (#677) Signed-off-by: 1222-takeshi Signed-off-by: 1222-takeshi --- .../behavior_velocity_planner/blind_spot.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml index 31f75d7f7c..5af1c99f8a 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -6,3 +6,4 @@ backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] max_future_movement_time: 10.0 # [second] + threshold_yaw_diff: 0.523 # [rad] From 0b36fb959b228f4055cf95c3723ca00f97bafb6e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Jan 2023 09:24:03 +0900 Subject: [PATCH 11/11] feat(planning_launch): add new param for yield maneuver in avoidance.param.yaml (#657) * feat(planning_launch): update avoidance param Signed-off-by: satoshi-ota * fix(avoidance): fix param name for readability Signed-off-by: satoshi-ota * fix(planning_launch): add missing param Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 150 ++++++++++-------- 1 file changed, 85 insertions(+), 65 deletions(-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 3af3a26836..40de19bd6c 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -2,75 +2,26 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false enable_safety_check: false - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - object_check_goal_distance: 20.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For safety check - safety_check_backward_distance: 50.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - # For longitudinal margin - longitudinal_collision_margin_buffer: 0.0 # [m] - - prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] - - road_shoulder_safety_margin: 0.0 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - object_last_seen_threshold: 2.0 - - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] - - # bound clipping for objects - enable_bound_clipping: false + enable_yield_maneuver: false # for debug publish_debug_marker: false print_debug_info: false - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - # avoidance is performed for the object type with true target_object: car: true @@ -82,11 +33,80 @@ motorcycle: false pedestrian: false - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + # detection range + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 50.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.0 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] target_velocity_matrix: - col_size: 4 + col_size: 2 matrix: - [2.78, 5.56, 11.1, 13.9, # velocity [m/s] - 0.20, 0.90, 1.10, 1.20] # margin [m] + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m]