From 473e7e9ed0a14a326c75937c121018da1730d5e2 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 31 Jul 2024 19:45:17 +0900 Subject: [PATCH 1/8] feat(static_obstacle_avoidance): add force deactivation duration time (#1101) add force cancel duration time Signed-off-by: Go Sakayori --- .../static_obstacle_avoidance.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml index 4776884734..ef113f6ff2 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml @@ -243,6 +243,8 @@ # For cancel maneuver cancel: enable: true # [-] + force: + duration_time: 2.0 # [s] # For yield maneuver yield: From b947098423af3d3d299fb7723e7ea8fce55c145e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 31 Jul 2024 20:45:37 +0900 Subject: [PATCH 2/8] feat(autonomous_emergency_braking): enable AEB stop in vehicle_cmd_gate and diag_graph_agg (#1099) * enable emergency handling for AEB stop Signed-off-by: Daniel Sanchez * update AEB params to work better at 30 kmph Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking.param.yaml | 6 +++--- .../control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 2 +- .../mrm_emergency_stop_operator.param.yaml | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml b/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml index 49ed6ee171..0d86229b1d 100644 --- a/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml +++ b/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml @@ -2,14 +2,14 @@ ros__parameters: # Ego path calculation use_predicted_trajectory: true - use_imu_path: false + use_imu_path: true use_pointcloud_data: true use_predicted_object_data: false use_object_velocity_calculation: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 - mpc_prediction_time_horizon: 1.5 + mpc_prediction_time_horizon: 4.5 mpc_prediction_time_interval: 0.1 # Debug @@ -29,7 +29,7 @@ # Point cloud clustering cluster_tolerance: 0.15 #[m] - cluster_minimum_height: 0.0 + cluster_minimum_height: 0.1 minimum_cluster_size: 10 maximum_cluster_size: 10000 diff --git a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 74affea696..dd99e6d160 100644 --- a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -2,7 +2,7 @@ ros__parameters: update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 - use_emergency_handling: false + use_emergency_handling: true check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false enable_cmd_limit_filter: true diff --git a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml b/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml index 1ee2699a23..4d576457f2 100644 --- a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml +++ b/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: update_rate: 30 - target_acceleration: -2.5 - target_jerk: -1.5 + target_acceleration: -3.0 + target_jerk: -3.0 From 5b9551cdac21eb439943a08b24b647f52ea9c0c5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 1 Aug 2024 09:21:45 +0900 Subject: [PATCH 3/8] feat(behavior_path _planner): divide planner manager modules into dependent slots (#1091) --- .../scene_module_manager.param.yaml | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml index 59ef4157b8..9f270b0637 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml @@ -3,80 +3,80 @@ # NOTE: The smaller the priority number is, the higher the module priority is. /**: ros__parameters: + # NOTE: modules which are not set true in the preset is ignored in the slot configuration + slots: + # NOTE: array of array is not supported + - slot1 + - slot2 + - slot3 + - slot4 + slot1: + - "start_planner" + slot2: + - "side_shift" + - "avoidance_by_lane_change" + - "static_obstacle_avoidance" + - "lane_change_left" + - "lane_change_right" + - "external_request_lane_change_left" + - "external_request_lane_change_right" + slot3: + - "goal_planner" + slot4: + - "dynamic_obstacle_avoidance" + external_request_lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 6 external_request_lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 6 lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 5 lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: false - priority: 5 start_planner: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 0 side_shift: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 2 goal_planner: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false - keep_last: true - priority: 1 static_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 4 avoidance_by_lane_change: enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 3 dynamic_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - keep_last: true - priority: 7 sampling_planner: enable_module: true enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - keep_last: false - priority: 16 From 83c6907b5b0d7bc9b10505ec6b4981e69f17b36d Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 2 Aug 2024 10:30:05 +0900 Subject: [PATCH 4/8] feat(autonomous_emergency_braking): add info marker to aeb and state check override (#1103) * add info marker and override for state Signed-off-by: Daniel Sanchez * make stop wall viz default Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../autonomous_emergency_braking.param.yaml | 1 + autoware_launch/rviz/autoware.rviz | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml b/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml index 0d86229b1d..d5c6356c38 100644 --- a/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml +++ b/autoware_launch/config/control/autoware_autonomous_emergency_braking/autonomous_emergency_braking.param.yaml @@ -6,6 +6,7 @@ use_pointcloud_data: true use_predicted_object_data: false use_object_velocity_calculation: true + check_autoware_state: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 61e8cf522c..a524f9f9bf 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2461,6 +2461,19 @@ Visualization Manager: Reliability Policy: Reliable Value: /control/autonomous_emergency_braking/debug/markers Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Info/AEB + Namespaces: + stop_factor_text: true + stop_virtual_wall: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/autonomous_emergency_braking/info/markers + Value: true Enabled: true Name: Control - Class: rviz_common/Group From f489384a3a210fd4cd9ab14707886a3c29450f9c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 16:22:28 +0900 Subject: [PATCH 5/8] feat(lane_change): add param for lateral angle deviation (#1087) * RT1-6514 adding lateral angle deviation param Signed-off-by: Zulfaqar Azmi * decrease angle deviation threshold to fix rtc issue Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index d301e96deb..05b3bfd825 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -6,7 +6,6 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] - lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] @@ -109,7 +108,9 @@ overhang_tolerance: 0.0 # [m] unsafe_hysteresis_threshold: 5 # [/] - finish_judge_lateral_threshold: 0.2 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + finish_judge_lateral_threshold: 0.1 # [m] + finish_judge_lateral_angle_deviation: 1.0 # [deg] # debug publish_debug_marker: true From 2244a256e75b9cbd4c728f95497aae6a8210bde1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 2 Aug 2024 16:49:16 +0900 Subject: [PATCH 6/8] feat(lane_change): use different rss param to deal with parked vehicle (#1104) use separate rss for parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change.param.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index 05b3bfd825..29942b6e38 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -38,6 +38,14 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + parked: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.0 cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 From c5034033422c5fba15db1d035bbf4cd5114e94c1 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 8 Aug 2024 11:10:31 +0900 Subject: [PATCH 7/8] feat: increase the number of processes monitored by process_monitor (#1110) Signed-off-by: tomoya.kimura --- .../config/system/system_monitor/process_monitor.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/system_monitor/process_monitor.param.yaml b/autoware_launch/config/system/system_monitor/process_monitor.param.yaml index 3d6d82fae5..64303dd472 100644 --- a/autoware_launch/config/system/system_monitor/process_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/process_monitor.param.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - num_of_procs: 5 + num_of_procs: 40 From 1cba8e2fdc8b42180d85ba7990c3d2a9381dc188 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Aug 2024 02:31:11 +0900 Subject: [PATCH 8/8] fix(mpc_lateral_controller): publish predicted trajectory in Frenet coordinate and visualize it on Rviz (#1111) Signed-off-by: Takayuki Murooka --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- autoware_launch/rviz/autoware.rviz | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml index 37772ac574..135aecc56b 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -75,4 +75,4 @@ average_num: 1000 steering_offset_limit: 0.02 - debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate + debug_publish_predicted_trajectory: true # publish debug predicted trajectory in Frenet coordinate diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index a524f9f9bf..18c391f0db 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2396,7 +2396,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate Value: true View Path: Alpha: 1