From 9d0b03283bcfeb08406aa722d039b23848dc4147 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 7 Feb 2023 16:41:56 +0900 Subject: [PATCH 001/233] chore: change control parameter for X2 (#231) Signed-off-by: Tomohito Ando --- .../external_cmd_selector/external_cmd_selector.param.yaml | 2 +- .../control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml index 6556656cc8..3de702aa00 100644 --- a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml +++ b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: update_rate: 10.0 - initial_selector_mode: "local" # ["local", "remote"] + initial_selector_mode: "remote" # ["local", "remote"] 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 54c87f45b6..e451ea24f9 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,9 +2,9 @@ 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 + use_start_request: true enable_cmd_limit_filter: true filter_activated_count_threshold: 5 filter_activated_velocity_threshold: 1.0 From 90e524f90ebdc060774dc066565e44212bfdaba6 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Mon, 22 May 2023 01:38:22 +0900 Subject: [PATCH 002/233] feat: add fault injection params for x2 --- .../simulator/fault_injection.param.yaml | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index d608a7ab11..899307ebb1 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -35,3 +35,29 @@ /sensing/gnss/node_alive_monitoring: "gnss_connection" /system/node_alive_monitoring: "system_topic_status" /vehicle/node_alive_monitoring: "vehicle_topic_status" + cpu_frequency: "CPU Frequency" + cpu_load_average: "CPU Load Average" + gpu_frequency: "GPU Frequency" + lidar_blockage_validation: "blockage_validation" + lidar_visibility: "left_upper: visibility_validation" + memory_usage: "Memory Usage" + network_crc_error: "Network CRC Error" + network_ip_packet_reassembles_failed: "IP Packet Reassembles Failed" + network_traffic: "Network Traffic" + perception_topic_status: "perception_topic_status" + process_high_load: "High-load" + process_high_mem: "High-mem" + process_tasks_summary: "Tasks Summary" + remote_control_topic_status: "remote_control_topic_status" + sensing_topic_status: "sensing_topic_status" + storage_connection: "HDD Connection" + storage_power_on_hours: "HDD PowerOnHours" + storage_read_data_rate: "HDD ReadDataRate" + storage_read_iops: "HDD ReadIOPS" + storage_recovered_error: "HDD RecoveredError" + storage_total_data_written: "HDD TotalDataWritten" + storage_write_data_rate: "HDD WriteDataRate" + storage_write_iops: "HDD WriteIOPS" + trajectory_curvature_validation: "trajectory_curvature_validation" + trajectory_interval_validation : "trajectory_interval_validation" + trajectory_relative_angle_validation: "trajectory_relative_angle_validation" From e7b2a28df0b4615ed06d00f774bf235d45be2592 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 14 Nov 2022 16:58:52 +0900 Subject: [PATCH 003/233] feat(system_launch): use mrm comfortable stop (#199) Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara --- .../system/emergency_handler/emergency_handler.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/emergency_handler/emergency_handler.param.yaml b/autoware_launch/config/system/emergency_handler/emergency_handler.param.yaml index 652a984ce5..d064f2a2dd 100644 --- a/autoware_launch/config/system/emergency_handler/emergency_handler.param.yaml +++ b/autoware_launch/config/system/emergency_handler/emergency_handler.param.yaml @@ -7,7 +7,7 @@ timeout_takeover_request: 10.0 use_takeover_request: false use_parking_after_stopped: false - use_comfortable_stop: false + use_comfortable_stop: true # setting whether to turn hazard lamp on for each situation turning_hazard_on: From 96d5fdd3d504bf548e700b5a73421ef2bb05fbb8 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Fri, 28 Jan 2022 09:55:32 +0900 Subject: [PATCH 004/233] change default parameter for scenario_simulation to true --- autoware_launch/launch/logging_simulator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 8780578a6e..9635b0c351 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -33,7 +33,7 @@ - + From e8d4ef900d9659e26cf819b896aea3d41a2472e0 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 18 Aug 2022 16:03:36 +0900 Subject: [PATCH 005/233] chore: extend the range for ndt_scan_matching (#172) --- .../localization/ndt_scan_matcher.param.yaml | 84 +++++++++++++++++++ ...op_box_filter_measurement_range.param.yaml | 8 +- 2 files changed, 88 insertions(+), 4 deletions(-) create mode 100644 autoware_launch/config/localization/ndt_scan_matcher.param.yaml diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml new file mode 100644 index 0000000000..2e48924b9d --- /dev/null +++ b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml @@ -0,0 +1,84 @@ +/**: + ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true + + # Vehicle reference frame + base_frame: "base_link" + + # Subscriber queue size + input_sensor_points_queue_size: 1 + + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 + + # The newton line search maximum step length + step_size: 0.1 + + # The ND voxel grid resolution + resolution: 2.0 + + # The number of iterations required to calculate alignment + max_iterations: 30 + + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 + + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 + + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.1 + + # The number of particles to estimate initial pose + initial_estimate_particles_num: 100 + + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + + # neighborhood search method + # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 + neighborhood_search_method: 0 + + # Number of threads used for parallel computing + num_threads: 4 + + # The covariance of output pose + # Do note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] + + # Regularization switch + regularization_enabled: false + + # Regularization scale factor + regularization_scale_factor: 0.01 + + # Dynamic map loading distance + dynamic_map_loading_update_distance: 20.0 + + # Dynamic map loading loading radius + dynamic_map_loading_map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 + + # A flag for using scan matching score based on de-grounded LiDAR scan + estimate_scores_for_degrounded_scan: false + + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 diff --git a/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml index ad55423154..be5a50ef0f 100644 --- a/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml +++ b/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml @@ -2,10 +2,10 @@ ros__parameters: input_frame: "base_link" output_frame: "base_link" - min_x: -60.0 - max_x: 60.0 - min_y: -60.0 - max_y: 60.0 + min_x: -100.0 + max_x: 100.0 + min_y: -100.0 + max_y: 100.0 min_z: -30.0 max_z: 50.0 negative: False From 0fa45b9766b68a6a194dda3673f0a51bac2b6892 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Mon, 22 May 2023 01:53:05 +0900 Subject: [PATCH 006/233] feat(ground_segmentation): x2 setting --- .../ground_segmentation.param.yaml | 68 +++++++++++++++++-- 1 file changed, 62 insertions(+), 6 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 3f33218eb3..59ef508b1f 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - additional_lidars: [] + additional_lidars: ["front_upper", "front_lower"] ransac_input_topics: [] use_single_frame_filter: False use_time_series_filter: True @@ -19,18 +19,74 @@ plugin: "ground_segmentation::ScanGroundFilterComponent" parameters: global_slope_max_angle_deg: 10.0 - local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + local_slope_max_angle_deg: 25.0 # recommended 30.0 for non elevation_grid_mode split_points_distance_tolerance: 0.2 use_virtual_ground_point: True split_height_distance: 0.2 non_ground_height_threshold: 0.20 + grid_size_m: 0.2 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 5 + detection_range_z_max: 2.5 + elevation_grid_mode: true + center_pcl_shift: 0.0 + + # common_ground_filter: + # plugin: "ground_segmentation::RayGroundFilterComponent" + # parameters: + # general_max_slope: 10.0 + # local_max_slope: 10.0 + # min_height_threshold: 0.2 + + front_upper_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + max_z: 2.5 # recommended 2.5 for non elevation_grid_mode + min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + negative: False + + front_upper_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode + use_virtual_ground_point: False + non_ground_height_threshold: 0.1 grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 + center_pcl_shift: 0.0 elevation_grid_mode: true - use_recheck_ground_cluster: true - use_lowest_point: true - low_priority_region_x: -20.0 + + front_lower_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + max_z: 2.5 # recommended 2.5 for non elevation_grid_mode + min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + negative: False + + front_lower_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.20 # recommended 0.1 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.05 for non elevation_grid_mode + use_virtual_ground_point: False + non_ground_height_threshold: 0.1 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 center_pcl_shift: 0.0 - radial_divider_angle_deg: 1.0 + elevation_grid_mode: true + use_recheck_ground_cluster: false From 5ac3e84a04f2299b4cd384349d17c579e8200916 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Mon, 22 May 2023 22:02:08 +0900 Subject: [PATCH 007/233] feat: specify input pointcloud for localization --- .../launch/components/tier4_localization_component.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 7dfe0a3dea..b6e2f8f18a 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -3,8 +3,8 @@ - - + + From 876fef8e2888dfa3508691b039f789e8c2440523 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 13 Jun 2023 09:33:22 +0900 Subject: [PATCH 008/233] feat: use ml model provider (#304) Signed-off-by: wep21 --- .../launch/components/tier4_perception_component.launch.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index e13b987607..7ac4b3dd3b 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -24,6 +24,10 @@ + + + + Date: Thu, 15 Jun 2023 14:16:47 +0900 Subject: [PATCH 009/233] feat(avoidance): change object_check_backward_distance from 100m to 10m (#388) Signed-off-by: kosuke55 --- .../avoidance/avoidance.param.yaml | 167 ++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 0000000000..f7f1cfc07e --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,167 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + 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] + + # 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_force_avoidance_for_stopped_vehicle: false + enable_safety_check: true + enable_yield_maneuver: true + enable_yield_maneuver_during_shifting: false + disable_path_update: false + use_hatched_road_markings: false + + # for debug + publish_debug_marker: false + print_debug_info: false + + # avoidance is performed for the object type with true + target_object: + car: + enable: true # [-] + max_expand_ratio: 0.0 # [-] + envelope_buffer_margin: 0.3 # [m] + safety_buffer_lateral: 0.7 # [m] + safety_buffer_longitudinal: 0.0 # [m] + truck: + enable: true + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + bus: + enable: true + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + trailer: + enable: true + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + unknown: + enable: false + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + safety_buffer_lateral: 0.7 + safety_buffer_longitudinal: 0.0 + bicycle: + enable: false + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 + motorcycle: + enable: false + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 + pedestrian: + enable: false + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + safety_buffer_lateral: 1.0 + safety_buffer_longitudinal: 1.0 + lower_distance_for_polygon_expansion: 30.0 # [m] + upper_distance_for_polygon_expansion: 100.0 # [m] + + # 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] + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # detection range + object_ignore_distance_traffic_light: 30.0 # [m] + object_ignore_distance_crosswalk_forward: 30.0 # [m] + object_ignore_distance_crosswalk_backward: 30.0 # [m] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 10.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: 100.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_execution_threshold: 0.499 # [m] + lateral_small_shift_threshold: 0.101 # [m] + road_shoulder_safety_margin: 0.3 # [m] + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + 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] + + # 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: 2 + matrix: + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] + + shift_line_pipeline: + trim: + quantize_filter_threshold: 0.2 + same_grad_filter_1_threshold: 0.1 + same_grad_filter_2_threshold: 0.2 + same_grad_filter_3_threshold: 0.5 + sharp_shift_filter_threshold: 0.2 From 6c4692494ab49a63b730c7027547b2bfa9536282 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 19 Jun 2023 13:41:12 +0900 Subject: [PATCH 010/233] feat(avoidance): can set stop/move judge threshold for each object class (#399) Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index f7f1cfc07e..d660de0743 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -29,48 +29,64 @@ target_object: car: enable: true # [-] + moving_speed_threshold: 1.0 # [m/s] + moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: enable: true + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bus: enable: true + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 trailer: enable: true + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 unknown: enable: false + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bicycle: enable: false + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: enable: false + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: enable: false + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 safety_buffer_lateral: 1.0 @@ -81,8 +97,6 @@ # 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] threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] # detection range object_ignore_distance_traffic_light: 30.0 # [m] From af99c8a5e30bca213dc917bef402ea74d7bed502 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 19 Jun 2023 21:54:04 +0900 Subject: [PATCH 011/233] feat(start_planner): start with acceleration (#402) Signed-off-by: kosuke55 --- .../start_planner/start_planner.param.yaml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 7d64d8c44a..7ebaf3a195 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -26,12 +26,11 @@ allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 - maximum_longitudinal_deviation: 1.0 # Note, should be the same or less than planning validator's "longitudinal_distance_deviation" + deceleration_interval: 15.0 lateral_jerk: 0.5 - lateral_acceleration_sampling_num: 3 + lateral_acceleration_sampling_num: 1 minimum_lateral_acc: 0.15 maximum_lateral_acc: 0.5 - maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true geometric_collision_check_distance_from_end: 0.0 From 6f2695d505567650f354719842142af0f923f18d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Jun 2023 16:53:09 +0900 Subject: [PATCH 012/233] feat(start_planner): change lateral acceleration sampling num (#404) Signed-off-by: kosuke55 --- .../start_planner/start_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 7ebaf3a195..2db0bf842c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -28,7 +28,7 @@ minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 - lateral_acceleration_sampling_num: 1 + lateral_acceleration_sampling_num: 3 minimum_lateral_acc: 0.15 maximum_lateral_acc: 0.5 # geometric pull out From dc9d4fd4ce48576273ceef2e08a4b2b2878b7973 Mon Sep 17 00:00:00 2001 From: Riki OTA Date: Thu, 29 Jun 2023 10:53:53 +0900 Subject: [PATCH 013/233] feat: change centerpoint model path (#314) feat: change centerpoint model path (#313) * chore: change centerpoint model path * Parameter to specify model path --------- Co-authored-by: Shohei Sakai --- autoware_launch/launch/autoware.launch.xml | 4 ++++ .../launch/components/tier4_perception_component.launch.xml | 4 ++-- autoware_launch/launch/logging_simulator.launch.xml | 3 +++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 38f8550f72..58ae8108ee 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -48,6 +48,10 @@ + + + + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 7ac4b3dd3b..b1f0703aac 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -26,8 +26,8 @@ - - + + + + + From cd5027a6a59f56213d148529389dd0183994f24c Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 14 Jul 2023 11:07:34 +0900 Subject: [PATCH 014/233] fix: launch args of centerpoint model path do not work (#326) fix launch parameter of centerpoint model path --- .../launch/components/tier4_perception_component.launch.xml | 4 ++-- autoware_launch/launch/logging_simulator.launch.xml | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index b1f0703aac..fdfba588b5 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -26,8 +26,8 @@ - - + + + + + From 52e39155afbac72c934a7cf87bc503eae391018a Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 19 Jul 2023 09:52:12 +0900 Subject: [PATCH 015/233] chore(run_out): disable slow down limit (#332) Signed-off-by: Tomohito Ando --- .../behavior_velocity_planner/run_out.param.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index c8905f66da..910ffc6c6c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -67,8 +67,14 @@ margin_behind: 0.5 # [m] ahead margin for detection area length margin_ahead: 1.0 # [m] behind margin for detection area length + # parameter to avoid sudden stopping + slow_down_limit: + enable: false + max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. + max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + # Only used when "detection_method" is set to Points # Points in this area are detected even if it is in the no obstacle segmentation area # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area \ No newline at end of file From 42321f2ef8b8f3ee97b0ca9c19a22ecd9b71974e Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 7 Aug 2023 09:48:20 +0900 Subject: [PATCH 016/233] chore(perception): change namespace of traffic light nodes (#342) Signed-off-by: Tomohito Ando --- .../launch/components/tier4_perception_component.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index fdfba588b5..70cde4433e 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -28,6 +28,7 @@ + Date: Mon, 7 Aug 2023 14:21:40 +0900 Subject: [PATCH 017/233] chore(perception): set fusion_only to true (#341) Signed-off-by: Tomohito Ando --- .../launch/components/tier4_perception_component.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 70cde4433e..ff8bd6c464 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -28,6 +28,7 @@ + From ce0349a754533b9909609f6b62729c8c5f0a5a93 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Wed, 5 Jul 2023 07:28:48 +0900 Subject: [PATCH 018/233] feat: disable lane departure checker (#317) * feat(obstacle_avoidance_planner): change optimization_center_offset Signed-off-by: 1222-takeshi * feat(system_error_monitor): do not monitor lane_departure_checker Signed-off-by: 1222-takeshi * feat: add comment Signed-off-by: 1222-takeshi --------- Signed-off-by: 1222-takeshi --- .../autoware_path_optimizer/path_optimizer.param.yaml | 4 ++-- .../system_error_monitor/system_error_monitor.param.yaml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index 17a044fb67..93cc644040 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -47,11 +47,11 @@ num_points: 100 # number of points for optimization [-] delta_arc_length: 1.0 # delta arc length for optimization [m] - # kinematics: + kinematics: # If this parameter is commented out, the parameter is set as below by default. # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8` # The 0.8 scale is adopted as it performed the best. - # optimization_center_offset: 2.3 # optimization center offset from base link + optimization_center_offset: 0.0 # optimization center offset from base link clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory # if collision_free_constraints.option.hard_constraint is true diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml index c1821a05ab..daa38a7b2b 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml @@ -18,7 +18,7 @@ required_modules: autonomous_driving: /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + # /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default # temporarily until logic is improved /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/localization/node_alive_monitoring: default From cad12cb4645cc8d15c3d5aae293e4d038a662b2d Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Wed, 5 Jul 2023 13:28:31 +0900 Subject: [PATCH 019/233] chore: update converged param (#318) chore: update likelihood threshold --- autoware_launch/config/localization/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml index 2e48924b9d..4c29059581 100644 --- a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml +++ b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml @@ -32,7 +32,7 @@ # If converged_param_type is 1 # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.1 + converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose initial_estimate_particles_num: 100 From 05d78a3f912674653ed224f6c2c01fba8d73bbf1 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 6 Jul 2023 17:00:19 +0900 Subject: [PATCH 020/233] chore(start_planner): disable back (#320) --- .../start_planner/start_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 2db0bf842c..75b20bbbfe 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -42,7 +42,7 @@ backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg # search start pose backward - enable_back: true + enable_back: false search_priority: "efficient_path" # "efficient_path" or "short_back_distance" max_back_distance: 20.0 backward_search_resolution: 2.0 From df4a839c6dc6a50fa306fbda0613fe8083caa544 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 9 Jul 2023 13:42:06 +0900 Subject: [PATCH 021/233] feat(behavior_path_planner): update behavior path module config (#323) Signed-off-by: satoshi-ota --- .../scene_module_manager.param.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 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 59289d30f5..d54c1b210f 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 @@ -6,7 +6,7 @@ external_request_lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: false - enable_simultaneous_execution_as_candidate_module: true + enable_simultaneous_execution_as_candidate_module: false keep_last: false priority: 6 max_module_size: 1 @@ -14,7 +14,7 @@ external_request_lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: false - enable_simultaneous_execution_as_candidate_module: true + enable_simultaneous_execution_as_candidate_module: false keep_last: false priority: 6 max_module_size: 1 @@ -22,22 +22,22 @@ lane_change_left: enable_rtc: false enable_simultaneous_execution_as_approved_module: true - enable_simultaneous_execution_as_candidate_module: true + enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 5 + priority: 4 max_module_size: 1 lane_change_right: enable_rtc: false enable_simultaneous_execution_as_approved_module: true - enable_simultaneous_execution_as_candidate_module: true + enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 5 + priority: 4 max_module_size: 1 start_planner: enable_rtc: false - enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false priority: 0 From 215c4ed48aed06ab366321bf8944d6c1f099caf1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 9 Jul 2023 13:42:20 +0900 Subject: [PATCH 022/233] feat(avoidacne): update avoidance config (#324) Signed-off-by: satoshi-ota Signed-off-by: Tomohito Ando --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index d660de0743..8f0c319464 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -127,6 +127,7 @@ lateral_collision_margin: 1.0 # [m] lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] + lateral_avoid_check_threshold: 0.1 # [m] road_shoulder_safety_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 From 08269ce0251690403256b136dd7542305d209057 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 13 Jul 2023 09:35:52 +0900 Subject: [PATCH 023/233] fix: netmonitor config (#321) fix(system_monitor): restrict monitored port --- .../system/system_monitor/net_monitor.param.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/system/system_monitor/net_monitor.param.yaml b/autoware_launch/config/system/system_monitor/net_monitor.param.yaml index d72b8d1334..adcae5a458 100644 --- a/autoware_launch/config/system/system_monitor/net_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/net_monitor.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - devices: ["*"] - traffic_reader_port: 7636 - monitor_program: "greengrass" - crc_error_check_duration: 1 - crc_error_count_threshold: 1 - reassembles_failed_check_duration: 1 - reassembles_failed_check_count: 1 + devices: ["enp2s0f1"] + traffic_reader_port: 7636 + monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 + reassembles_failed_check_duration: 1 + reassembles_failed_check_count: 1 From 4809fa9dea13133366404315966536dc982297e2 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Fri, 21 Jul 2023 13:03:10 +0900 Subject: [PATCH 024/233] chore: update perception settings (#329) * chore: use unknown object * feat: expand camera usage range * chore(ground_segmentation): change z range Signed-off-by: Tomohito Ando --- .../ground_segmentation.param.yaml | 14 +++++++------- .../tier4_perception_component.launch.xml | 2 ++ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 59ef508b1f..c0cad93c6a 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -11,8 +11,8 @@ max_x: 150.0 min_y: -70.0 max_y: 70.0 - margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height - margin_min_z: -2.5 # to extend the crop box min_z from ground + max_z: 3.2 + min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False common_ground_filter: @@ -27,7 +27,7 @@ grid_size_m: 0.2 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 5 - detection_range_z_max: 2.5 + detection_range_z_max: 3.2 elevation_grid_mode: true center_pcl_shift: 0.0 @@ -44,7 +44,7 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 2.5 # recommended 2.5 for non elevation_grid_mode + max_z: 3.2 # recommended 2.5 for non elevation_grid_mode min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False @@ -60,7 +60,7 @@ grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 - detection_range_z_max: 2.5 + detection_range_z_max: 3.2 center_pcl_shift: 0.0 elevation_grid_mode: true @@ -70,7 +70,7 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 2.5 # recommended 2.5 for non elevation_grid_mode + max_z: 3.2 # recommended 2.5 for non elevation_grid_mode min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False @@ -86,7 +86,7 @@ grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 - detection_range_z_max: 2.5 + detection_range_z_max: 3.2 center_pcl_shift: 0.0 elevation_grid_mode: true use_recheck_ground_cluster: false diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index ff8bd6c464..39e117e05b 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -30,6 +30,8 @@ + + Date: Fri, 21 Jul 2023 17:52:46 +0900 Subject: [PATCH 025/233] feat: enable camera_lidar_radar_fusion (#330) --- autoware_launch/launch/autoware.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 58ae8108ee..1d3b850f60 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -43,7 +43,7 @@ - + From ab476dfe84b77dd9023d9df166f880290f6afdc2 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 3 Aug 2023 10:17:03 +0900 Subject: [PATCH 026/233] fix(occlusion_spot): add lacking param (#336) Signed-off-by: satoshi-ota Co-authored-by: satoshi-ota Signed-off-by: Tomohito Ando --- .../trajectory_follower/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/longitudinal/pid.param.yaml | 12 ++++++------ .../autoware_velocity_smoother/Analytical.param.yaml | 4 ++-- 3 files changed, 10 insertions(+), 10 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..ffcfad3e39 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -45,8 +45,8 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.27 # steering dynamics time constant (1d approximation) [s] + input_delay: 0.17 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.15 # steering dynamics time constant (1d approximation) [s] steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 5f023e1685..22575eceee 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - delay_compensation_time: 0.1 + delay_compensation_time: 0.15 enable_smooth_stop: true enable_overshoot_emergency: true @@ -58,16 +58,16 @@ # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 - emergency_jerk: -3.0 + emergency_acc: -2.5 + emergency_jerk: -1.5 # acceleration limit - max_acc: 3.0 - min_acc: -5.0 + max_acc: 1.86 + min_acc: -3.36 # jerk limit max_jerk: 2.0 - min_jerk: -5.0 + min_jerk: -2.0 # slope compensation lpf_pitch_gain: 0.95 diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml index 329714e3d3..232610713f 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml @@ -17,8 +17,8 @@ kp: 0.3 backward: - start_jerk: -0.1 - min_jerk_mild_stop: -0.3 + start_jerk: -0.3 + min_jerk_mild_stop: -0.5 min_jerk: -1.5 min_acc_mild_stop: -1.0 min_acc: -2.5 From 4847c81d2c7b6c8fde0b94cf60278966def89de5 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Mon, 7 Aug 2023 15:25:56 +0900 Subject: [PATCH 027/233] chore(system_error_monitor): update diag (#334) * chore(system_error_monitor): update diag * style(pre-commit): autofix --------- Co-authored-by: 0x126 <0x126@users.noreply.github.com> --- .../diagnostic_aggregator/system.param.yaml | 9 +++++++++ .../system_error_monitor.param.yaml | 13 ++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/system.param.yaml b/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/system.param.yaml index 27cbe3fed2..5d559b56cb 100644 --- a/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/system.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/system.param.yaml @@ -13,3 +13,12 @@ path: storage_error contains: ["bagpacker"] timeout: 3.0 + fms_connection: + type: diagnostic_aggregator/AnalyzerGroup + path: fms_connection + analyzers: + connection_error: + type: diagnostic_aggregator/GenericAnalyzer + path: connection_error + contains: ["edge_core_internet_connection"] + timeout: 10.0 diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml index daa38a7b2b..86a0d6dbb6 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml @@ -17,32 +17,43 @@ ros__parameters: required_modules: autonomous_driving: + # Control (from control.param.yaml) /autoware/control/autonomous_driving/node_alive_monitoring: default # /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default # temporarily until logic is improved /autoware/control/control_command_gate/node_alive_monitoring: default + # Localization (from localization.param.yaml) /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/scan_matching_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/localization/performance_monitoring/localization_stability: default /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "error", lf_at: "none", spf_at: "none" } + # Map (from map.param.yaml) /autoware/map/node_alive_monitoring: default + # Perception (from perception.param.yaml) /autoware/perception/node_alive_monitoring: default + # Planning (from planning.param.yaml) /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default + # Sensors (from sensor_kit.param.yaml) # /autoware/sensing/node_alive_monitoring: default - # /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false", ignore_until_waiting_for_route: "true"} + /autoware/sensing/lidar/pandar/health_monitoring/connection: { sf_at: "none", lf_at: "none", spf_at: "error", auto_recovery: "true"} + /autoware/sensing/lidar/pandar/health_monitoring/temperature: { sf_at: "warn", lf_at: "error", spf_at: "none", auto_recovery: "true"} + /autoware/sensing/lidar/pandar/health_monitoring/ptp: { sf_at: "none", lf_at: "none", spf_at: "warn", auto_recovery: "true"} + # System (from system.param.yaml in universe and system.param.yaml in system_launch) /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/system/duplicated_node_checker: default + /autoware/system/fms_connection: { sf_at: "warn", lf_at: "none", spf_at: "none" } + # Vehicle (from vehicle.param.yaml in universe and vehicle.param.yaml in system_launch) /autoware/vehicle/node_alive_monitoring: default external_control: From 839e5459ddac87982358e772ff3ebffbb9da6016 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 8 Aug 2023 09:51:34 +0900 Subject: [PATCH 028/233] refactor(traffic_light_arbiter): add traffic_light_arbiter param file (#343) refactor(traffic_light_arbiter): add traffic_light_arbiter param file (#489) Signed-off-by: Tomohito Ando Co-authored-by: Kenzo Lobos Tsunekawa Signed-off-by: Tomohito Ando --- .../launch/components/tier4_perception_component.launch.xml | 1 + .../launch/components/tier4_simulator_component.launch.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 39e117e05b..efc4e2111b 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -32,6 +32,7 @@ + + Date: Thu, 17 Aug 2023 17:10:38 +0900 Subject: [PATCH 029/233] feat(behavior_velocity_planner): change walkway stop line distance (#350) feat(behavior_velocity_planner): change walkway stop line distance margin Signed-off-by: Makoto Kurihara Signed-off-by: Tomohito Ando --- .../behavior_velocity_planner/walkway.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml index 07f493edcd..778d685141 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml @@ -2,4 +2,4 @@ ros__parameters: walkway: stop_duration: 0.1 # [s] stop time at stop position - stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists + stop_distance_from_crosswalk: 1.5 # [m] make stop line away from crosswalk when no explicit stop line exists From 33c34411cc44f9ea21b7f19669934cb993d08ef5 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 9 Aug 2023 17:32:18 +0900 Subject: [PATCH 030/233] feat: launch V2X node in autoware launch (#349) * feat: launch V2X node in autoware launch Signed-off-by: Tomohito Ando * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: TomohitoAndo --- autoware_launch/launch/autoware.launch.xml | 10 ++++++++++ autoware_launch/launch/logging_simulator.launch.xml | 2 ++ autoware_launch/launch/planning_simulator.launch.xml | 2 ++ 3 files changed, 14 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 1d3b850f60..a172314df5 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -22,6 +22,7 @@ + @@ -125,6 +126,15 @@ + + + + + + + + + + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index d038e7096b..bb0a9f7711 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -72,6 +72,8 @@ + + From ee931d7d3f46868441cd0e49ebaaf2154798ec28 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 14 Aug 2023 11:09:50 +0900 Subject: [PATCH 031/233] feat(behavior_path_planner): set params for short distance lane change (#351) Signed-off-by: Makoto Kurihara --- .../lane_change/lane_change.param.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 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 10757691c9..ea81d53288 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 @@ -2,15 +2,15 @@ ros__parameters: lane_change: backward_lane_length: 200.0 #[m] - prepare_duration: 4.0 # [s] + prepare_duration: 3.0 # [s] - 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] + backward_length_buffer_for_end_of_lane: 7.0 # [m] + backward_length_buffer_for_blocking_object: 0.5 # [m] + lane_change_finish_judge_buffer: 0.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] - minimum_lane_changing_velocity: 2.78 # [m/s] + minimum_lane_changing_velocity: 0.75 # [m/s] prediction_time_resolution: 0.5 # [s] longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 @@ -63,8 +63,8 @@ # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] - min_values: [0.4, 0.4, 0.4] - max_values: [0.65, 0.65, 0.65] + min_values: [0.15, 0.15, 0.15] + max_values: [0.75, 0.75, 0.75] # target object target_object: From 8f11ca52ec5ad193d4306cd8c2b0f5b6f640427b Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 15 Aug 2023 20:56:47 +0900 Subject: [PATCH 032/233] feat(behavior_path_planner): configure dynamic drivable area expansion (#352) * feat(behavior_path_planner): configure dynamic drivable area expansion Signed-off-by: Makoto Kurihara * fix(behavior_path_planner): fix to expand only front offset Signed-off-by: Makoto Kurihara --------- Signed-off-by: Makoto Kurihara --- .../drivable_area_expansion.param.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index f3393ff5a4..53ea2d60cd 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -18,6 +18,11 @@ extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase extra_front_overhang: 0.5 # [m] extra length to add to the front overhang extra_width: 1.0 # [m] extra length to add to the width + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the ego footprint + rear: 0.5 # [m] extra length to add to the rear of the ego footprint + left: 0.5 # [m] extra length to add to the left of the ego footprint + right: 0.5 # [m] extra length to add to the rear of the ego footprint dynamic_objects: avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -34,3 +39,6 @@ - road_border - curbstone distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid + compensate: + enable: false # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction + extra_distance: 3.0 # [m] extra distance to add to the compensation From 17d4dd317f2e2e540adde9735d11c1a8c5b85e50 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 16 Aug 2023 11:39:20 +0900 Subject: [PATCH 033/233] feat(autoware_launch): changes for shiojiri run (#356) * chore(autoware_launch): disable rviz on main ecu Signed-off-by: t4-x2 * feat(autoware_launch): change perception type to camera_lidar_fusion Signed-off-by: t4-x2 --------- Signed-off-by: t4-x2 Co-authored-by: t4-x2 --- autoware_launch/launch/autoware.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index a172314df5..efed227402 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -40,11 +40,11 @@ - + - + From f36d8d54034ba288b34071d5c9aa060098d1bbbd Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 16 Aug 2023 11:41:17 +0900 Subject: [PATCH 034/233] feat(behavior_path_planner): tune for short distance lane change (#354) Signed-off-by: Makoto Kurihara Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Signed-off-by: Tomohito Ando --- .../lane_change/lane_change.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 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 ea81d53288..ca7d01e593 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 @@ -8,9 +8,9 @@ backward_length_buffer_for_blocking_object: 0.5 # [m] lane_change_finish_judge_buffer: 0.0 # [m] - lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_jerk: 0.75 # [m/s3] - minimum_lane_changing_velocity: 0.75 # [m/s] + minimum_lane_changing_velocity: 1.0 # [m/s] prediction_time_resolution: 0.5 # [s] longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 @@ -64,7 +64,7 @@ lateral_acceleration: velocity: [0.0, 4.0, 10.0] min_values: [0.15, 0.15, 0.15] - max_values: [0.75, 0.75, 0.75] + max_values: [1.25, 1.25, 1.25] # target object target_object: From 3152528fb08fb3a370a565e7fdeefe48e3f0383f Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 21 Aug 2023 14:05:36 +0900 Subject: [PATCH 035/233] chore(avoidance): update road shoulder margin (#367) Signed-off-by: Tomohito Ando --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 8f0c319464..d392c67cbd 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -128,7 +128,8 @@ lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] lateral_avoid_check_threshold: 0.1 # [m] - road_shoulder_safety_margin: 0.3 # [m] + soft_road_shoulder_margin: 0.8 # [m] + hard_road_shoulder_margin: 0.8 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 # avoidance distance parameters From bea24e01d0a2940c1e45d528ad6ea7c9ed2bc59a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Sun, 20 Aug 2023 22:02:40 +0900 Subject: [PATCH 036/233] chore(rtc_auto_mode_manager): disable auto approval for the avoidance module (#361) chore(rtc_auto_mode_manager): disable auto approval for the avoidance module Signed-off-by: Makoto Kurihara --- .../behavior_planning/rtc_replayer/rtc_replayer.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml index 9d761b15ee..c0e3e147dc 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml @@ -26,8 +26,8 @@ - "traffic_light" - "lane_change_left" - "lane_change_right" - - "avoidance_left" - - "avoidance_right" + # - "avoidance_left" + # - "avoidance_right" - "avoidance_by_lane_change_left" - "avoidance_by_lane_change_right" - "goal_planner" From 16eae7fec2a79d1339bd62152ff797c911c8a85b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 22 Aug 2023 15:23:20 +0900 Subject: [PATCH 037/233] change merge_from_private stop_margin to 0.5m Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 069f336959..e84d4cf152 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -117,6 +117,6 @@ intersection_to_occlusion: false merge_from_private: - stopline_margin: 3.0 + stopline_margin: 1.7 stop_duration_sec: 1.0 stop_distance_threshold: 1.0 From 63d5699e63c8845ba7b997a6a4065774e8fcf379 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 22 Aug 2023 11:17:12 +0900 Subject: [PATCH 038/233] feat(autoware_launch): no conflict of arc length between drivable area expansion and optimization path planning Signed-off-by: Takayuki Murooka --- .../drivable_area_expansion.param.yaml | 7 +++++++ .../autoware_path_optimizer/path_optimizer.param.yaml | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 53ea2d60cd..3ef779a8a9 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -34,6 +34,13 @@ max_arc_length: 100.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. + expansion: + method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. + # 'lanelet': add lanelets overlapped by the ego footprints + # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area + max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + max_path_arc_length: 80.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index 93cc644040..ea859fcde3 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -45,7 +45,7 @@ common: num_points: 100 # number of points for optimization [-] - delta_arc_length: 1.0 # delta arc length for optimization [m] + delta_arc_length: 0.5 # delta arc length for optimization [m] kinematics: # If this parameter is commented out, the parameter is set as below by default. From 173ad3279d50cf6f3647b44b62f38df6ab049a5e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 22 Aug 2023 11:15:51 +0900 Subject: [PATCH 039/233] feat(autoware_launch): set terminal obstacle stop margin to 6.0 Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 42e5ca054a..32e2f41ae7 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -12,8 +12,8 @@ idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] - safe_distance_margin : 5.0 # This is also used as a stop margin [m] - terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + safe_distance_margin : 6.0 # This is also used as a stop margin [m] + terminal_safe_distance_margin : 6.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] From a4b71372cfa2003281a1499de0651a2987b65321 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 24 Aug 2023 16:32:50 +0900 Subject: [PATCH 040/233] chore(e2e_simulator_launch): disable V2X in e2e simulator (#378) Signed-off-by: Tomohito Ando --- autoware_launch/launch/e2e_simulator.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml index d05d0acb62..718acc0ce8 100644 --- a/autoware_launch/launch/e2e_simulator.launch.xml +++ b/autoware_launch/launch/e2e_simulator.launch.xml @@ -81,6 +81,8 @@ + + From 9961f402dd8b28d9df93a2504c92a967767129a7 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 29 Aug 2023 11:47:57 +0900 Subject: [PATCH 041/233] feat(trajectory_follower): use trajectory for pitch calculation (#382) Signed-off-by: Makoto Kurihara --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 22575eceee..060bc6d9ca 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -70,6 +70,8 @@ min_jerk: -2.0 # slope compensation + # pitch + use_trajectory_for_pitch_calculation: true lpf_pitch_gain: 0.95 slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive adaptive_trajectory_velocity_th: 1.0 From 366c3105fb1084985635bc951c14df8daccbde5c Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 29 Aug 2023 11:48:35 +0900 Subject: [PATCH 042/233] feat(behavior_path_planner): prevent from approaching the curbstone (#384) Signed-off-by: Makoto Kurihara --- .../autoware_path_optimizer/path_optimizer.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index ea859fcde3..5b6fb7d174 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -51,7 +51,7 @@ # If this parameter is commented out, the parameter is set as below by default. # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8` # The 0.8 scale is adopted as it performed the best. - optimization_center_offset: 0.0 # optimization center offset from base link + optimization_center_offset: 2.0 # optimization center offset from base link clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory # if collision_free_constraints.option.hard_constraint is true From 153d2a47bc0160af640cb8587bd3924a1b62d6ff Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Thu, 31 Aug 2023 11:26:16 +0900 Subject: [PATCH 043/233] Disable yaw bias estimation of EKF --- .../config/localization/ekf_localizer.param.yaml | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 19bfd2d498..f52051d44f 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -1,12 +1,10 @@ /**: ros__parameters: - node: - show_debug_info: false - enable_yaw_bias_estimation: true - predict_frequency: 50.0 - tf_rate: 50.0 - publish_tf: true - extend_state_step: 50 + show_debug_info: false + enable_yaw_bias_estimation: false + predict_frequency: 50.0 + tf_rate: 50.0 + extend_state_step: 50 pose_measurement: # for Pose measurement From 0dcff162c61ef42f533b7bb101bf10d78001023d Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 8 Sep 2023 14:34:34 +0900 Subject: [PATCH 044/233] Adjust parameters according to vehicle size --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 32e2f41ae7..9c6e16a87b 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -105,7 +105,7 @@ max_obstacles_collision_time: 10.0 # how far the blocking obstacle stopped_obstacle_velocity_threshold: 0.5 slow_down: - max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.8 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 successive_num_to_entry_slow_down_condition: 5 From aeb89014c811f5ee21a574c53a6aec1ccc5b4fa0 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 13 Sep 2023 09:09:47 +0900 Subject: [PATCH 045/233] chore: display virtul wall of obstacle cruise slowdown (#407) display virtul wall of obstacle cruise slowdown --- autoware_launch/rviz/autoware.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 61e8cf522c..820d32829c 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2215,7 +2215,7 @@ Visualization Manager: Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: true + Enabled: false Name: DebugMarker Namespaces: {} @@ -2226,7 +2226,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker Value: true - Enabled: false + Enabled: true Name: ObstacleCruise - Class: rviz_default_plugins/MarkerArray Enabled: false From 88c5af3c984c266d89c553fd685bebef5d2add80 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Wed, 13 Sep 2023 09:34:47 +0900 Subject: [PATCH 046/233] fix: lidar_model_param_path (#388) * fix: lidar_model_param_path * chore: move * chore: delete unused param --- .../launch/components/tier4_perception_component.launch.xml | 2 +- autoware_launch/launch/logging_simulator.launch.xml | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index efc4e2111b..6008aae2a6 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -27,7 +27,7 @@ - + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index cfb833083b..93931cb752 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -36,7 +36,6 @@ - @@ -74,7 +73,6 @@ - From f8c6ff9e8bcb9f749510bc2d8d8fcf7415f160bb Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 15 Sep 2023 09:51:32 +0900 Subject: [PATCH 047/233] chore(tier4_simulator_component): change namespace1 to camera7 (#426) Signed-off-by: Tomohito Ando --- .../launch/components/tier4_simulator_component.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/launch/components/tier4_simulator_component.launch.xml b/autoware_launch/launch/components/tier4_simulator_component.launch.xml index 46cd3dc080..998cfc7cd4 100644 --- a/autoware_launch/launch/components/tier4_simulator_component.launch.xml +++ b/autoware_launch/launch/components/tier4_simulator_component.launch.xml @@ -28,6 +28,7 @@ + Date: Mon, 25 Sep 2023 14:37:17 +0900 Subject: [PATCH 048/233] fix(autoware_launch): tune avoidance clearance (#444) Tune avoidance clearance --- .../avoidance/avoidance.param.yaml | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index d392c67cbd..afb0674029 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -33,7 +33,8 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - safety_buffer_lateral: 0.7 # [m] + avoid_margin_lateral: 0.0 # [m] + safety_buffer_lateral: 0.8 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: enable: true @@ -41,7 +42,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 bus: enable: true @@ -49,7 +51,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 trailer: enable: true @@ -57,7 +60,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 unknown: enable: false @@ -73,7 +77,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.5 safety_buffer_longitudinal: 1.0 motorcycle: enable: false @@ -81,7 +86,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.5 safety_buffer_longitudinal: 1.0 pedestrian: enable: false @@ -89,7 +95,8 @@ moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.5 safety_buffer_longitudinal: 1.0 lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] From 3f0b5cecf93081cf63063383e618b1664fde9703 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Thu, 5 Oct 2023 09:22:42 +0900 Subject: [PATCH 049/233] fix(autoware_launch): tune avoidance clearance v2.3.0 (#445) Tune avoidance clearance From 6d5fe21b0f3c4bea36700bc1c254acbf94fc0e2b Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Sat, 7 Oct 2023 11:24:47 +0900 Subject: [PATCH 050/233] fix(autoware_launch): tune avoidance clearance for pedestrian (#477) tune avoidance clearance for pedestrian --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index afb0674029..c3c8bc8ae9 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -78,7 +78,7 @@ max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.5 + safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: enable: false @@ -87,7 +87,7 @@ max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.5 + safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: enable: false @@ -96,7 +96,7 @@ max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.5 + safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] From b59ca0b7d95eefc37a84d906272dbc14c5d9a8bf Mon Sep 17 00:00:00 2001 From: takeshi-iwanari Date: Mon, 30 Oct 2023 14:02:57 +0900 Subject: [PATCH 051/233] feat(system_monitor): increase the number of monitored processes (#534) Signed-off-by: takeshi.iwanari --- .../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 50ef597c8209f737aa257df51a0d12db8a155c9c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 6 Sep 2023 23:37:28 +0900 Subject: [PATCH 052/233] fix(autoware_launch): revert obstacle_avoidance_planner parameter (#404) Signed-off-by: Takayuki Murooka --- .../autoware_path_optimizer/path_optimizer.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index 5b6fb7d174..ea859fcde3 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -51,7 +51,7 @@ # If this parameter is commented out, the parameter is set as below by default. # The logic could be `optimization_center_offset = vehicle_info.wheelbase * 0.8` # The 0.8 scale is adopted as it performed the best. - optimization_center_offset: 2.0 # optimization center offset from base link + optimization_center_offset: 0.0 # optimization center offset from base link clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory # if collision_free_constraints.option.hard_constraint is true From 0b078ef827d4b860b1524933e1e826eb41e43089 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 13 Sep 2023 13:35:30 +0900 Subject: [PATCH 053/233] feat(autoware_launch): not keep stopped for steering convergence (#410) Signed-off-by: Takayuki Murooka --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 060bc6d9ca..f86511be22 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -6,7 +6,7 @@ enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: true - enable_keep_stopped_until_steer_convergence: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 From c8c6508fa4ada895e6ab7cf8602583a32e6be534 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Wed, 13 Sep 2023 16:51:02 +0900 Subject: [PATCH 054/233] chore: cherry pick misc (#423) * chore: remove unknown object form avoidance target (#391) * Adjust parameters according to vehicle size * chore: display virtul wall of obstacle cruise slowdown (#407) display virtul wall of obstacle cruise slowdown * fix: lidar_model_param_path (#388) * fix: lidar_model_param_path * chore: move * chore: delete unused param --------- Co-authored-by: Shohei Sakai --- autoware_launch/launch/autoware.launch.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index efed227402..86725281ec 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -50,9 +50,6 @@ - - - From 251bb53aab7fd0517d832638ac16aeb6b393ddf3 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 13 Sep 2023 18:07:49 +0900 Subject: [PATCH 055/233] hotfix(rviz): visualize mrm summary by default Signed-off-by: kminoda --- autoware_launch/rviz/autoware.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 820d32829c..96b17995c2 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -263,7 +263,7 @@ Visualization Manager: Enabled: true Name: Vehicle - Class: rviz_plugins/MrmSummaryOverlayDisplay - Enabled: false + Enabled: true Font Size: 10 Left: 512 Max Letter Num: 100 From 62c9a7cde7319870160288e3b191cc26c206b2e6 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 14 Sep 2023 15:06:41 +0900 Subject: [PATCH 056/233] fix(planning_launch): fix lane change param Signed-off-by: Fumiya Watanabe --- .../lane_change/lane_change.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 ca7d01e593..daa151be22 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 @@ -8,9 +8,9 @@ backward_length_buffer_for_blocking_object: 0.5 # [m] lane_change_finish_judge_buffer: 0.0 # [m] - lane_changing_lateral_jerk: 0.75 # [m/s3] + lane_changing_lateral_jerk: 0.5 # [m/s3] - minimum_lane_changing_velocity: 1.0 # [m/s] + minimum_lane_changing_velocity: 1.5 # [m/s] prediction_time_resolution: 0.5 # [s] longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 @@ -64,7 +64,7 @@ lateral_acceleration: velocity: [0.0, 4.0, 10.0] min_values: [0.15, 0.15, 0.15] - max_values: [1.25, 1.25, 1.25] + max_values: [1.0, 0.75, 0.5] # target object target_object: @@ -85,7 +85,7 @@ prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] check_objects_on_current_lanes: false check_objects_on_other_lanes: false - use_all_predicted_path: true + use_all_predicted_path: false # lane change regulations regulation: @@ -101,7 +101,7 @@ # lane change cancel cancel: enable_on_prepare_phase: true - enable_on_lane_changing_phase: true + enable_on_lane_changing_phase: false delta_time: 1.0 # [s] duration: 5.0 # [s] max_lateral_jerk: 100.0 # [m/s3] From 1294ebbffbb9ac7296f57143a4661b0096cf6e31 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 15 Sep 2023 13:44:47 +0900 Subject: [PATCH 057/233] feat(autoware_launch): update planning control parameters Signed-off-by: Takayuki Murooka --- .../control/trajectory_follower/lateral/mpc.param.yaml | 2 +- .../rtc_replayer/rtc_replayer.param.yaml | 8 ++++---- 2 files changed, 5 insertions(+), 5 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 ffcfad3e39..9fb71ce014 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -7,7 +7,7 @@ admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing + enable_path_smoothing: true # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml index c0e3e147dc..8f16690498 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml @@ -11,8 +11,8 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" - - "avoidance_by_lane_change_left" - - "avoidance_by_lane_change_right" + # - "avoidance_by_lane_change_left" + # - "avoidance_by_lane_change_right" - "goal_planner" - "start_planner" - "intersection_occlusion" @@ -28,8 +28,8 @@ - "lane_change_right" # - "avoidance_left" # - "avoidance_right" - - "avoidance_by_lane_change_left" - - "avoidance_by_lane_change_right" + # - "avoidance_by_lane_change_left" + # - "avoidance_by_lane_change_right" - "goal_planner" - "start_planner" - "intersection_occlusion" From 48912f5ee8523f67042426cfb403d6e1fe8f4c71 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 15 Sep 2023 13:45:29 +0900 Subject: [PATCH 058/233] fix Signed-off-by: Takayuki Murooka --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9fb71ce014..ffcfad3e39 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -7,7 +7,7 @@ admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: true # flag for path smoothing + enable_path_smoothing: false # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) From 481065b6f085873c9745877d54c88f5556490090 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 21 Sep 2023 14:05:53 +0900 Subject: [PATCH 059/233] tuning for shiojiri (#432) Signed-off-by: Mamoru Sobue --- .../occupancy_grid_map/binary_bayes_filter_updater.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml index 4e335e3574..6237b6fed6 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml @@ -5,4 +5,4 @@ occupied_to_free: 0.05 free_to_occupied: 0.2 free_to_free: 0.8 - v_ratio: 10.0 + v_ratio: 4.5 From 33192058434403e106c8060c4e194c2c1bfcecc2 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 19 Sep 2023 10:21:47 +0900 Subject: [PATCH 060/233] fix: disable unused roi_based_clustering for x2 Signed-off-by: badai-nguyen --- .../launch/components/tier4_perception_component.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 6008aae2a6..c39e69c7e6 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -33,6 +33,8 @@ + + Date: Tue, 12 Sep 2023 22:21:37 +0900 Subject: [PATCH 061/233] feat(autoware_launch): sparse optimization path planning Signed-off-by: Takayuki Murooka --- .../autoware_path_optimizer/path_optimizer.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index ea859fcde3..1f78eecb02 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -44,8 +44,8 @@ enable_optimization_validation: false common: - num_points: 100 # number of points for optimization [-] - delta_arc_length: 0.5 # delta arc length for optimization [m] + num_points: 50 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] kinematics: # If this parameter is commented out, the parameter is set as below by default. From 5bd0aeac2b1bbf88c06939475d8b3e5f80838b48 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 12 Sep 2023 23:15:41 +0900 Subject: [PATCH 062/233] feat(autoware_launch): add stop_on_curve feature in obstacle_cruise_planner Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 9c6e16a87b..5a48e92eab 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -23,9 +23,9 @@ nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] stop_on_curve: - enable_approaching: false - additional_safe_distance_margin: 3.0 # [m] - min_safe_distance_margin: 3.0 # [m] + enable_approaching: true + additional_safe_distance_margin: 2.0 # [m] + min_safe_distance_margin: 3.5 # [m] suppress_sudden_obstacle_stop: true stop_obstacle_type: From 8ee1fc754f61bf7e03b0dd3897fff1ac560fb87c Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 21 Sep 2023 04:16:17 +0000 Subject: [PATCH 063/233] style(pre-commit): autofix --- autoware_launch/config/localization/ekf_localizer.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index f52051d44f..76be7af757 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -42,3 +42,5 @@ # for velocity measurement limitation (Set 0.0 if you want to ignore) threshold_observable_velocity_mps: 0.0 # [m/s] pose_frame_id: "map" + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] From 503a1968092139503f6b89c8f37a089a0d550c9f Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 21 Sep 2023 13:23:21 +0900 Subject: [PATCH 064/233] update default parameter of threshold_observable_velocity_mps in EKF Signed-off-by: kminoda --- autoware_launch/config/localization/ekf_localizer.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 76be7af757..096db82bdd 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -43,4 +43,4 @@ threshold_observable_velocity_mps: 0.0 # [m/s] pose_frame_id: "map" # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + threshold_observable_velocity_mps: 0.5 # [m/s] From d61c05006853d4310824521b8963b65d3ecd1953 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 11:38:42 +0900 Subject: [PATCH 065/233] fix(autoware_launch): stabler drivable area expansion Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/drivable_area_expansion.param.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 3ef779a8a9..2f8676e1bf 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -39,7 +39,6 @@ # 'lanelet': add lanelets overlapped by the ego footprints # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 80.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area @@ -49,3 +48,7 @@ compensate: enable: false # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction extra_distance: 3.0 # [m] extra distance to add to the compensation + replan_checker: + enable: true # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly + # not compatible with dynamic_objects.avoid + max_deviation: 1.0 # [m] full replan is only done if the path changes by more than this distance From 5948b65e227ef5da01405201eb0851e2904ee64b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 26 Sep 2023 00:52:00 +0900 Subject: [PATCH 066/233] feat(autoware_launch): tune merge_from_private Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index e84d4cf152..bc62ea43ff 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -117,6 +117,6 @@ intersection_to_occlusion: false merge_from_private: - stopline_margin: 1.7 + stop_line_margin: 1.0 stop_duration_sec: 1.0 stop_distance_threshold: 1.0 From 4afe4e79ee5c5dca3f8a5c3498a0f18991ef9e42 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 26 Sep 2023 18:32:46 +0900 Subject: [PATCH 067/233] Update autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index bc62ea43ff..b4c364a897 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -117,6 +117,6 @@ intersection_to_occlusion: false merge_from_private: - stop_line_margin: 1.0 + stop_line_margin: 1.2 stop_duration_sec: 1.0 stop_distance_threshold: 1.0 From b6deb789cc34951913f2abcda8f7d89cb535b07c Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 27 Sep 2023 10:22:38 +0900 Subject: [PATCH 068/233] fix: remove additional ground segmentation (#447) Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index c0cad93c6a..d80c7e449c 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - additional_lidars: ["front_upper", "front_lower"] + additional_lidars: [] ransac_input_topics: [] use_single_frame_filter: False use_time_series_filter: True From 148c6640d2a3e5b56bec92e985df45b5f7d5b576 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sun, 1 Oct 2023 11:12:34 +0900 Subject: [PATCH 069/233] feat(lane_change): enable lane change in crosswalk/intersection if ego vehicle gets stuck (#451) Signed-off-by: Fumiya Watanabe --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 5 +++++ 1 file changed, 5 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 daa151be22..15f55caaaf 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 @@ -98,6 +98,11 @@ velocity: 0.5 # [m/s] stop_time: 3.0 # [s] + # ego vehicle stuck detection + stuck_detection: + velocity: 0.1 # [m/s] + stop_time: 3.0 # [s] + # lane change cancel cancel: enable_on_prepare_phase: true From 289361238a6f7d72534629bf6a50b64a270ed228 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 2 Oct 2023 11:48:53 +0900 Subject: [PATCH 070/233] fix(motion_velocity_smoother): change curvature calculation distance parameter (#556) (#463) Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- .../autoware_velocity_smoother/velocity_smoother.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml index ff97ae8dfb..4bae27a353 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml @@ -8,11 +8,11 @@ # -- curve parameters -- # common parameters - curvature_calculation_distance: 2.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] + curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] # lateral acceleration limit parameters enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. - max_lateral_accel: 1.0 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.0 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + max_lateral_accel: 0.6 # max lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate 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 min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] From dfdaa1601efcae3e17c721f0cfcdb5bba02f1837 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 3 Oct 2023 00:17:59 +0900 Subject: [PATCH 071/233] feat(autoware_launch): tune merge_from_private stop distance (#466) Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index b4c364a897..fa1ed0dac0 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -117,6 +117,6 @@ intersection_to_occlusion: false merge_from_private: - stop_line_margin: 1.2 + stop_line_margin: 1.7 stop_duration_sec: 1.0 stop_distance_threshold: 1.0 From ab40779bfd6fa9d0337542a4410d8c9260061598 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 3 Oct 2023 00:22:33 +0900 Subject: [PATCH 072/233] fix(avoidance): do not check distance between object and goal (#468) Signed-off-by: Fumiya Watanabe --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index c3c8bc8ae9..22dd9d1ab0 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -111,7 +111,7 @@ object_ignore_distance_crosswalk_backward: 30.0 # [m] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 10.0 # [m] - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 0.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] From d8768b7d6b19f4668d9a374c4bdd3f4c27c1f45b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 4 Oct 2023 15:17:14 +0900 Subject: [PATCH 073/233] fix(mpc_lateral_controller): enable path smoothing (#472) hotfix(mpc_lateral_controller): enable path smoothing Signed-off-by: kminoda --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ffcfad3e39..9fb71ce014 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -7,7 +7,7 @@ admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing + enable_path_smoothing: true # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) From 6449e99b728037790963d63977b13dec59575fd8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 4 Oct 2023 15:23:56 +0900 Subject: [PATCH 074/233] fix(control_launch): fix steer rate limit at low velocity (#467) * fix(control_launch): fix steer rate limit at low velocity Signed-off-by: Fumiya Watanabe * Update autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Takayuki Murooka --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9fb71ce014..874e875c24 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -49,7 +49,7 @@ vehicle_model_steer_tau: 0.15 # steering dynamics time constant (1d approximation) [s] steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] - steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] + steer_rate_lim_dps_list_by_velocity: [10.0, 10.0, 10.0] # steering angle rate limit list depending on velocity [deg/s] velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0] # velocity list for steering angle rate limit interpolation in ascending order [m/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] From ccc02d3d76681238952d26b79e8686573341fbfd Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 4 Oct 2023 15:30:05 +0900 Subject: [PATCH 075/233] feat(system error monitor): tune warn rate for perception mrm (#453) feat(system_error_monitor): tune warn rate for perception mrm Signed-off-by: kminoda --- .../config/system/component_state_monitor/topics.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index 3845a96108..a9b18001d7 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -59,7 +59,7 @@ topic_type: sensor_msgs/msg/PointCloud2 best_effort: true transient_local: false - warn_rate: 5.0 + warn_rate: 6.5 error_rate: 1.0 timeout: 1.0 From dbcdbafb9b96015a6a19f46d176916b355d361ab Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 4 Oct 2023 16:27:28 +0900 Subject: [PATCH 076/233] feat(autoware_launch): dynamic timeout for no intention to walk decision in crosswalk (#610) (#471) * feat(autoware_launch): dynamic timeout for no intention to walk decision in crosswalk * update config * revert a parg of config --------- Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/crosswalk.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index f7cf23e015..9db75db825 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -57,8 +57,8 @@ disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] - timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + timeout_set_for_no_intention_to_walk: [3.0, 0.0] # [s] + timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: From ccd436c28c04a0c3baad60998936c5c07e7b97a2 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:55:18 +0900 Subject: [PATCH 077/233] feat(lane_change): expand target lanes for object filtering (#601) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 5 +++++ 1 file changed, 5 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 15f55caaaf..3ecf423e4f 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 @@ -60,6 +60,11 @@ left_offset: 1.0 # [m] right_offset: 1.0 # [m] + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] From f48b86e5f77c68543dd4a57888e998b8cd089109 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Wed, 4 Oct 2023 18:03:14 +0900 Subject: [PATCH 078/233] include expansion Signed-off-by: Zulfaqar Azmi --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 4 ++-- 1 file changed, 2 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 3ecf423e4f..45642d61c1 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 @@ -62,8 +62,8 @@ # lane expansion for object filtering lane_expansion: - left_offset: 0.0 # [m] - right_offset: 0.0 # [m] + left_offset: 1.0 # [m] + right_offset: 1.0 # [m] # lateral acceleration map lateral_acceleration: From 090098f63c9a5f364ceaed27da2529b486dafc67 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 19 Sep 2023 12:11:33 +0900 Subject: [PATCH 079/233] fix conflict Signed-off-by: scepter914 --- .../launch/components/tier4_perception_component.launch.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index c39e69c7e6..dfed153531 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -123,6 +123,10 @@ name="object_recognition_detection_object_range_splitter_radar_fusion_param_path" value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar_fusion.param.yaml" /> + Date: Mon, 2 Oct 2023 16:44:20 +0900 Subject: [PATCH 080/233] change perception mode Signed-off-by: scepter914 --- autoware_launch/launch/autoware.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 86725281ec..6146a252db 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -44,7 +44,7 @@ - + From f6b3ac6ab834cf596bf578ed21ca0cdae602c221 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 5 Oct 2023 16:23:17 +0900 Subject: [PATCH 081/233] change default perception mode into camera_lidar_fusion Signed-off-by: scepter914 --- autoware_launch/launch/autoware.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 6146a252db..86725281ec 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -44,7 +44,7 @@ - + From a21a61dac3a02e62bbe5a25f44ac1f36984eedad Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Sep 2023 14:04:52 +0900 Subject: [PATCH 082/233] feat(autoware_launch): add traffic protected level for amber color in intersection (#588) * feat(autoware_launch): add traffic protected level for amber color in intersection Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../intersection.param.yaml | 21 ++++++------------- 1 file changed, 6 insertions(+), 15 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index fa1ed0dac0..42c0155725 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -70,23 +70,14 @@ collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 partially_prioritized: - collision_start_margin_time: 3.0 + collision_start_margin_time: 2.0 collision_end_margin_time: 2.0 not_prioritized: - collision_start_margin_time: 3.0 - collision_end_margin_time: 2.0 - yield_on_green_traffic_light: - distance_to_assigned_lanelet_start: 10.0 - duration: 3.0 - object_dist_to_stopline: 10.0 - ignore_on_amber_traffic_light: - object_expected_deceleration: - car: 2.0 - bike: 5.0 - ignore_on_red_traffic_light: - object_margin_to_path: 2.0 - avoid_collision_by_acceleration: - object_time_margin_to_collision_point: 4.0 + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object + keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module + minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity occlusion: enable: true From b8cca0004df5fa826b8fea610a7e56c0eeffb0d5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Oct 2023 21:17:04 +0900 Subject: [PATCH 083/233] feat(intersection): ignore occlusion beyond high curvature point (#619) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 42c0155725..110158d827 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -97,11 +97,11 @@ occlusion_detection_hold_time: 1.5 temporal_stop_time_before_peeking: 0.1 temporal_stop_before_attention_area: false - creep_velocity_without_traffic_light: 1.388 - static_occlusion_with_traffic_light_timeout: 0.5 - - debug: - ttc: [0] + absence_traffic_light: + creep_velocity: 1.388 # [m/s] + maximum_peeking_distance: 6.0 # [m] + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.5 enable_rtc: intersection: false From aaaff04ae8e170073339995cccf53eb261767d19 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 6 Oct 2023 22:14:40 +0900 Subject: [PATCH 084/233] feat(intersection): yield initially on green light (#623) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 110158d827..dff1d05e81 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -78,6 +78,10 @@ keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 10.0 + duration: 3.0 + range: 50.0 # [m] occlusion: enable: true From 79c7e08d59f71912c5a35850f72bbcc75263374c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 12 Oct 2023 12:17:34 +0900 Subject: [PATCH 085/233] feat: change maximum_peeking_distance from 6.0 to -0.5 in order to revert the behavior Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index dff1d05e81..0067509da6 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -103,7 +103,7 @@ temporal_stop_before_attention_area: false absence_traffic_light: creep_velocity: 1.388 # [m/s] - maximum_peeking_distance: 6.0 # [m] + maximum_peeking_distance: -0.5 # [m] attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 From caa52f224bd752d9b3d56e81aa653a6cd242ea07 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 10 Oct 2023 14:27:56 +0900 Subject: [PATCH 086/233] update parameter Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 5a48e92eab..6004b6999f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -189,7 +189,7 @@ default: moving: min_lat_margin: 0.2 - max_lat_margin: 1.0 + max_lat_margin: 0.7 min_ego_velocity: 2.0 max_ego_velocity: 8.0 static: From 0e7c5f513c18e3c824e5816570597ba2772d2c33 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 12 Oct 2023 12:46:28 +0900 Subject: [PATCH 087/233] feat(autoware_launch): disable smooth_stop in pid_longitudinal_controller (#446) Signed-off-by: Takayuki Murooka --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index f86511be22..3f80578ba1 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -2,7 +2,7 @@ ros__parameters: delay_compensation_time: 0.15 - enable_smooth_stop: true + enable_smooth_stop: false enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: true From b43773aa811735a2060723980d40d0764b1f4ad0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 09:39:09 +0900 Subject: [PATCH 088/233] feat(avoidance): check if the avoidance path is in drivable area (#584) * feat(avoidance): check if the avoidance path is in drivable area Signed-off-by: satoshi-ota * refactor(avoidance): remove unused param Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 22dd9d1ab0..37feb06159 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -139,6 +139,7 @@ hard_road_shoulder_margin: 0.8 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] @@ -175,12 +176,6 @@ min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] max_avoidance_acceleration: 0.5 # [m/ss] - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] - shift_line_pipeline: trim: quantize_filter_threshold: 0.2 From 4b1fe2133ba50591951b8d2da20fe597aab4de3c Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 13 Oct 2023 20:56:17 +0900 Subject: [PATCH 089/233] feat(avoidance): tune avoidance margin (#487) Signed-off-by: kminoda --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 37feb06159..fd6265027c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -32,9 +32,9 @@ moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 0.0 # [m] - safety_buffer_lateral: 0.8 # [m] + envelope_buffer_margin: 0.5 # [m] + avoid_margin_lateral: 0.7 # [m] + safety_buffer_lateral: 0.3 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: enable: true From 285e7581b0824fe994d6fec3487c68888c6471c7 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 12 Oct 2023 16:39:23 +0900 Subject: [PATCH 090/233] no stuck detection when turning left Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 0067509da6..93c9b1ddce 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -24,7 +24,7 @@ bicycle: false unknown: false turn_direction: - left: true + left: false right: true straight: true use_stuck_stopline: true From 352ccdc8db4e36da5bd356d66b15b9747e3d7c13 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 15 Oct 2023 14:36:08 +0900 Subject: [PATCH 091/233] feat(autoware_launch): disable intersection area to expand drivable area (#497) Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index fd6265027c..f2e4bcab91 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -19,7 +19,12 @@ enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false disable_path_update: false - use_hatched_road_markings: false + + # drivable area setting + use_adjacent_lane: true + use_opposite_lane: true + use_intersection_areas: false + use_hatched_road_markings: true # for debug publish_debug_marker: false From 494d7f7f820ecb65f032e975a75a8197e4a2af06 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 15 Oct 2023 13:50:13 +0900 Subject: [PATCH 092/233] feat(intersection): ignore decelerating vehicle on amber traffic light (#635) * feat(intersection): ignore decelerating vehicle on amber traffic light Signed-off-by: Mamoru Sobue * tuning Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 93c9b1ddce..bf41a8e0bf 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -70,8 +70,8 @@ collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 partially_prioritized: - collision_start_margin_time: 2.0 - collision_end_margin_time: 2.0 + collision_start_margin_time: 4.0 + collision_end_margin_time: 6.0 not_prioritized: collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object @@ -81,7 +81,9 @@ yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 duration: 3.0 - range: 50.0 # [m] + object_dist_to_stopline: 10.0 # [m] + ignore_on_amber_traffic_light: + object_expected_deceleration: 2.0 # [m/ss] occlusion: enable: true From 224b368a0f9e2cfb7d17d27dcdbda16628eb9dc1 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 16 Oct 2023 16:41:08 +0900 Subject: [PATCH 093/233] =?UTF-8?q?feat(autoware=5Flaunch):=20longer=20yie?= =?UTF-8?q?ld=20time=20of=20green=20traffic=20light=20in=20in=E2=80=A6=20(?= =?UTF-8?q?#498)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(autoware_launch): longer yield time of green traffic light in intersection Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index bf41a8e0bf..a3cf61cc16 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -80,7 +80,7 @@ minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 - duration: 3.0 + duration: 5.0 object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] From f14f69a96504017fd51b75e9eb3a3f16fb960d21 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Oct 2023 15:46:30 +0900 Subject: [PATCH 094/233] feat(lane_change): add rss paramas for stuck (#493) Signed-off-by: kosuke55 --- .../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 45642d61c1..ff7b7a64fe 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 @@ -39,6 +39,14 @@ longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.5 + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 0.5 + lateral_distance_max_threshold: 0.75 + longitudinal_distance_min_threshold: 2.0 + longitudinal_velocity_delta_time: 0.3 + stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 rear_vehicle_reaction_time: 1.5 From a0a9d1253737205e310d89d95acfba110665fdd7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 17 Oct 2023 15:47:28 +0900 Subject: [PATCH 095/233] fix(lane_change): use low acceleration, check ego lane objects (#495) Signed-off-by: Takamasa Horibe --- .../lane_change/lane_change.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 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 ff7b7a64fe..89a4fcd7eb 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 @@ -24,8 +24,8 @@ length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) # longitudinal acceleration - min_longitudinal_acc: -1.0 - max_longitudinal_acc: 1.0 + min_longitudinal_acc: -0.5 + max_longitudinal_acc: 0.2 # safety check safety_check: @@ -96,7 +96,7 @@ intersection: true turns: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] - check_objects_on_current_lanes: false + check_objects_on_current_lanes: true check_objects_on_other_lanes: false use_all_predicted_path: false From 6e5a5bc3c662eb13fc43200c0bd9ae204886dd6e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Oct 2023 15:48:33 +0900 Subject: [PATCH 096/233] feat(lane_change): ignore pedestrian (#503) Signed-off-by: kosuke55 --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 89a4fcd7eb..d3d7e38058 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 @@ -88,7 +88,7 @@ unknown: false bicycle: true motorcycle: true - pedestrian: true + pedestrian: false # collision check enable_collision_check_for_prepare_phase: From 5bd4c2eac4111fb9bcf60270eee61966a8a10e9a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Oct 2023 15:49:57 +0900 Subject: [PATCH 097/233] feat(lane_change): change stuck velocity to 0.5 (#504) Signed-off-by: kosuke55 --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 d3d7e38058..91b09f633f 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 @@ -113,7 +113,7 @@ # ego vehicle stuck detection stuck_detection: - velocity: 0.1 # [m/s] + velocity: 0.5 # [m/s] stop_time: 3.0 # [s] # lane change cancel From 1fefe6d40807e347b806090c396d784ff033ca0c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 17 Oct 2023 16:13:39 +0900 Subject: [PATCH 098/233] feat: enable smooth stop in pid_longitudinal_controller (#505) Revert "feat(autoware_launch): disable smooth_stop in pid_longitudinal_controller (#446)" This reverts commit f18ef3ae061c7ac1cc45244416a6960ed8015c37. --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 3f80578ba1..f86511be22 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -2,7 +2,7 @@ ros__parameters: delay_compensation_time: 0.15 - enable_smooth_stop: false + enable_smooth_stop: true enable_overshoot_emergency: true enable_large_tracking_error_emergency: true enable_slope_compensation: true From 9c8898c11eb9e0d9919b623ddc22ae0cb31669b9 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Oct 2023 09:44:27 +0900 Subject: [PATCH 099/233] feat(autoware_launch): longer yield time when green in intersection (#508) Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index a3cf61cc16..648324e337 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -80,7 +80,7 @@ minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 - duration: 5.0 + duration: 8.0 object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] From 76eb9f5def03fc780b738e2440038048cbc3b0c3 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Oct 2023 21:03:59 +0900 Subject: [PATCH 100/233] feat(autoware_launch): tune engage_acceleration: 0.1 -> 0.6 (#510) Signed-off-by: Takayuki Murooka --- .../autoware_velocity_smoother/velocity_smoother.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml index 4bae27a353..e08d1d02e1 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml @@ -25,7 +25,7 @@ # 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.5 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_acceleration: 0.6 # 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] From befa8e8f7cce11adc3ec46ea2c780d57f9798e6d Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Sun, 22 Oct 2023 22:28:42 +0900 Subject: [PATCH 101/233] fix(autoware_launch): fix parameter for light weight grand segmentation (#506) * fix:(autoware_launch) fix parameter for light weight grand segmentation Signed-off-by: scepter914 * update ground segmentation param Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../ground_segmentation/ground_segmentation.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index d80c7e449c..53c20b5813 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -7,10 +7,10 @@ common_crop_box_filter: parameters: - min_x: -100.0 - max_x: 150.0 - min_y: -70.0 - max_y: 70.0 + min_x: -70.0 + max_x: 120.0 + min_y: -75.0 + max_y: 75.0 max_z: 3.2 min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False From a1e4ec0a9c140aeb3b68f693b462883f50251e6c Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Sun, 22 Oct 2023 22:30:04 +0900 Subject: [PATCH 102/233] fix: fix stop_duration_sec param for merge_from_private (#516) fix stop_duration_sec param Signed-off-by: scepter914 --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 648324e337..ae1574c970 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -115,5 +115,5 @@ merge_from_private: stop_line_margin: 1.7 - stop_duration_sec: 1.0 + stop_duration_sec: 1.5 stop_distance_threshold: 1.0 From b879a7f63473bdfa4f21a6328dd0e680f0c32a67 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 23 Oct 2023 09:58:12 +0900 Subject: [PATCH 103/233] fix(avoidance): tuning avoidance parameter for objects on narrow lane (#514) Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index f2e4bcab91..783d687711 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -118,7 +118,7 @@ object_check_backward_distance: 10.0 # [m] object_check_goal_distance: 0.0 # [m] # filtering parking objects - threshold_distance_object_is_on_center: 1.0 # [m] + threshold_distance_object_is_on_center: 0.5 # [m] object_check_shiftable_ratio: 0.6 # [-] object_check_min_road_shoulder_width: 0.5 # [m] # lost object compensation From 55c3d7ff63666324078b4f0d24403b8874ea67cb Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 23 Oct 2023 10:27:32 +0900 Subject: [PATCH 104/233] feat(avoidance): update road shoulder margin (#509) feat(avoidance): tune avoidance margin Signed-off-by: kminoda --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 783d687711..5f70142be5 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -140,8 +140,8 @@ lateral_execution_threshold: 0.499 # [m] lateral_small_shift_threshold: 0.101 # [m] lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.8 # [m] - hard_road_shoulder_margin: 0.8 # [m] + soft_road_shoulder_margin: 0.3 # [m] + hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] From 42fa585a4735198c4d637e3a808f22494a41f93e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 20:54:05 +0900 Subject: [PATCH 105/233] feat(autoware_launch): make a margin to stop line in traffic light/stopline (#520) Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/stop_line.param.yaml | 2 +- .../behavior_velocity_planner/traffic_light.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml index 5fa183d73a..0d4e806c2f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: stop_line: - stop_margin: 0.0 + stop_margin: 0.5 stop_duration_sec: 1.0 use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml index 23746a61b6..464e92279a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: traffic_light: - stop_margin: 0.0 + stop_margin: 0.5 tl_state_timeout: 1.0 stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 From a57a6f4438225bdc6d0bd428bd77471d38ed087d Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 25 Oct 2023 21:55:18 +0900 Subject: [PATCH 106/233] feat(avoidance): reduce envelope of pedestrian object for avoidance (#489) reduce envelope of pedestrian object for avoidance --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 5f70142be5..aab54868e1 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -81,7 +81,7 @@ moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 + envelope_buffer_margin: 0.3 avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 @@ -90,7 +90,7 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 + envelope_buffer_margin: 0.3 avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 @@ -99,7 +99,7 @@ moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 + envelope_buffer_margin: 0.3 avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 From 79ad4434d6b2b3ac0ad8e9dda289899ca20e4d80 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 31 Oct 2023 10:39:32 +0900 Subject: [PATCH 107/233] feat(lane_change): tuning params for terminal stop distance (#525) Signed-off-by: kosuke55 --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 4 ++-- 1 file changed, 2 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 91b09f633f..86ce0057ba 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 @@ -8,7 +8,7 @@ backward_length_buffer_for_blocking_object: 0.5 # [m] lane_change_finish_judge_buffer: 0.0 # [m] - lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_jerk: 0.3 # [m/s3] minimum_lane_changing_velocity: 1.5 # [m/s] prediction_time_resolution: 0.5 # [s] @@ -77,7 +77,7 @@ lateral_acceleration: velocity: [0.0, 4.0, 10.0] min_values: [0.15, 0.15, 0.15] - max_values: [1.0, 0.75, 0.5] + max_values: [0.5, 0.5, 0.5] # target object target_object: From 0d4abbfb8e6ea51528204a311f91cfddb0f16598 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 31 Oct 2023 10:40:46 +0900 Subject: [PATCH 108/233] feat(lane_change): increat cancel delta_time to suppress chattering (#529) Signed-off-by: kosuke55 --- .../behavior_path_planner/lane_change/lane_change.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 86ce0057ba..2605bfd563 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 @@ -120,7 +120,7 @@ cancel: enable_on_prepare_phase: true enable_on_lane_changing_phase: false - delta_time: 1.0 # [s] + delta_time: 2.0 # [s] duration: 5.0 # [s] max_lateral_jerk: 100.0 # [m/s3] overhang_tolerance: 0.0 # [m] From 51f84a83a03f5cc65d3766fa9d980fe0d7197cca Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 31 Oct 2023 10:44:46 +0900 Subject: [PATCH 109/233] feat: enable use_intersection_area in avoidance (#524) Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index aab54868e1..8112fe3caa 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -23,7 +23,7 @@ # drivable area setting use_adjacent_lane: true use_opposite_lane: true - use_intersection_areas: false + use_intersection_areas: true use_hatched_road_markings: true # for debug From 695511559e196e4cf25a811bf10a7fff4a1e2e41 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 31 Oct 2023 10:45:37 +0900 Subject: [PATCH 110/233] =?UTF-8?q?feat(behavior=5Fvelocity=5Frun=5Fout):?= =?UTF-8?q?=20ignore=20momentary=20detection=20caused=20by=E2=80=A6=20(#53?= =?UTF-8?q?1)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(behavior_velocity_run_out): ignore momentary detection caused by false positive (#647) * feat(behavior_velocity_run_out): ignore momentary detection caused by false positive * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: Tomohito ANDO Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_velocity_planner/run_out.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index 910ffc6c6c..e5fdabef3a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -77,4 +77,4 @@ # Points in this area are detected even if it is in the no obstacle segmentation area # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area \ No newline at end of file + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area From 2174c0ff0441f46ec8a3f47373de881889d8a975 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 16:07:35 +0900 Subject: [PATCH 111/233] feat: add use_conservative_buffer_longitudinal in avoidance Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 8112fe3caa..340adc5577 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -41,6 +41,7 @@ avoid_margin_lateral: 0.7 # [m] safety_buffer_lateral: 0.3 # [m] safety_buffer_longitudinal: 0.0 # [m] + use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -50,6 +51,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bus: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -59,6 +61,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true trailer: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -68,6 +71,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true unknown: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -76,6 +80,7 @@ envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bicycle: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -85,6 +90,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true motorcycle: enable: false moving_speed_threshold: 1.0 # 3.6km/h @@ -94,6 +100,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true pedestrian: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -103,6 +110,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] From 9cbb9c8b04e7f71701d1939527240a57f90cf924 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 16:24:59 +0900 Subject: [PATCH 112/233] feat: set use_conservative_buffer_longitudinal to false Signed-off-by: Takayuki Murooka --- .../avoidance/avoidance.param.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 340adc5577..02a6fbfb86 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -41,7 +41,7 @@ avoid_margin_lateral: 0.7 # [m] safety_buffer_lateral: 0.3 # [m] safety_buffer_longitudinal: 0.0 # [m] - use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. + use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -51,7 +51,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false bus: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -61,7 +61,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false trailer: enable: true moving_speed_threshold: 1.0 # 3.6km/h @@ -71,7 +71,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false unknown: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -80,7 +80,7 @@ envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false bicycle: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -90,7 +90,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false motorcycle: enable: false moving_speed_threshold: 1.0 # 3.6km/h @@ -100,7 +100,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false pedestrian: enable: false moving_speed_threshold: 0.28 # 1.0km/h @@ -110,7 +110,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: true + use_conservative_buffer_longitudinal: false lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] From e69ebb036e8eba3dac6c95e2bf0f9b994a974395 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 31 Oct 2023 11:22:35 +0900 Subject: [PATCH 113/233] feat: tune pid longitudinal controller (#536) Signed-off-by: Takayuki Murooka --- .../trajectory_follower/longitudinal/pid.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index f86511be22..5d7e200bde 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -39,9 +39,9 @@ brake_keeping_acc: -0.2 # smooth stop state - smooth_stop_max_strong_acc: -0.5 - smooth_stop_min_strong_acc: -0.8 - smooth_stop_weak_acc: -0.3 + smooth_stop_max_strong_acc: -0.8 + smooth_stop_min_strong_acc: -1.3 + smooth_stop_weak_acc: -0.6 smooth_stop_weak_stop_acc: -0.8 smooth_stop_strong_stop_acc: -3.4 smooth_stop_max_fast_vel: 0.5 @@ -67,7 +67,7 @@ # jerk limit max_jerk: 2.0 - min_jerk: -2.0 + min_jerk: -5.0 # slope compensation # pitch From aef1a5fddf4d73e1d7ca7e021a60dfe269b4532b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 31 Oct 2023 11:27:58 +0900 Subject: [PATCH 114/233] fix: tune intersection of decision after stopping (#523) fix: tune intersection Signed-off-by: Takayuki Murooka --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index ae1574c970..e905b89630 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -77,7 +77,7 @@ collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module - minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + minimum_upstream_velocity: 1.38 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 duration: 8.0 From 1631b2f2a581286bad24b6968beb8e79c1082a50 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 31 Oct 2023 15:12:12 +0900 Subject: [PATCH 115/233] feat: change emergency_state_overshoot_stop_dist: 1.5 -> 0.8 (#539) Signed-off-by: Takayuki Murooka --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 5d7e200bde..0e1c2ba075 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -15,7 +15,7 @@ stopped_state_entry_duration_time: 0.1 stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 - emergency_state_overshoot_stop_dist: 1.5 + emergency_state_overshoot_stop_dist: 0.8 emergency_state_traj_trans_dev: 3.0 emergency_state_traj_rot_dev: 0.7854 From 4d8da3fb2b861e237a35760754b2840016d542c1 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 8 Nov 2023 16:55:16 +0900 Subject: [PATCH 116/233] feat(scenario_planning): change common jerk to 0.3 mainly for jerk filtered smoother (#456) * feat(scenario_planning): change common jerk to 0.3 mainly for jerk filtered smoother Signed-off-by: Makoto Kurihara * feat(autoware_launch): change to JerkFiltered smoother Signed-off-by: Makoto Kurihara * chore: increase jerk limit for acceleration --------- Signed-off-by: Makoto Kurihara Co-authored-by: Shinnosuke Hirakawa --- .../planning/scenario_planning/common/common.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 5680a99713..1704ae82de 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,8 +6,8 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + min_jerk: -0.3 # min jerk [m/sss] + max_jerk: 0.6 # max jerk [m/sss] # constraints to be observed limit: From c158d3a90e70a5eb7ab2fe366fe148a68eca5312 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 9 Nov 2023 17:32:57 +0900 Subject: [PATCH 117/233] revert: feat(scenario_planning): change common jerk to 0.3 mainly for jerk filtered smoother (#456) (#542) Revert "feat(scenario_planning): change common jerk to 0.3 mainly for jerk filtered smoother (#456)" This reverts commit 7dbe194fa7b2a77a254a2601840139b93dd6de80. --- .../planning/scenario_planning/common/common.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 1704ae82de..5680a99713 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,8 +6,8 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.3 # min jerk [m/sss] - max_jerk: 0.6 # max jerk [m/sss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # constraints to be observed limit: From 97be5bb0750c431dae95d1705a3cd1f8b273d5f5 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 17 Nov 2023 14:15:46 +0900 Subject: [PATCH 118/233] fix(tier4_perception_component): add detection by tracker option param (#540) Signed-off-by: badai-nguyen Co-authored-by: Tomohito ANDO --- .../launch/components/tier4_perception_component.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index dfed153531..a213c4aa34 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -35,6 +35,7 @@ + Date: Mon, 20 Nov 2023 10:33:37 +0900 Subject: [PATCH 119/233] feat: use radar (#546) use radar Signed-off-by: Takayuki Murooka Co-authored-by: Takayuki Murooka --- autoware_launch/launch/autoware.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 86725281ec..6146a252db 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -44,7 +44,7 @@ - + From 89cc5732a3aa90b331b0924b3d75528dba6d5583 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 22 Nov 2023 17:25:36 +0900 Subject: [PATCH 120/233] chore: tune avoidance lateral margin (#549) Signed-off-by: Tomohito Ando --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 02a6fbfb86..7101df72c2 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -37,9 +37,9 @@ moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.5 # [m] - avoid_margin_lateral: 0.7 # [m] - safety_buffer_lateral: 0.3 # [m] + envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 1.0 # [m] + safety_buffer_lateral: 0.1 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: From 8e455308edbe2188b9248d435f4e6bcbc535764b Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 22 Nov 2023 17:27:33 +0900 Subject: [PATCH 121/233] fix(perception): add detection by tracker param file (#548) * fix(perception): add detection_by_tracker param file (#676) Signed-off-by: badai-nguyen * fix: disable DBT for car Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen Co-authored-by: Tomohito ANDO --- .../launch/components/tier4_perception_component.launch.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index a213c4aa34..9a6a98a78e 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -124,6 +124,10 @@ name="object_recognition_detection_object_range_splitter_radar_fusion_param_path" value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar_fusion.param.yaml" /> + Date: Thu, 23 Nov 2023 21:36:44 +0900 Subject: [PATCH 122/233] chore(obstacle_cruise_planner): change min_behavior_stop_margin to prevent ego from approaching after stopping (#551) chore(obstacle_cruise_planner): change min_behavior_stop_margin to avoid stop wall to approach Signed-off-by: Tomohito Ando --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 6004b6999f..a6378ec757 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -21,7 +21,7 @@ nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index - min_behavior_stop_margin: 3.0 # [m] + min_behavior_stop_margin: 6.0 # [m] stop_on_curve: enable_approaching: true additional_safe_distance_margin: 2.0 # [m] From 9f3b0dcde587687bdc80ea8ff349ef8c3ba1c910 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 24 Nov 2023 17:12:25 +0900 Subject: [PATCH 123/233] chore(mpc): disable steering offset removal (#550) Signed-off-by: Tomohito Ando --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 874e875c24..9a32c7d329 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -69,7 +69,7 @@ # steer offset steering_offset: - enable_auto_steering_offset_removal: true + enable_auto_steering_offset_removal: false update_vel_threshold: 5.56 update_steer_threshold: 0.035 average_num: 1000 From 0b23602b12a636cc1321fe7091d00e2bfc4a299a Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 24 Nov 2023 17:14:07 +0900 Subject: [PATCH 124/233] feat: use optimization velocity smoother (#545) * feat(scenario_planning): change common jerk to 0.3 mainly for jerk filtered smoother Signed-off-by: Makoto Kurihara * feat(autoware_launch): change to JerkFiltered smoother Signed-off-by: Makoto Kurihara * Increase max_jerk: 0.3 -> 0.6 --------- Signed-off-by: Makoto Kurihara Co-authored-by: Makoto Kurihara Co-authored-by: Takayuki Murooka --- .../planning/scenario_planning/common/common.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 5680a99713..1704ae82de 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,8 +6,8 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + min_jerk: -0.3 # min jerk [m/sss] + max_jerk: 0.6 # max jerk [m/sss] # constraints to be observed limit: From e5c18eb75d3f9a6d2c00ffcd1a580f1fe1af247e Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 4 Dec 2023 20:30:06 +0900 Subject: [PATCH 125/233] feat(component_state_monitor): monitor pose_estimator output (#692) (#554) Signed-off-by: kminoda Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../system/component_state_monitor/topics.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index a9b18001d7..9e81adf3e3 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -50,6 +50,19 @@ error_rate: 1.0 timeout: 1.0 +- module: localization + mode: [online, logging_simulation] + type: autonomous + args: + node_name_suffix: pose_estimator_pose + topic: /localization/pose_estimator/pose_with_covariance + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + - module: perception mode: [online, logging_simulation] type: launch From a28af1b01bcba9bdbdfc85afe87e0ac9b7ba99fe Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 8 Dec 2023 09:34:59 +0900 Subject: [PATCH 126/233] feat(intersection): check path margin for overshoot vehicles on redight (#654) (#555) feat(intersection): check path margin for overshoot vehicles on red light (#654) Co-authored-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index e905b89630..480c0f0f8a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -84,6 +84,8 @@ object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: true From 61c2fdc76530e7582d73395080106862b7c648fd Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Fri, 12 Jan 2024 15:41:07 +0900 Subject: [PATCH 127/233] fix: param name for `merge_from_private` --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 480c0f0f8a..e9ace39e26 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -116,6 +116,6 @@ intersection_to_occlusion: false merge_from_private: - stop_line_margin: 1.7 + stopline_margin: 1.7 stop_duration_sec: 1.5 stop_distance_threshold: 1.0 From 51971214d76ea8e8fcf8b48f394ea93b060c70ed Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 19 Jan 2024 16:04:26 +0900 Subject: [PATCH 128/233] feat: x2 planning preset (#562) * add planning preset of x2 * modify rtc setting --- .../config/planning/preset/x2_preset.yaml | 117 ++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 autoware_launch/config/planning/preset/x2_preset.yaml diff --git a/autoware_launch/config/planning/preset/x2_preset.yaml b/autoware_launch/config/planning/preset/x2_preset.yaml new file mode 100644 index 0000000000..f74be4906e --- /dev/null +++ b/autoware_launch/config/planning/preset/x2_preset.yaml @@ -0,0 +1,117 @@ +launch: + # behavior path modules + - arg: + name: launch_avoidance_module + default: "true" + - arg: + name: launch_avoidance_by_lane_change_module + default: "false" + - arg: + name: launch_dynamic_avoidance_module + default: "false" + - arg: + name: launch_lane_change_right_module + default: "true" + - arg: + name: launch_lane_change_left_module + default: "true" + - arg: + name: launch_external_request_lane_change_right_module + default: "false" + - arg: + name: launch_external_request_lane_change_left_module + default: "false" + - arg: + name: launch_goal_planner_module + default: "true" + - arg: + name: launch_start_planner_module + default: "true" + - arg: + name: launch_side_shift_module + default: "false" + + # behavior velocity modules + - arg: + name: launch_crosswalk_module + default: "true" + - arg: + name: launch_walkway_module + default: "true" + - arg: + name: launch_traffic_light_module + default: "true" + - arg: + name: launch_intersection_module + default: "true" + - arg: + name: launch_merge_from_private_module + default: "true" + - arg: + name: launch_blind_spot_module + default: "true" + - arg: + name: launch_detection_area_module + default: "true" + - arg: + name: launch_virtual_traffic_light_module + default: "false" + - arg: + name: launch_no_stopping_area_module + default: "true" + - arg: + name: launch_stop_line_module + default: "true" + - arg: + name: launch_occlusion_spot_module + default: "false" + - arg: + name: launch_run_out_module + default: "true" + - arg: + name: launch_speed_bump_module + default: "false" + - arg: + name: launch_out_of_lane_module + default: "true" + - arg: + name: launch_no_drivable_lane_module + default: "false" + + # motion planning modules + - arg: + name: motion_path_smoother_type + default: elastic_band + # option: elastic_band + # none + + - arg: + name: motion_path_planner_type + default: obstacle_avoidance_planner + # option: obstacle_avoidance_planner + # path_sampler + # none + + - arg: + name: motion_stop_planner_type + default: obstacle_cruise_planner + # option: obstacle_stop_planner + # obstacle_cruise_planner + # none + + - arg: + name: motion_velocity_smoother_type + default: JerkFiltered + # option: JerkFiltered + # L2 + # Linf(Unstable) + # Analytical + + - arg: + name: launch_surround_obstacle_checker + default: "true" + + # parking modules + - arg: + name: launch_parking_module + default: "true" From a2afb785f51f441b250cf8f4729d81e550ad1196 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 31 Jan 2024 13:10:08 +0900 Subject: [PATCH 129/233] feat: use x2 planning module preset (#569) * use x2 planning module preset as default * change planning module preset of simulator launch --- autoware_launch/launch/autoware.launch.xml | 2 +- autoware_launch/launch/e2e_simulator.launch.xml | 2 +- autoware_launch/launch/logging_simulator.launch.xml | 2 +- autoware_launch/launch/planning_simulator.launch.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 6146a252db..851d361f33 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml index 718acc0ce8..d1e7d7a6e8 100644 --- a/autoware_launch/launch/e2e_simulator.launch.xml +++ b/autoware_launch/launch/e2e_simulator.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 93931cb752..fa33147824 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index bb0a9f7711..fb6604600a 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -7,7 +7,7 @@ - + From 79b3d1862b7e5e929955050fdd645823390c1a2f Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 15 Feb 2024 18:03:03 +0900 Subject: [PATCH 130/233] feat: include l4_toolkit (#576) * feat: include l4_toolkit * rename package name * Update autoware_launch/launch/logging_simulator.launch.xml * Update autoware_launch/launch/planning_simulator.launch.xml --- autoware_launch/launch/autoware.launch.xml | 6 ++++++ autoware_launch/launch/logging_simulator.launch.xml | 2 ++ autoware_launch/launch/planning_simulator.launch.xml | 2 ++ 3 files changed, 10 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 851d361f33..3b19a73964 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -23,6 +23,7 @@ + @@ -132,6 +133,11 @@ + + + + + + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index fb6604600a..5c3190760b 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -74,6 +74,8 @@ + + From 15ac6b011b5a418afa98f5565fe4ea99da38d96a Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 16 Feb 2024 10:05:29 +0900 Subject: [PATCH 131/233] =?UTF-8?q?feat(planning):=20add=20enable=5Fall=5F?= =?UTF-8?q?modules=5Fauto=5Fmode=20argument=20to=20launch=20f=E2=80=A6=20(?= =?UTF-8?q?#577)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(planning): add enable_all_modules_auto_mode argument to launch files for planning modules (#798) * Add auto mode setting for all modules Signed-off-by: kyoichi-sugahara Co-authored-by: Kyoichi Sugahara --- autoware_launch/launch/autoware.launch.xml | 2 ++ autoware_launch/launch/planning_simulator.launch.xml | 2 ++ 2 files changed, 4 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 3b19a73964..604889d67c 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -51,6 +51,8 @@ + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 5c3190760b..97ffeb1319 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -74,6 +74,8 @@ + + From d0e336cf12c7c7ff8be8255a9aa7f9dc00c0668d Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 26 Feb 2024 15:42:32 +0900 Subject: [PATCH 132/233] feat(traffic_light_arbiter): add parameter of signal match validator (#579) Signed-off-by: Tomohito Ando --- .../traffic_light_arbiter/traffic_light_arbiter.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml index dfe12ff352..0c23c7b81d 100644 --- a/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml +++ b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml @@ -2,5 +2,5 @@ ros__parameters: external_time_tolerance: 5.0 perception_time_tolerance: 1.0 - external_priority: false - enable_signal_matching: false + external_priority: true + enable_signal_matching: true From 23e0231bad7c665a3d6364091152b186014506b4 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 28 Feb 2024 13:32:38 +0900 Subject: [PATCH 133/233] chore(traffic_light): update timeout threshold (#582) Signed-off-by: Tomohito Ando --- .../behavior_velocity_planner/traffic_light.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml index 464e92279a..0df51663b7 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -2,7 +2,7 @@ ros__parameters: traffic_light: stop_margin: 0.5 - tl_state_timeout: 1.0 + tl_state_timeout: 0.36 stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true From 8d49a337994b38ada44dd311dc39ef9837e0cd65 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 8 Mar 2024 11:44:00 +0900 Subject: [PATCH 134/233] feat: add is_simulation variable in autoware.launch.xml (#889) (#584) Signed-off-by: Takayuki Murooka Signed-off-by: Tomohito Ando Co-authored-by: Takayuki Murooka --- autoware_launch/launch/autoware.launch.xml | 1 + autoware_launch/launch/planning_simulator.launch.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 604889d67c..0b349cfcb7 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -53,6 +53,7 @@ + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 97ffeb1319..051ad53af5 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -78,6 +78,7 @@ + From f00363f012cfcfe8c244d153e281796c25a9d7cf Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 12 Mar 2024 00:13:24 +0900 Subject: [PATCH 135/233] feat: sp tuning for low obstacle (#578) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: tune condition of between cruise and stop (#565) Tune condition of between cruise and stop * fix(traffic_light): stop if the traffic light signal timed out (#727) (#571) Signed-off-by: Fumiya Watanabe Co-authored-by: Fumiya Watanabe * feat(component_state_monitor): monitor traffic light recognition outp… (#572) feat(component_state_monitor): monitor traffic light recognition output (#720) Signed-off-by: Tomohito Ando * update ground segmentation Signed-off-by: yoshiri * update comparemap parameter Signed-off-by: yoshiri * update clustering parameters Signed-off-by: yoshiri * fix tracking iou threshold parameters Signed-off-by: yoshiri * feat: set smaller threshold for euclidean clustering Signed-off-by: yoshiri * chore: fit parameter for v3.0.0 Signed-off-by: yoshiri --------- Signed-off-by: Fumiya Watanabe Signed-off-by: Tomohito Ando Signed-off-by: yoshiri Co-authored-by: Shohei Sakai Co-authored-by: Tomohito ANDO Co-authored-by: Fumiya Watanabe Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> --- ...voxel_grid_based_euclidean_cluster.param.yaml | 2 +- .../pointcloud_map_filter.param.yaml | 4 ++-- .../data_association_matrix.param.yaml | 2 +- .../ground_segmentation.param.yaml | 16 ++++++++-------- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml index 26b027f007..b99612ab8f 100644 --- a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -3,7 +3,7 @@ tolerance: 0.7 voxel_leaf_size: 0.3 min_points_number_per_voxel: 1 - min_cluster_size: 10 + min_cluster_size: 3 max_cluster_size: 3000 use_height: false input_frame: "base_link" diff --git a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml index 62b3074c15..5b9ad199fa 100644 --- a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml @@ -2,13 +2,13 @@ ros__parameters: # voxel size for downsample filter - down_sample_voxel_size: 0.1 + down_sample_voxel_size: 0.05 # distance threshold for compare compare distance_threshold: 0.5 # ratio to reduce voxel_leaf_size and neighbor points distance threshold in z axis - downsize_ratio_z_axis: 0.6 + downsize_ratio_z_axis: 0.3 # publish voxelized map pointcloud for debug publish_debug_pcd: False diff --git a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 13f2220655..9743a9ce59 100644 --- a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -56,7 +56,7 @@ min_iou_matrix: # If value is negative, it will be ignored. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + [0.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 53c20b5813..37edca19e0 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - additional_lidars: [] + additional_lidars: ["front_lower"] ransac_input_topics: [] use_single_frame_filter: False use_time_series_filter: True @@ -20,10 +20,10 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 25.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.2 + split_points_distance_tolerance: 0.15 use_virtual_ground_point: True split_height_distance: 0.2 - non_ground_height_threshold: 0.20 + non_ground_height_threshold: 0.12 grid_size_m: 0.2 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 5 @@ -54,7 +54,7 @@ global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode - split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode + split_height_distance: 0.15 # recommended 0.15 for non elevation_grid_mode use_virtual_ground_point: False non_ground_height_threshold: 0.1 grid_size_m: 0.1 @@ -79,14 +79,14 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.20 # recommended 0.1 for non elevation_grid_mode - split_height_distance: 0.2 # recommended 0.05 for non elevation_grid_mode - use_virtual_ground_point: False + split_points_distance_tolerance: 0.10 # recommended 0.1 for non elevation_grid_mode + split_height_distance: 0.05 # recommended 0.05 for non elevation_grid_mode + use_virtual_ground_point: true non_ground_height_threshold: 0.1 grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 center_pcl_shift: 0.0 - elevation_grid_mode: true + elevation_grid_mode: false use_recheck_ground_cluster: false From 4e48305a270640e2f16bc0fd4f0aa863a5e67a70 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 12 Mar 2024 00:22:32 +0900 Subject: [PATCH 136/233] feat(hazard_lights_selector): add hazard_lights_selector (#581) * hazard_lights_selector Signed-off-by: Mamoru Sobue * style(pre-commit): autofix --------- Signed-off-by: Mamoru Sobue Co-authored-by: soblin --- .../hazard_lights_selector/hazard_lights_selector.param.yaml | 3 +++ .../launch/components/tier4_system_component.launch.xml | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) create mode 100644 autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml diff --git a/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml b/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml new file mode 100644 index 0000000000..68273fd990 --- /dev/null +++ b/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 10 # hazard lights command publish rate [Hz] diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml index 6fda11ac7a..b6dea867fe 100644 --- a/autoware_launch/launch/components/tier4_system_component.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.launch.xml @@ -15,10 +15,10 @@ - - + + From c612bd5a29ed8e19a16f9eda41b7fd6bbf756070 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 12 Mar 2024 13:20:50 +0900 Subject: [PATCH 137/233] feat: tune condition of between cruise and stop (#565) (#590) Tune condition of between cruise and stop Co-authored-by: Shohei Sakai Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> --- .../obstacle_cruise_planner.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index a6378ec757..de3be97df4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -78,8 +78,8 @@ stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle # hysteresis for cruise and stop - obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_cruise_to_stop : 4.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 4.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] # if crossing vehicle is determined as target obstacles or not crossing_obstacle: From bf4bdc5caf92cf5a125bccd1a5ded000997f2f67 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 21 Mar 2024 09:23:32 +0900 Subject: [PATCH 138/233] feat(diagnostic_graph_aggregator): add graph config files (#593) * feat(diagnostic_graph_aggregator): add graph config files Signed-off-by: Tomohito Ando * fix pre-commit error Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- .../autoware.planning_simulator.yaml | 47 +++ .../diagnostic_graph_aggregator/autoware.yaml | 68 ++++ .../diagnostic_graph_aggregator/control.yaml | 96 ++++++ .../diagnostic_graph_aggregator/hardware.yaml | 297 ++++++++++++++++++ .../localization.yaml | 96 ++++++ .../diagnostic_graph_aggregator/map.yaml | 56 ++++ .../diagnostic_graph_aggregator/others.yaml | 118 +++++++ .../perception.yaml | 76 +++++ .../diagnostic_graph_aggregator/planning.yaml | 171 ++++++++++ .../diagnostic_graph_aggregator/sensing.yaml | 263 ++++++++++++++++ .../diagnostic_graph_aggregator/system.yaml | 78 +++++ .../diagnostic_graph_aggregator/vehicle.yaml | 76 +++++ 12 files changed, 1442 insertions(+) create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml create mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml new file mode 100644 index 0000000000..72b4c30aa0 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml @@ -0,0 +1,47 @@ +files: + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/control.yaml } + +# Exclude paths that does not need on planning simulator +edits: + - path: /map/001-topic_status/pointcloud_map-error + type: remove + - path: /map/001-topic_status/pointcloud_map + type: remove + +nodes: + - path: /autoware/modes/local + type: stale + + - path: /autoware/modes/remote + type: stale + + # Stop is always available + - path: /autoware/modes/stop + type: ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, link: /map/autonomous_available } + - { type: link, link: /planning/autonomous_available } + - { type: link, link: /control/autonomous_available } + + - path: /autoware/modes/pull_over + type: and + list: + - { type: link, link: /map/pull_over_available } + - { type: link, link: /planning/pull_over_available } + - { type: link, link: /control/pull_over_available } + + - path: /autoware/modes/comfortable_stop + type: and + list: + - { type: link, link: /map/comfortable_stop_available } + - { type: link, link: /planning/comfortable_stop_available } + - { type: link, link: /control/comfortable_stop_available } + + # Emergency stop is always available + - path: /autoware/modes/emergency_stop + type: ok diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml new file mode 100644 index 0000000000..ee16bc9819 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml @@ -0,0 +1,68 @@ +files: + - { path: $(dirname)/hardware.yaml } + - { path: $(dirname)/sensing.yaml } + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/localization.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/perception.yaml } + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/vehicle.yaml } + - { path: $(dirname)/system.yaml } + - { path: $(dirname)/others.yaml } + +nodes: + - path: /autoware/modes/local + type: stale + + - path: /autoware/modes/remote + type: stale + + # Stop is always available + - path: /autoware/modes/stop + type: ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, link: /hardware/autonomous_available } + - { type: link, link: /sensing/autonomous_available } + - { type: link, link: /map/autonomous_available } + - { type: link, link: /localization/autonomous_available } + - { type: link, link: /planning/autonomous_available } + - { type: link, link: /perception/autonomous_available } + - { type: link, link: /control/autonomous_available } + - { type: link, link: /vehicle/autonomous_available } + - { type: link, link: /system/autonomous_available } + - { type: link, link: /others/autonomous_available } + + - path: /autoware/modes/pull_over + type: and + list: + - { type: link, link: /hardware/pull_over_available } + - { type: link, link: /sensing/pull_over_available } + - { type: link, link: /map/pull_over_available } + - { type: link, link: /localization/pull_over_available } + - { type: link, link: /planning/pull_over_available } + - { type: link, link: /perception/pull_over_available } + - { type: link, link: /control/pull_over_available } + - { type: link, link: /vehicle/pull_over_available } + - { type: link, link: /system/pull_over_available } + - { type: link, link: /others/pull_over_available } + + - path: /autoware/modes/comfortable_stop + type: and + list: + - { type: link, link: /hardware/comfortable_stop_available } + - { type: link, link: /sensing/comfortable_stop_available } + - { type: link, link: /map/comfortable_stop_available } + - { type: link, link: /localization/comfortable_stop_available } + - { type: link, link: /planning/comfortable_stop_available } + - { type: link, link: /perception/comfortable_stop_available } + - { type: link, link: /control/comfortable_stop_available } + - { type: link, link: /vehicle/comfortable_stop_available } + - { type: link, link: /system/comfortable_stop_available } + - { type: link, link: /others/comfortable_stop_available } + + # Emergency stop is always available + - path: /autoware/modes/emergency_stop + type: ok diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml new file mode 100644 index 0000000000..1df804359b --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml @@ -0,0 +1,96 @@ +nodes: + - path: /control/autonomous_available + type: and + list: + - { type: link, link: /control/emergency_stop } + - { type: link, link: /control/pull_over } + - { type: link, link: /control/comfortable_stop } + + - path: /control/pull_over_available + type: and + list: + - { type: link, link: /control/emergency_stop } + - { type: link, link: /control/comfortable_stop } + + - path: /control/comfortable_stop_available + type: and + list: + - { type: link, link: /control/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /control/emergency_stop + type: and + list: + - { type: link, link: /control/001-topic_status/control_command-error } + - { type: link, link: /control/004-lane_departure-error } + - { type: link, link: /control/005-trajectory_deviation-error } + - { type: link, link: /control/010-max_distance_deviation-error } + + - path: /control/comfortable_stop + type: and + + - path: /control/pull_over + type: and + + - path: /control/none + type: and + list: + - { type: link, link: /control/003-gate_heartbeat } + - { type: link, link: /control/007-external_command_converter_heartbeat } + - { type: link, link: /control/008-external_command_selector_heartbeat } + + # Intermediate paths + - path: /control/001-topic_status/control_command-error + type: warn-to-ok + list: + - { type: link, link: /control/001-topic_status/control_command } + + - path: /control/004-lane_departure-error + type: warn-to-ok + list: + - { type: link, link: /control/004-lane_departure } + + - path: /control/005-trajectory_deviation-error + type: warn-to-ok + list: + - { type: link, link: /control/005-trajectory_deviation } + + - path: /control/010-max_distance_deviation-error + type: warn-to-ok + list: + - { type: link, link: /control/010-max_distance_deviation } + + # Diagnostics paths + - path: /control/001-topic_status/control_command + diag: "topic_state_monitor_control_command_control_cmd: control_topic_status" + type: diag + + - path: /control/003-gate_heartbeat + diag: "vehicle_cmd_gate: heartbeat" + type: diag + + - path: /control/004-lane_departure + diag: "lane_departure_checker_node: lane_departure" + type: diag + + - path: /control/005-trajectory_deviation + diag: "lane_departure_checker_node: trajectory_deviation" + type: diag + + - path: /control/007-external_command_converter_heartbeat + diag: "external_cmd_converter: remote_control_topic_status" + type: diag + + - path: /control/008-external_command_selector_heartbeat + diag: "external_cmd_selector: heartbeat" + type: diag + + - path: /control/010-max_distance_deviation + diag: "control_validator: control_validation_max_distance_deviation" + type: diag + + # - path: /control/011-slip_detection + # diag: "slip_detection: " + # type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml new file mode 100644 index 0000000000..c228442c64 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml @@ -0,0 +1,297 @@ +nodes: + - path: /hardware/autonomous_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } + - { type: link, link: /hardware/pull_over } + - { type: link, link: /hardware/comfortable_stop } + + - path: /hardware/pull_over_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } + - { type: link, link: /hardware/comfortable_stop } + + - path: /hardware/comfortable_stop_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /hardware/emergency_stop + type: and + list: + - { type: link, link: /hardware/cpu/002-usage-error } + - { type: link, link: /hardware/cpu/004-throttling-error } + - { type: link, link: /hardware/hdd/001-temperature-error } + - { type: link, link: /hardware/hdd/003-connection-error } + - { type: link, link: /hardware/hdd/006-soft_error-error } + - { type: link, link: /hardware/hdd/007-load-error } + - { type: link, link: /hardware/memory/001-usage-error } + - { type: link, link: /hardware/memory/002-ecc-error } + - { type: link, link: /hardware/network/003-crc-error } + - { type: link, link: /hardware/network/004-packet_reassembles-error } + - { type: link, link: /hardware/gpu/001-temperature-error } + - { type: link, link: /hardware/gpu/002-usage-error } + - { type: link, link: /hardware/gpu/003-memory-error } + - { type: link, link: /hardware/gpu/004-throttling-error } + - { type: link, link: /hardware/gpu/005-frequency-error } + + - path: /hardware/pull_over + type: and + list: + - { type: link, link: /hardware/hdd/002-usage-error } + + - path: /hardware/comfortable_stop + type: and + + - path: /hardware/none + type: and + list: + - { type: link, link: /hardware/cpu/001-temperature } + - { type: link, link: /hardware/cpu/003-load_average } + - { type: link, link: /hardware/cpu/005-frequency } + - { type: link, link: /hardware/hdd/004-total_written } + - { type: link, link: /hardware/hdd/005-warranty_period } + - { type: link, link: /hardware/network/001-usage } + - { type: link, link: /hardware/network/002-traffic } + - { type: link, link: /hardware/ntp/001-sync } + - { type: link, link: /hardware/process/001-summary } + - { type: link, link: /hardware/process/002-load } + - { type: link, link: /hardware/process/003-memory } + - { type: link, link: /hardware/bios/001-battery } + + # Intermediate paths + - path: /hardware/cpu/002-usage-error + type: warn-to-ok + list: + - { type: link, link: /hardware/cpu/002-usage } + + - path: /hardware/cpu/004-throttling-error + type: warn-to-ok + list: + - { type: link, link: /hardware/cpu/004-throttling } + + - path: /hardware/hdd/001-temperature-error + type: warn-to-ok + list: + - { type: link, link: /hardware/hdd/001-temperature } + + - path: /hardware/hdd/002-usage-error + type: warn-to-ok + list: + - { type: link, link: /hardware/hdd/002-usage } + + - path: /hardware/hdd/003-connection-error + type: warn-to-ok + list: + - { type: link, link: /hardware/hdd/003-connection } + + - path: /hardware/hdd/006-soft_error-error + type: warn-to-ok + list: + - { type: link, link: /hardware/hdd/006-soft_error } + + - path: /hardware/hdd/007-load-error + type: warn-to-ok + list: + - { type: link, link: /hardware/hdd/007-load } + + - path: /hardware/memory/001-usage-error + type: warn-to-ok + list: + - { type: link, link: /hardware/memory/001-usage } + + - path: /hardware/memory/002-ecc-error + type: warn-to-ok + list: + - { type: link, link: /hardware/memory/002-ecc } + + - path: /hardware/network/003-crc-error + type: warn-to-ok + list: + - { type: link, link: /hardware/network/003-crc } + + - path: /hardware/gpu/001-temperature-error + type: warn-to-ok + list: + - { type: link, link: /hardware/gpu/001-temperature } + + - path: /hardware/gpu/002-usage-error + type: warn-to-ok + list: + - { type: link, link: /hardware/gpu/002-usage } + + - path: /hardware/gpu/003-memory-error + type: warn-to-ok + list: + - { type: link, link: /hardware/gpu/003-memory } + + - path: /hardware/gpu/004-throttling-error + type: warn-to-ok + list: + - { type: link, link: /hardware/gpu/004-throttling } + + - path: /hardware/gpu/005-frequency-error + type: warn-to-ok + list: + - { type: link, link: /hardware/gpu/005-frequency } + + - path: /hardware/network/004-packet_reassembles-error + type: warn-to-ok + list: + - { type: link, link: /hardware/network/004-packet_reassembles } + + - path: /hardware/hdd/007-load + type: and + list: + - { type: link, link: /hardware/hdd/007-load/read_rate } + - { type: link, link: /hardware/hdd/007-load/read_iops } + - { type: link, link: /hardware/hdd/007-load/write_rate } + - { type: link, link: /hardware/hdd/007-load/write_iops } + + - path: /hardware/process/002-load + type: and + list: + - { type: link, link: /hardware/process/002-load/proc-0 } + - { type: link, link: /hardware/process/002-load/proc-1 } + - { type: link, link: /hardware/process/002-load/proc-2 } + - { type: link, link: /hardware/process/002-load/proc-3 } + - { type: link, link: /hardware/process/002-load/proc-4 } + + - path: /hardware/process/003-memory + type: and + list: + - { type: link, link: /hardware/process/003-memory/proc-0 } + - { type: link, link: /hardware/process/003-memory/proc-1 } + - { type: link, link: /hardware/process/003-memory/proc-2 } + - { type: link, link: /hardware/process/003-memory/proc-3 } + - { type: link, link: /hardware/process/003-memory/proc-4 } + + # Diagnostics paths + - path: /hardware/cpu/001-temperature + diag: "cpu_monitor: CPU Temperature" + type: diag + - path: /hardware/cpu/002-usage + diag: "cpu_monitor: CPU Usage" + type: diag + - path: /hardware/cpu/003-load_average + diag: "cpu_monitor: CPU Load Average" + type: diag + - path: /hardware/cpu/004-throttling + diag: "cpu_monitor: CPU Thermal Throttling" + type: diag + - path: /hardware/cpu/005-frequency + diag: "cpu_monitor: CPU Frequency" + type: diag + + - path: /hardware/hdd/001-temperature + diag: "hdd_monitor: HDD Temperature" + type: diag + - path: /hardware/hdd/002-usage + diag: "hdd_monitor: HDD Usage" + type: diag + - path: /hardware/hdd/003-connection + diag: "hdd_monitor: HDD Connection" + type: diag + - path: /hardware/hdd/004-total_written + diag: "hdd_monitor: HDD TotalDataWritten" + type: diag + - path: /hardware/hdd/005-warranty_period + diag: "hdd_monitor: HDD PowerOnHours" + type: diag + - path: /hardware/hdd/006-soft_error + diag: "hdd_monitor: HDD RecoveredError" + type: diag + - path: /hardware/hdd/007-load/read_rate + diag: "hdd_monitor: HDD ReadDataRate" + type: diag + - path: /hardware/hdd/007-load/write_rate + diag: "hdd_monitor: HDD WriteDataRate" + type: diag + - path: /hardware/hdd/007-load/read_iops + diag: "hdd_monitor: HDD ReadIOPS" + type: diag + - path: /hardware/hdd/007-load/write_iops + diag: "hdd_monitor: HDD WriteIOPS" + type: diag + + - path: /hardware/memory/001-usage + diag: "mem_monitor: Memory Usage" + type: diag + - path: /hardware/memory/002-ecc + diag: "mem_monitor: Memory ECC" + type: diag + + - path: /hardware/network/001-usage + diag: "net_monitor: Network Usage" + type: diag + - path: /hardware/network/002-traffic + diag: "net_monitor: Network Traffic" + type: diag + - path: /hardware/network/003-crc + diag: "net_monitor: Network CRC Error" + type: diag + - path: /hardware/network/004-packet_reassembles + diag: "net_monitor: IP Packet Reassembles Failed" + type: diag + + - path: /hardware/ntp/001-sync + diag: "ntp_monitor: NTP Offset" + type: diag + + - path: /hardware/process/001-summary + diag: "process_monitor: Tasks Summary" + type: diag + - path: /hardware/process/002-load/proc-0 + diag: "process_monitor: High-load Proc[0]" + type: diag + - path: /hardware/process/002-load/proc-1 + diag: "process_monitor: High-load Proc[1]" + type: diag + - path: /hardware/process/002-load/proc-2 + diag: "process_monitor: High-load Proc[2]" + type: diag + - path: /hardware/process/002-load/proc-3 + diag: "process_monitor: High-load Proc[3]" + type: diag + - path: /hardware/process/002-load/proc-4 + diag: "process_monitor: High-load Proc[4]" + type: diag + - path: /hardware/process/003-memory/proc-0 + diag: "process_monitor: High-mem Proc[0]" + type: diag + - path: /hardware/process/003-memory/proc-1 + diag: "process_monitor: High-mem Proc[1]" + type: diag + - path: /hardware/process/003-memory/proc-2 + diag: "process_monitor: High-mem Proc[2]" + type: diag + - path: /hardware/process/003-memory/proc-3 + diag: "process_monitor: High-mem Proc[3]" + type: diag + - path: /hardware/process/003-memory/proc-4 + diag: "process_monitor: High-mem Proc[4]" + type: diag + + - path: /hardware/gpu/001-temperature + diag: "gpu_monitor: GPU Temperature" + type: diag + - path: /hardware/gpu/002-usage + diag: "gpu_monitor: GPU Usage" + type: diag + - path: /hardware/gpu/003-memory + diag: "gpu_monitor: GPU Memory Usage" + type: diag + - path: /hardware/gpu/004-throttling + diag: "gpu_monitor: GPU Thermal Throttling" + type: diag + - path: /hardware/gpu/005-frequency + diag: "gpu_monitor: GPU Frequency" + type: diag + + - path: /hardware/bios/001-battery + diag: "voltage_monitor: CMOS Battery Status" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml new file mode 100644 index 0000000000..a0e263c256 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml @@ -0,0 +1,96 @@ +nodes: + - path: /localization/autonomous_available + type: and + list: + - { type: link, link: /localization/emergency_stop } + - { type: link, link: /localization/pull_over } + - { type: link, link: /localization/comfortable_stop } + + - path: /localization/pull_over_available + type: and + list: + - { type: link, link: /localization/emergency_stop } + - { type: link, link: /localization/comfortable_stop } + + - path: /localization/comfortable_stop_available + type: and + list: + - { type: link, link: /localization/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /localization/emergency_stop + type: and + list: + - { type: link, link: /localization/001-topic_status/initialpose-error } + - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter-error } + - { type: link, link: /localization/002-tf-error } + - { type: link, link: /localization/003-matching_score-error } + - { type: link, link: /localization/004-accuracy-error } + - { type: link, link: /localization/005-accuracy_lateral-error } + + - path: /localization/comfortable_stop + type: and + + - path: /localization/pull_over + type: and + + - path: /localization/none + type: and + + # Intermediate paths + - path: /localization/001-topic_status/initialpose-error + type: warn-to-ok + list: + - { type: link, link: /localization/001-topic_status/initialpose } + + - path: /localization/001-topic_status/pose_twist_fusion_filter-error + type: warn-to-ok + list: + - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter } + + - path: /localization/002-tf-error + type: warn-to-ok + list: + - { type: link, link: /localization/002-tf } + + - path: /localization/003-matching_score-error + type: warn-to-ok + list: + - { type: link, link: /localization/003-matching_score } + + - path: /localization/004-accuracy-error + type: warn-to-ok + list: + - { type: link, link: /localization/004-accuracy } + + - path: /localization/005-accuracy_lateral-error + type: warn-to-ok + list: + - { type: link, link: /localization/005-accuracy_lateral } + + # Diagnostics paths + - path: /localization/001-topic_status/initialpose + diag: "topic_state_monitor_initialpose3d: localization_topic_status" + type: diag + + - path: /localization/001-topic_status/pose_twist_fusion_filter + diag: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" + type: diag + + - path: /localization/002-tf + diag: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" + type: diag + + - path: /localization/003-matching_score + diag: ndt_scan_matcher + type: diag + + - path: /localization/004-accuracy + diag: "localization_error_monitor: localization_accuracy" + type: diag + + - path: /localization/005-accuracy_lateral + diag: "localization_error_monitor: localization_accuracy_lateral_direction" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml new file mode 100644 index 0000000000..ad6479ef76 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml @@ -0,0 +1,56 @@ +nodes: + - path: /map/autonomous_available + type: and + list: + - { type: link, link: /map/emergency_stop } + - { type: link, link: /map/pull_over } + - { type: link, link: /map/comfortable_stop } + + - path: /map/pull_over_available + type: and + list: + - { type: link, link: /map/emergency_stop } + - { type: link, link: /map/comfortable_stop } + + - path: /map/comfortable_stop_available + type: and + list: + - { type: link, link: /map/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /map/emergency_stop + type: and + list: + - { type: link, link: /map/001-topic_status/vector_map-error } + - { type: link, link: /map/001-topic_status/pointcloud_map-error } + + - path: /map/comfortable_stop + type: and + + - path: /map/pull_over + type: and + + - path: /map/none + type: and + + # Intermediate paths + - path: /map/001-topic_status/vector_map-error + type: warn-to-ok + list: + - { type: link, link: /map/001-topic_status/vector_map } + + - path: /map/001-topic_status/pointcloud_map-error + type: warn-to-ok + list: + - { type: link, link: /map/001-topic_status/pointcloud_map } + + # Diagnostics paths + - path: /map/001-topic_status/vector_map + diag: "topic_state_monitor_vector_map: map_topic_status" + type: diag + + - path: /map/001-topic_status/pointcloud_map + diag: "topic_state_monitor_pointcloud_map: map_topic_status" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml new file mode 100644 index 0000000000..8bab9f68b3 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -0,0 +1,118 @@ +nodes: + - path: /others/autonomous_available + type: and + list: + - { type: link, link: /others/emergency_stop } + - { type: link, link: /others/pull_over } + - { type: link, link: /others/comfortable_stop } + + - path: /others/pull_over_available + type: and + list: + - { type: link, link: /others/emergency_stop } + - { type: link, link: /others/comfortable_stop } + + - path: /others/comfortable_stop_available + type: and + list: + - { type: link, link: /others/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /others/emergency_stop + type: and + list: + - { type: link, link: /others/002-blockage_validation-error } + - { type: link, link: /others/004-concat_status } + - { type: link, link: /others/005-visibility_validation-error } + + - path: /others/comfortable_stop + type: and + list: + - { type: link, link: /others/010-emergency_vehicle-error } + + - path: /others/pull_over + type: and + + - path: /others/none + type: and + + # Intermediate paths + - path: /others/002-blockage_validation + type: and + list: + - { type: link, link: /others/002-blockage_validation/front_lower } + - { type: link, link: /others/002-blockage_validation/front_upper } + - { type: link, link: /others/002-blockage_validation/left_lower } + - { type: link, link: /others/002-blockage_validation/left_upper } + - { type: link, link: /others/002-blockage_validation/right_lower } + - { type: link, link: /others/002-blockage_validation/right_upper } + - { type: link, link: /others/002-blockage_validation/rear_lower } + - { type: link, link: /others/002-blockage_validation/rear_upper } + + - path: /others/005-visibility_validation + type: and + list: + - { type: link, link: /others/005-visibility_validation/left_upper } + - { type: link, link: /others/005-visibility_validation/right_upper } + + - path: /others/002-blockage_validation-error + type: warn-to-ok + list: + - { type: link, link: /others/002-blockage_validation } + + - path: /others/005-visibility_validation-error + type: warn-to-ok + list: + - { type: link, link: /others/005-visibility_validation } + + - path: /others/010-emergency_vehicle-error + type: warn-to-ok + list: + - { type: link, link: /others/010-emergency_vehicle } + + # Diagnostics paths + - path: /others/002-blockage_validation/front_lower + diag: "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation" + type: diag + - path: /others/002-blockage_validation/front_upper + diag: "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation" + type: diag + - path: /others/002-blockage_validation/left_lower + diag: "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation" + type: diag + - path: /others/002-blockage_validation/left_upper + diag: "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation" + type: diag + - path: /others/002-blockage_validation/right_lower + diag: "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation" + type: diag + - path: /others/002-blockage_validation/right_upper + diag: "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation" + type: diag + - path: /others/002-blockage_validation/rear_lower + diag: "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation" + type: diag + - path: /others/002-blockage_validation/rear_upper + diag: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" + type: diag + + - path: /others/004-concat_status + diag: "concatenate_data: concat_status" + type: diag + + - path: /others/005-visibility_validation/left_upper + diag: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" + type: diag + - path: /others/005-visibility_validation/right_upper + diag: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" + type: diag + + - path: /others/010-emergency_vehicle + diag: "emergency_vehicle_detector: emergency_vehicle" + type: diag + + - path: /others/011-daytime_monitor + diag: "daytime_monitor: daytime_status" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml new file mode 100644 index 0000000000..e6b9ad5aa3 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml @@ -0,0 +1,76 @@ +nodes: + - path: /perception/autonomous_available + type: and + list: + - { type: link, link: /perception/emergency_stop } + - { type: link, link: /perception/pull_over } + - { type: link, link: /perception/comfortable_stop } + + - path: /perception/pull_over_available + type: and + list: + - { type: link, link: /perception/emergency_stop } + - { type: link, link: /perception/comfortable_stop } + + - path: /perception/comfortable_stop_available + type: and + list: + - { type: link, link: /perception/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /perception/emergency_stop + type: and + list: + - { type: link, link: /perception/001-topic_status/traffic_signals-error } + - { type: link, link: /perception/001-topic_status/objects-error } + - { type: link, link: /perception/001-topic_status/pointcloud-error } + - { type: link, link: /perception/002-detection_delay-error } + + - path: /perception/comfortable_stop + type: and + + - path: /perception/pull_over + type: and + + - path: /perception/none + type: and + + # Intermediate paths + - path: /perception/001-topic_status/traffic_signals-error + type: warn-to-ok + list: + - { type: link, link: /perception/001-topic_status/traffic_signals } + + - path: /perception/001-topic_status/objects-error + type: warn-to-ok + list: + - { type: link, link: /perception/001-topic_status/objects } + + - path: /perception/001-topic_status/pointcloud-error + type: warn-to-ok + list: + - { type: link, link: /perception/001-topic_status/pointcloud } + + - path: /perception/002-detection_delay-error + type: warn-to-ok + list: + - { type: link, link: /perception/002-detection_delay } + + # Diagnostics paths + - path: /perception/001-topic_status/traffic_signals + diag: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" + type: diag + + - path: /perception/001-topic_status/objects + diag: "topic_state_monitor_object_recognition_objects: perception_topic_status" + type: diag + + - path: /perception/001-topic_status/pointcloud + diag: "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status" + type: diag + + - path: /perception/002-detection_delay + diag: "multi_object_tracker: Perception delay check from original header stamp" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml new file mode 100644 index 0000000000..e41e7d5c22 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml @@ -0,0 +1,171 @@ +nodes: + - path: /planning/autonomous_available + type: and + list: + - { type: link, link: /planning/emergency_stop } + - { type: link, link: /planning/pull_over } + - { type: link, link: /planning/comfortable_stop } + + - path: /planning/pull_over_available + type: and + list: + - { type: link, link: /planning/emergency_stop } + - { type: link, link: /planning/comfortable_stop } + + - path: /planning/comfortable_stop_available + type: and + list: + - { type: link, link: /planning/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /planning/emergency_stop + type: and + list: + - { type: link, link: /planning/001-topic_status/route-error } + - { type: link, link: /planning/001-topic_status/trajectory-error } + - { type: link, link: /planning/003-trajectory_finite_validation-error } + - { type: link, link: /planning/004-trajectory_interval_validation-error } + - { type: link, link: /planning/005-trajectory_curvature_validation-error } + - { type: link, link: /planning/006-trajectory_relative_angle_validation-error } + - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation-error } + - { type: link, link: /planning/008-trajectory_acceleration_validation-error } + - { type: link, link: /planning/009-trajectory_deceleration_validation-error } + - { type: link, link: /planning/010-trajectory_steering_validation-error } + - { type: link, link: /planning/011-trajectory_steering_rate_validation-error } + - { type: link, link: /planning/012-trajectory_velocity_deviation_validation-error } + - { type: link, link: /planning/013-collision_checker-error } + + - path: /planning/comfortable_stop + type: and + + - path: /planning/pull_over + type: and + + - path: /planning/none + type: and + + # Intermediate paths + - path: /planning/001-topic_status/route-error + type: warn-to-ok + list: + - { type: link, link: /planning/001-topic_status/route } + + - path: /planning/001-topic_status/trajectory-error + type: warn-to-ok + list: + - { type: link, link: /planning/001-topic_status/trajectory } + + - path: /planning/003-trajectory_finite_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/003-trajectory_finite_validation } + + - path: /planning/004-trajectory_interval_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/004-trajectory_interval_validation } + + - path: /planning/005-trajectory_curvature_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/005-trajectory_curvature_validation } + + - path: /planning/006-trajectory_relative_angle_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/006-trajectory_relative_angle_validation } + + - path: /planning/007-trajectory_lateral_acceleration_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation } + + - path: /planning/008-trajectory_acceleration_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/008-trajectory_acceleration_validation } + + - path: /planning/009-trajectory_deceleration_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/009-trajectory_deceleration_validation } + + - path: /planning/010-trajectory_steering_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/010-trajectory_steering_validation } + + - path: /planning/011-trajectory_steering_rate_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/011-trajectory_steering_rate_validation } + + - path: /planning/012-trajectory_velocity_deviation_validation-error + type: warn-to-ok + list: + - { type: link, link: /planning/012-trajectory_velocity_deviation_validation } + + - path: /planning/013-collision_checker-error + type: warn-to-ok + list: + - { type: link, link: /planning/013-collision_checker } + + # Diagnostics paths + - path: /planning/001-topic_status/route + diag: "topic_state_monitor_mission_planning_route: planning_topic_status" + type: diag + + - path: /planning/001-topic_status/trajectory + diag: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" + type: diag + + # Not implemented + # - path: /planning/002-trajectory_size_validation + # diag: "planning_validator: trajectory_validation_size" + # type: diag + + - path: /planning/003-trajectory_finite_validation + diag: "planning_validator: trajectory_validation_finite" + type: diag + + - path: /planning/004-trajectory_interval_validation + diag: "planning_validator: trajectory_validation_interval" + type: diag + + - path: /planning/005-trajectory_curvature_validation + diag: "planning_validator: trajectory_validation_curvature" + type: diag + + - path: /planning/006-trajectory_relative_angle_validation + diag: "planning_validator: trajectory_validation_relative_angle" + type: diag + + - path: /planning/007-trajectory_lateral_acceleration_validation + diag: "planning_validator: trajectory_validation_lateral_acceleration" + type: diag + + - path: /planning/008-trajectory_acceleration_validation + diag: "planning_validator: trajectory_validation_acceleration" + type: diag + + - path: /planning/009-trajectory_deceleration_validation + diag: "planning_validator: trajectory_validation_deceleration" + type: diag + + - path: /planning/010-trajectory_steering_validation + diag: "planning_validator: trajectory_validation_steering" + type: diag + + - path: /planning/011-trajectory_steering_rate_validation + diag: "planning_validator: trajectory_validation_steering_rate" + type: diag + + - path: /planning/012-trajectory_velocity_deviation_validation + diag: "planning_validator: trajectory_validation_velocity_deviation" + type: diag + + - path: /planning/013-collision_checker + diag: "collision_checker: collision_check" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml new file mode 100644 index 0000000000..e7b47368c4 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml @@ -0,0 +1,263 @@ +nodes: + - path: /sensing/autonomous_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + - { type: link, link: /sensing/pull_over } + - { type: link, link: /sensing/comfortable_stop } + + - path: /sensing/pull_over_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + - { type: link, link: /sensing/comfortable_stop } + + - path: /sensing/comfortable_stop_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /sensing/emergency_stop + type: and + list: + - { type: link, link: /sensing/imu/001-monitor-error } + - { type: link, link: /sensing/imu/002-connection } + - { type: link, link: /sensing/lidar/pndr/001-connection } + - { type: link, link: /sensing/lidar/pndr/002-temperature-error } + - { type: link, link: /sensing/lidar/pndr/003-ptp } + - { type: link, link: /sensing/camera/001-connection } + - { type: link, link: /sensing/radar/001-connection } + + - path: /sensing/pull_over + type: and + list: + - { type: link, link: /sensing/lidar/pndr/002-temperature } + - { type: link, link: /sensing/gnss/001-connection-error } + + - path: /sensing/comfortable_stop + type: and + + - path: /sensing/none + type: and + list: + - { type: link, link: /sensing/imu/003-gyro_bias } + - { type: link, link: /sensing/gnss/002-quality } + + # Intermediate paths + - path: /sensing/lidar/pndr/002-temperature-error + type: warn-to-ok + list: + - { type: link, link: /sensing/lidar/pndr/002-temperature } + + - path: /sensing/imu/001-monitor-error + type: warn-to-ok + list: + - { type: link, link: /sensing/imu/001-monitor } + + - path: /sensing/gnss/001-connection-error + type: warn-to-ok + list: + - { type: link, link: /sensing/gnss/001-connection } + + - path: /sensing/lidar/pndr/001-connection + type: and + list: + - { type: link, link: /sensing/lidar/front_lower/connection } + - { type: link, link: /sensing/lidar/front_upper/connection } + - { type: link, link: /sensing/lidar/left_lower/connection } + - { type: link, link: /sensing/lidar/left_upper/connection } + - { type: link, link: /sensing/lidar/right_lower/connection } + - { type: link, link: /sensing/lidar/right_upper/connection } + - { type: link, link: /sensing/lidar/rear_lower/connection } + - { type: link, link: /sensing/lidar/rear_upper/connection } + + - path: /sensing/lidar/pndr/002-temperature + type: and + list: + - { type: link, link: /sensing/lidar/front_lower/temperature } + - { type: link, link: /sensing/lidar/front_upper/temperature } + - { type: link, link: /sensing/lidar/left_lower/temperature } + - { type: link, link: /sensing/lidar/left_upper/temperature } + - { type: link, link: /sensing/lidar/right_lower/temperature } + - { type: link, link: /sensing/lidar/right_upper/temperature } + - { type: link, link: /sensing/lidar/rear_lower/temperature } + - { type: link, link: /sensing/lidar/rear_upper/temperature } + + - path: /sensing/lidar/pndr/003-ptp + type: and + list: + - { type: link, link: /sensing/lidar/front_lower/ptp } + - { type: link, link: /sensing/lidar/front_upper/ptp } + - { type: link, link: /sensing/lidar/left_lower/ptp } + - { type: link, link: /sensing/lidar/left_upper/ptp } + - { type: link, link: /sensing/lidar/right_lower/ptp } + - { type: link, link: /sensing/lidar/right_upper/ptp } + - { type: link, link: /sensing/lidar/rear_lower/ptp } + - { type: link, link: /sensing/lidar/rear_upper/ptp } + + - path: /sensing/camera/001-connection + type: and + list: + - { type: link, link: /sensing/camera/0/connection } + - { type: link, link: /sensing/camera/1/connection } + - { type: link, link: /sensing/camera/2/connection } + - { type: link, link: /sensing/camera/3/connection } + - { type: link, link: /sensing/camera/4/connection } + - { type: link, link: /sensing/camera/5/connection } + - { type: link, link: /sensing/camera/6/connection } + - { type: link, link: /sensing/camera/7/connection } + + - path: /sensing/radar/001-connection + type: and + list: + - { type: link, link: /sensing/radar/front_center/connection } + - { type: link, link: /sensing/radar/front_left/connection } + - { type: link, link: /sensing/radar/front_right/connection } + - { type: link, link: /sensing/radar/rear_center/connection } + - { type: link, link: /sensing/radar/rear_left/connection } + - { type: link, link: /sensing/radar/rear_right/connection } + + # Diagnostics paths + - path: /sensing/imu/001-monitor + diag: "imu_monitor: yaw_rate_status" + type: diag + - path: /sensing/imu/002-connection + diag: "topic_state_monitor_imu_data: sensing_topic_status" + type: diag + - path: /sensing/imu/003-gyro_bias + diag: "gyro_bias_estimator: gyro_bias_validator" + type: diag + + - path: /sensing/gnss/001-connection + diag: "topic_state_monitor_gnss_pose: sensing_topic_status" + type: diag + - path: /sensing/gnss/002-quality + diag: "septentrio_driver: Quality indicators" + type: diag + + - path: /sensing/lidar/front_lower/connection + diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" + type: diag + - path: /sensing/lidar/front_lower/ptp + diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp" + type: diag + - path: /sensing/lidar/front_lower/temperature + diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" + type: diag + + - path: /sensing/lidar/front_upper/connection + diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_connection" + type: diag + - path: /sensing/lidar/front_upper/ptp + diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp" + type: diag + - path: /sensing/lidar/front_upper/temperature + diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature" + type: diag + + - path: /sensing/lidar/left_lower/connection + diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_connection" + type: diag + - path: /sensing/lidar/left_lower/ptp + diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp" + type: diag + - path: /sensing/lidar/left_lower/temperature + diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature" + type: diag + + - path: /sensing/lidar/left_upper/connection + diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_connection" + type: diag + - path: /sensing/lidar/left_upper/ptp + diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp" + type: diag + - path: /sensing/lidar/left_upper/temperature + diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature" + type: diag + + - path: /sensing/lidar/right_lower/connection + diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_connection" + type: diag + - path: /sensing/lidar/right_lower/ptp + diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp" + type: diag + - path: /sensing/lidar/right_lower/temperature + diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature" + type: diag + + - path: /sensing/lidar/right_upper/connection + diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_connection" + type: diag + - path: /sensing/lidar/right_upper/ptp + diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp" + type: diag + - path: /sensing/lidar/right_upper/temperature + diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature" + type: diag + + - path: /sensing/lidar/rear_lower/connection + diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection" + type: diag + - path: /sensing/lidar/rear_lower/ptp + diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp" + type: diag + - path: /sensing/lidar/rear_lower/temperature + diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature" + type: diag + + - path: /sensing/lidar/rear_upper/connection + diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection" + type: diag + - path: /sensing/lidar/rear_upper/ptp + diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" + type: diag + - path: /sensing/lidar/rear_upper/temperature + diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature" + type: diag + + - path: /sensing/camera/0/connection + diag: "topic_state_monitor_camera0: sensing_topic_status" + type: diag + - path: /sensing/camera/1/connection + diag: "topic_state_monitor_camera1: sensing_topic_status" + type: diag + - path: /sensing/camera/2/connection + diag: "topic_state_monitor_camera2: sensing_topic_status" + type: diag + - path: /sensing/camera/3/connection + diag: "topic_state_monitor_camera3: sensing_topic_status" + type: diag + - path: /sensing/camera/4/connection + diag: "topic_state_monitor_camera4: sensing_topic_status" + type: diag + - path: /sensing/camera/5/connection + diag: "topic_state_monitor_camera5: sensing_topic_status" + type: diag + - path: /sensing/camera/6/connection + diag: "topic_state_monitor_camera6: sensing_topic_status" + type: diag + - path: /sensing/camera/7/connection + diag: "topic_state_monitor_camera7: sensing_topic_status" + type: diag + + - path: /sensing/radar/front_center/connection + diag: "topic_state_monitor_radar_front_center: sensing_topic_status" + type: diag + - path: /sensing/radar/front_left/connection + diag: "topic_state_monitor_radar_front_left: sensing_topic_status" + type: diag + - path: /sensing/radar/front_right/connection + diag: "topic_state_monitor_radar_front_right: sensing_topic_status" + type: diag + - path: /sensing/radar/rear_center/connection + diag: "topic_state_monitor_radar_rear_center: sensing_topic_status" + type: diag + - path: /sensing/radar/rear_left/connection + diag: "topic_state_monitor_radar_rear_left: sensing_topic_status" + type: diag + - path: /sensing/radar/rear_right/connection + diag: "topic_state_monitor_radar_rear_right: sensing_topic_status" + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml new file mode 100644 index 0000000000..f64a1029c7 --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml @@ -0,0 +1,78 @@ +nodes: + - path: /system/autonomous_available + type: and + list: + - { type: link, link: /system/emergency_stop } + - { type: link, link: /system/pull_over } + - { type: link, link: /system/comfortable_stop } + + - path: /system/pull_over_available + type: and + list: + - { type: link, link: /system/emergency_stop } + - { type: link, link: /system/comfortable_stop } + + - path: /system/comfortable_stop_available + type: and + list: + - { type: link, link: /system/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /system/emergency_stop + type: and + list: + - { type: link, link: /system/001-topic_status-error } + - { type: link, link: /system/002-emergency_stop_operation-error } + + - path: /system/comfortable_stop + type: and + + - path: /system/pull_over + type: and + list: + - { type: link, link: /system/005-fms_connection-error } + + - path: /system/none + type: and + list: + - { type: link, link: /system/003-bagpacker_status } + - { type: link, link: /system/004-bagpacker_disk_space } + + # Intermediate paths + - path: /system/001-topic_status-error + type: warn-to-ok + list: + - { type: link, link: /system/001-topic_status } + + - path: /system/002-emergency_stop_operation-error + type: warn-to-ok + list: + - { type: link, link: /system/002-emergency_stop_operation } + + - path: /system/005-fms_connection-error + type: warn-to-ok + list: + - { type: link, link: /system/005-fms_connection } + + # Diagnostics paths + - path: /system/001-topic_status + diag: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" + type: diag + + - path: /system/002-emergency_stop_operation + diag: "vehicle_cmd_gate: emergency_stop_operation" + type: diag + + - path: /system/003-bagpacker_status + diag: bagpacker_state + type: diag + + - path: /system/004-bagpacker_disk_space + diag: bagpacker_state/disk_usage + type: diag + + - path: /system/005-fms_connection + diag: edge_core_internet_connection + type: diag diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml new file mode 100644 index 0000000000..6a31c5510e --- /dev/null +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml @@ -0,0 +1,76 @@ +nodes: + - path: /vehicle/autonomous_available + type: and + list: + - { type: link, link: /vehicle/emergency_stop } + - { type: link, link: /vehicle/pull_over } + - { type: link, link: /vehicle/comfortable_stop } + + - path: /vehicle/pull_over_available + type: and + list: + - { type: link, link: /vehicle/emergency_stop } + - { type: link, link: /vehicle/comfortable_stop } + + - path: /vehicle/comfortable_stop_available + type: and + list: + - { type: link, link: /vehicle/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /vehicle/emergency_stop + type: and + list: + - { type: link, link: /vehicle/001-topic_status/velocity-error } + - { type: link, link: /vehicle/001-topic_status/steering-error } + - { type: link, link: /vehicle/005-vehicle_heartbeat-error } + - { type: link, link: /vehicle/006-vehicle_errors-error } + + - path: /vehicle/comfortable_stop + type: and + + - path: /vehicle/pull_over + type: and + + - path: /vehicle/none + type: and + + # Intermediate paths + - path: /vehicle/001-topic_status/velocity-error + type: warn-to-ok + list: + - { type: link, link: /vehicle/001-topic_status/velocity } + + - path: /vehicle/001-topic_status/steering-error + type: warn-to-ok + list: + - { type: link, link: /vehicle/001-topic_status/steering } + + - path: /vehicle/005-vehicle_heartbeat-error + type: warn-to-ok + list: + - { type: link, link: /vehicle/005-vehicle_heartbeat } + + - path: /vehicle/006-vehicle_errors-error + type: warn-to-ok + list: + - { type: link, link: /vehicle/006-vehicle_errors } + + # Diagnostics paths + - path: /vehicle/001-topic_status/velocity + diag: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" + type: diag + + - path: /vehicle/001-topic_status/steering + diag: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" + type: diag + + - path: /vehicle/005-vehicle_heartbeat + diag: "j6_interface: vehicle_heartbeat_errors" + type: diag + + - path: /vehicle/006-vehicle_errors + diag: "j6_interface: vehicle_errors" + type: diag From 70368b507d27934f8edc21b77225746b010b7c0c Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 22 Mar 2024 17:04:19 +0900 Subject: [PATCH 139/233] chore(diagnostic_graph_aggregator): add daytime monitor setting (#595) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/others.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 8bab9f68b3..bb369aa143 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -34,6 +34,8 @@ nodes: - path: /others/pull_over type: and + list: + - { type: link, link: /others/011-daytime_monitor } - path: /others/none type: and From 63f63312dc25e125fd6bb2cc2c5c31cb961e5c3b Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 22 Mar 2024 17:04:52 +0900 Subject: [PATCH 140/233] chore(diagnostic_graph_aggregator): add slip detection (#596) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/control.yaml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml index 1df804359b..be8934b962 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml @@ -27,6 +27,7 @@ nodes: - { type: link, link: /control/004-lane_departure-error } - { type: link, link: /control/005-trajectory_deviation-error } - { type: link, link: /control/010-max_distance_deviation-error } + - { type: link, link: /control/011-slip_detection } - path: /control/comfortable_stop type: and @@ -91,6 +92,6 @@ nodes: diag: "control_validator: control_validation_max_distance_deviation" type: diag - # - path: /control/011-slip_detection - # diag: "slip_detection: " - # type: diag + - path: /control/011-slip_detection + diag: "slip_detector: slip_status" + type: diag From 9826477e994609f9ee4728a709ce7140b806fa7a Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 25 Mar 2024 13:00:32 +0900 Subject: [PATCH 141/233] fix(perception_launch): remove unnecessary param (#597) Signed-off-by: badai-nguyen Co-authored-by: Shohei Sakai --- .../launch/components/tier4_perception_component.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 9a6a98a78e..5d9ecebe1f 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -30,8 +30,6 @@ - - From 1d5b47c30187a5826813e3ad3dce45455a9bed0d Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 26 Mar 2024 16:54:19 +0900 Subject: [PATCH 142/233] chore(diagnostic_graph_aggregator): update sensing.yaml for topic state monitor (#599) Signed-off-by: Tomohito Ando --- .../diagnostic_graph_aggregator/sensing.yaml | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml index e7b47368c4..ae0eedd419 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml @@ -125,14 +125,14 @@ nodes: diag: "imu_monitor: yaw_rate_status" type: diag - path: /sensing/imu/002-connection - diag: "topic_state_monitor_imu_data: sensing_topic_status" + diag: "topic_state_monitor_imu_data: imu_topic_status" type: diag - path: /sensing/imu/003-gyro_bias diag: "gyro_bias_estimator: gyro_bias_validator" type: diag - path: /sensing/gnss/001-connection - diag: "topic_state_monitor_gnss_pose: sensing_topic_status" + diag: "topic_state_monitor_gnss_pose: gnss_topic_status" type: diag - path: /sensing/gnss/002-quality diag: "septentrio_driver: Quality indicators" @@ -219,45 +219,45 @@ nodes: type: diag - path: /sensing/camera/0/connection - diag: "topic_state_monitor_camera0: sensing_topic_status" + diag: "topic_state_monitor_camera0: camera0_topic_status" type: diag - path: /sensing/camera/1/connection - diag: "topic_state_monitor_camera1: sensing_topic_status" + diag: "topic_state_monitor_camera1: camera1_topic_status" type: diag - path: /sensing/camera/2/connection - diag: "topic_state_monitor_camera2: sensing_topic_status" + diag: "topic_state_monitor_camera2: camera2_topic_status" type: diag - path: /sensing/camera/3/connection - diag: "topic_state_monitor_camera3: sensing_topic_status" + diag: "topic_state_monitor_camera3: camera3_topic_status" type: diag - path: /sensing/camera/4/connection - diag: "topic_state_monitor_camera4: sensing_topic_status" + diag: "topic_state_monitor_camera4: camera4_topic_status" type: diag - path: /sensing/camera/5/connection - diag: "topic_state_monitor_camera5: sensing_topic_status" + diag: "topic_state_monitor_camera5: camera5_topic_status" type: diag - path: /sensing/camera/6/connection - diag: "topic_state_monitor_camera6: sensing_topic_status" + diag: "topic_state_monitor_camera6: camera6_topic_status" type: diag - path: /sensing/camera/7/connection - diag: "topic_state_monitor_camera7: sensing_topic_status" + diag: "topic_state_monitor_camera7: camera7_topic_status" type: diag - path: /sensing/radar/front_center/connection - diag: "topic_state_monitor_radar_front_center: sensing_topic_status" + diag: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" type: diag - path: /sensing/radar/front_left/connection - diag: "topic_state_monitor_radar_front_left: sensing_topic_status" + diag: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" type: diag - path: /sensing/radar/front_right/connection - diag: "topic_state_monitor_radar_front_right: sensing_topic_status" + diag: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" type: diag - path: /sensing/radar/rear_center/connection - diag: "topic_state_monitor_radar_rear_center: sensing_topic_status" + diag: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" type: diag - path: /sensing/radar/rear_left/connection - diag: "topic_state_monitor_radar_rear_left: sensing_topic_status" + diag: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" type: diag - path: /sensing/radar/rear_right/connection - diag: "topic_state_monitor_radar_rear_right: sensing_topic_status" + diag: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" type: diag From 822ab2dc66fd77438663ac0f155b0664c83a87f9 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 27 Mar 2024 14:03:21 +0900 Subject: [PATCH 143/233] feat(diagnostic_graph_aggregator): add timeout (#600) Signed-off-by: Tomohito Ando --- .../diagnostic_graph_aggregator/control.yaml | 8 ++++ .../diagnostic_graph_aggregator/hardware.yaml | 39 +++++++++++++++++ .../localization.yaml | 6 +++ .../diagnostic_graph_aggregator/map.yaml | 2 + .../diagnostic_graph_aggregator/others.yaml | 13 ++++++ .../perception.yaml | 4 ++ .../diagnostic_graph_aggregator/planning.yaml | 13 ++++++ .../diagnostic_graph_aggregator/sensing.yaml | 43 +++++++++++++++++++ .../diagnostic_graph_aggregator/system.yaml | 5 +++ .../diagnostic_graph_aggregator/vehicle.yaml | 4 ++ 10 files changed, 137 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml index be8934b962..2188a5bc09 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml @@ -67,31 +67,39 @@ nodes: - path: /control/001-topic_status/control_command diag: "topic_state_monitor_control_command_control_cmd: control_topic_status" type: diag + timeout: 1.0 - path: /control/003-gate_heartbeat diag: "vehicle_cmd_gate: heartbeat" type: diag + timeout: 1.0 - path: /control/004-lane_departure diag: "lane_departure_checker_node: lane_departure" type: diag + timeout: 1.0 - path: /control/005-trajectory_deviation diag: "lane_departure_checker_node: trajectory_deviation" type: diag + timeout: 1.0 - path: /control/007-external_command_converter_heartbeat diag: "external_cmd_converter: remote_control_topic_status" type: diag + timeout: 1.0 - path: /control/008-external_command_selector_heartbeat diag: "external_cmd_selector: heartbeat" type: diag + timeout: 1.0 - path: /control/010-max_distance_deviation diag: "control_validator: control_validation_max_distance_deviation" type: diag + timeout: 1.0 - path: /control/011-slip_detection diag: "slip_detector: slip_status" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml index c228442c64..8854a4b6bc 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml @@ -174,124 +174,163 @@ nodes: - path: /hardware/cpu/001-temperature diag: "cpu_monitor: CPU Temperature" type: diag + timeout: 3.0 - path: /hardware/cpu/002-usage diag: "cpu_monitor: CPU Usage" type: diag + timeout: 3.0 - path: /hardware/cpu/003-load_average diag: "cpu_monitor: CPU Load Average" type: diag + timeout: 3.0 - path: /hardware/cpu/004-throttling diag: "cpu_monitor: CPU Thermal Throttling" type: diag + timeout: 3.0 - path: /hardware/cpu/005-frequency diag: "cpu_monitor: CPU Frequency" type: diag + timeout: 3.0 - path: /hardware/hdd/001-temperature diag: "hdd_monitor: HDD Temperature" type: diag + timeout: 3.0 - path: /hardware/hdd/002-usage diag: "hdd_monitor: HDD Usage" type: diag + timeout: 3.0 - path: /hardware/hdd/003-connection diag: "hdd_monitor: HDD Connection" type: diag + timeout: 3.0 - path: /hardware/hdd/004-total_written diag: "hdd_monitor: HDD TotalDataWritten" type: diag + timeout: 3.0 - path: /hardware/hdd/005-warranty_period diag: "hdd_monitor: HDD PowerOnHours" type: diag + timeout: 3.0 - path: /hardware/hdd/006-soft_error diag: "hdd_monitor: HDD RecoveredError" type: diag + timeout: 3.0 - path: /hardware/hdd/007-load/read_rate diag: "hdd_monitor: HDD ReadDataRate" type: diag + timeout: 3.0 - path: /hardware/hdd/007-load/write_rate diag: "hdd_monitor: HDD WriteDataRate" type: diag + timeout: 3.0 - path: /hardware/hdd/007-load/read_iops diag: "hdd_monitor: HDD ReadIOPS" type: diag + timeout: 3.0 - path: /hardware/hdd/007-load/write_iops diag: "hdd_monitor: HDD WriteIOPS" type: diag + timeout: 3.0 - path: /hardware/memory/001-usage diag: "mem_monitor: Memory Usage" type: diag + timeout: 3.0 - path: /hardware/memory/002-ecc diag: "mem_monitor: Memory ECC" type: diag + timeout: 3.0 - path: /hardware/network/001-usage diag: "net_monitor: Network Usage" type: diag + timeout: 3.0 - path: /hardware/network/002-traffic diag: "net_monitor: Network Traffic" type: diag + timeout: 3.0 - path: /hardware/network/003-crc diag: "net_monitor: Network CRC Error" type: diag + timeout: 3.0 - path: /hardware/network/004-packet_reassembles diag: "net_monitor: IP Packet Reassembles Failed" type: diag + timeout: 3.0 - path: /hardware/ntp/001-sync diag: "ntp_monitor: NTP Offset" type: diag + timeout: 10.0 - path: /hardware/process/001-summary diag: "process_monitor: Tasks Summary" type: diag + timeout: 3.0 - path: /hardware/process/002-load/proc-0 diag: "process_monitor: High-load Proc[0]" type: diag + timeout: 3.0 - path: /hardware/process/002-load/proc-1 diag: "process_monitor: High-load Proc[1]" type: diag + timeout: 3.0 - path: /hardware/process/002-load/proc-2 diag: "process_monitor: High-load Proc[2]" type: diag + timeout: 3.0 - path: /hardware/process/002-load/proc-3 diag: "process_monitor: High-load Proc[3]" type: diag + timeout: 3.0 - path: /hardware/process/002-load/proc-4 diag: "process_monitor: High-load Proc[4]" type: diag + timeout: 3.0 - path: /hardware/process/003-memory/proc-0 diag: "process_monitor: High-mem Proc[0]" type: diag + timeout: 3.0 - path: /hardware/process/003-memory/proc-1 diag: "process_monitor: High-mem Proc[1]" type: diag + timeout: 3.0 - path: /hardware/process/003-memory/proc-2 diag: "process_monitor: High-mem Proc[2]" type: diag + timeout: 3.0 - path: /hardware/process/003-memory/proc-3 diag: "process_monitor: High-mem Proc[3]" type: diag + timeout: 3.0 - path: /hardware/process/003-memory/proc-4 diag: "process_monitor: High-mem Proc[4]" type: diag + timeout: 3.0 - path: /hardware/gpu/001-temperature diag: "gpu_monitor: GPU Temperature" type: diag + timeout: 3.0 - path: /hardware/gpu/002-usage diag: "gpu_monitor: GPU Usage" type: diag + timeout: 3.0 - path: /hardware/gpu/003-memory diag: "gpu_monitor: GPU Memory Usage" type: diag + timeout: 3.0 - path: /hardware/gpu/004-throttling diag: "gpu_monitor: GPU Thermal Throttling" type: diag + timeout: 3.0 - path: /hardware/gpu/005-frequency diag: "gpu_monitor: GPU Frequency" type: diag + timeout: 3.0 - path: /hardware/bios/001-battery diag: "voltage_monitor: CMOS Battery Status" type: diag + timeout: 3.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml index a0e263c256..f8b6db43a6 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml @@ -74,23 +74,29 @@ nodes: - path: /localization/001-topic_status/initialpose diag: "topic_state_monitor_initialpose3d: localization_topic_status" type: diag + timeout: 1.0 - path: /localization/001-topic_status/pose_twist_fusion_filter diag: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" type: diag + timeout: 1.0 - path: /localization/002-tf diag: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" type: diag + timeout: 1.0 - path: /localization/003-matching_score diag: ndt_scan_matcher type: diag + timeout: 1.0 - path: /localization/004-accuracy diag: "localization_error_monitor: localization_accuracy" type: diag + timeout: 1.0 - path: /localization/005-accuracy_lateral diag: "localization_error_monitor: localization_accuracy_lateral_direction" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml index ad6479ef76..4c2aa69c0e 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml @@ -50,7 +50,9 @@ nodes: - path: /map/001-topic_status/vector_map diag: "topic_state_monitor_vector_map: map_topic_status" type: diag + timeout: 1.0 - path: /map/001-topic_status/pointcloud_map diag: "topic_state_monitor_pointcloud_map: map_topic_status" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index bb369aa143..552bde17e9 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -78,43 +78,56 @@ nodes: - path: /others/002-blockage_validation/front_lower diag: "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/front_upper diag: "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/left_lower diag: "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/left_upper diag: "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/right_lower diag: "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/right_upper diag: "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/rear_lower diag: "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation" type: diag + timeout: 1.0 - path: /others/002-blockage_validation/rear_upper diag: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" type: diag + timeout: 1.0 - path: /others/004-concat_status diag: "concatenate_data: concat_status" type: diag + timeout: 1.0 - path: /others/005-visibility_validation/left_upper diag: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" type: diag + timeout: 1.0 - path: /others/005-visibility_validation/right_upper diag: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" type: diag + timeout: 1.0 - path: /others/010-emergency_vehicle diag: "emergency_vehicle_detector: emergency_vehicle" type: diag + timeout: 1.0 - path: /others/011-daytime_monitor diag: "daytime_monitor: daytime_status" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml index e6b9ad5aa3..425cb97e17 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml @@ -62,15 +62,19 @@ nodes: - path: /perception/001-topic_status/traffic_signals diag: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" type: diag + timeout: 1.0 - path: /perception/001-topic_status/objects diag: "topic_state_monitor_object_recognition_objects: perception_topic_status" type: diag + timeout: 1.0 - path: /perception/001-topic_status/pointcloud diag: "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status" type: diag + timeout: 1.0 - path: /perception/002-detection_delay diag: "multi_object_tracker: Perception delay check from original header stamp" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml index e41e7d5c22..16aab65635 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml @@ -116,10 +116,12 @@ nodes: - path: /planning/001-topic_status/route diag: "topic_state_monitor_mission_planning_route: planning_topic_status" type: diag + timeout: 1.0 - path: /planning/001-topic_status/trajectory diag: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" type: diag + timeout: 1.0 # Not implemented # - path: /planning/002-trajectory_size_validation @@ -129,43 +131,54 @@ nodes: - path: /planning/003-trajectory_finite_validation diag: "planning_validator: trajectory_validation_finite" type: diag + timeout: 1.0 - path: /planning/004-trajectory_interval_validation diag: "planning_validator: trajectory_validation_interval" type: diag + timeout: 1.0 - path: /planning/005-trajectory_curvature_validation diag: "planning_validator: trajectory_validation_curvature" type: diag + timeout: 1.0 - path: /planning/006-trajectory_relative_angle_validation diag: "planning_validator: trajectory_validation_relative_angle" type: diag + timeout: 1.0 - path: /planning/007-trajectory_lateral_acceleration_validation diag: "planning_validator: trajectory_validation_lateral_acceleration" type: diag + timeout: 1.0 - path: /planning/008-trajectory_acceleration_validation diag: "planning_validator: trajectory_validation_acceleration" type: diag + timeout: 1.0 - path: /planning/009-trajectory_deceleration_validation diag: "planning_validator: trajectory_validation_deceleration" type: diag + timeout: 1.0 - path: /planning/010-trajectory_steering_validation diag: "planning_validator: trajectory_validation_steering" type: diag + timeout: 1.0 - path: /planning/011-trajectory_steering_rate_validation diag: "planning_validator: trajectory_validation_steering_rate" type: diag + timeout: 1.0 - path: /planning/012-trajectory_velocity_deviation_validation diag: "planning_validator: trajectory_validation_velocity_deviation" type: diag + timeout: 1.0 - path: /planning/013-collision_checker diag: "collision_checker: collision_check" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml index ae0eedd419..08caf57125 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml @@ -124,140 +124,183 @@ nodes: - path: /sensing/imu/001-monitor diag: "imu_monitor: yaw_rate_status" type: diag + timeout: 5.0 - path: /sensing/imu/002-connection diag: "topic_state_monitor_imu_data: imu_topic_status" type: diag + timeout: 1.0 - path: /sensing/imu/003-gyro_bias diag: "gyro_bias_estimator: gyro_bias_validator" type: diag + timeout: 1.0 - path: /sensing/gnss/001-connection diag: "topic_state_monitor_gnss_pose: gnss_topic_status" type: diag + timeout: 5.0 - path: /sensing/gnss/002-quality diag: "septentrio_driver: Quality indicators" type: diag + timeout: 5.0 - path: /sensing/lidar/front_lower/connection diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/front_lower/ptp diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/front_lower/temperature diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/front_upper/connection diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/front_upper/ptp diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/front_upper/temperature diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/left_lower/connection diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/left_lower/ptp diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/left_lower/temperature diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/left_upper/connection diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/left_upper/ptp diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/left_upper/temperature diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/right_lower/connection diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/right_lower/ptp diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/right_lower/temperature diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/right_upper/connection diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/right_upper/ptp diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/right_upper/temperature diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_lower/connection diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_lower/ptp diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_lower/temperature diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_upper/connection diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_upper/ptp diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" type: diag + timeout: 5.0 - path: /sensing/lidar/rear_upper/temperature diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature" type: diag + timeout: 5.0 - path: /sensing/camera/0/connection diag: "topic_state_monitor_camera0: camera0_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/1/connection diag: "topic_state_monitor_camera1: camera1_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/2/connection diag: "topic_state_monitor_camera2: camera2_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/3/connection diag: "topic_state_monitor_camera3: camera3_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/4/connection diag: "topic_state_monitor_camera4: camera4_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/5/connection diag: "topic_state_monitor_camera5: camera5_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/6/connection diag: "topic_state_monitor_camera6: camera6_topic_status" type: diag + timeout: 1.0 - path: /sensing/camera/7/connection diag: "topic_state_monitor_camera7: camera7_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/front_center/connection diag: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/front_left/connection diag: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/front_right/connection diag: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/rear_center/connection diag: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/rear_left/connection diag: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" type: diag + timeout: 1.0 - path: /sensing/radar/rear_right/connection diag: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" type: diag + timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml index f64a1029c7..23c5a6467e 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml @@ -60,19 +60,24 @@ nodes: - path: /system/001-topic_status diag: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" type: diag + timeout: 1.0 - path: /system/002-emergency_stop_operation diag: "vehicle_cmd_gate: emergency_stop_operation" type: diag + timeout: 1.0 - path: /system/003-bagpacker_status diag: bagpacker_state type: diag + timeout: 3.0 - path: /system/004-bagpacker_disk_space diag: bagpacker_state/disk_usage type: diag + timeout: 3.0 - path: /system/005-fms_connection diag: edge_core_internet_connection type: diag + timeout: 10.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml index 6a31c5510e..4f3190d572 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml @@ -62,15 +62,19 @@ nodes: - path: /vehicle/001-topic_status/velocity diag: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" type: diag + timeout: 1.0 - path: /vehicle/001-topic_status/steering diag: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" type: diag + timeout: 1.0 - path: /vehicle/005-vehicle_heartbeat diag: "j6_interface: vehicle_heartbeat_errors" type: diag + timeout: 1.0 - path: /vehicle/006-vehicle_errors diag: "j6_interface: vehicle_errors" type: diag + timeout: 1.0 From 1b87dc63d6cfb00a2685206300cb31225df9a339 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 2 Apr 2024 09:40:11 +0900 Subject: [PATCH 144/233] feat: launch diagnostic graph converter tool (#606) * feat: launch diagnostic graph converter tool Signed-off-by: Tomohito Ando * fix comments Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- autoware_launch/launch/autoware.launch.xml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 0b349cfcb7..b96bfdcbc0 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -152,5 +152,12 @@ if="$(var rviz)" respawn="$(var rviz_respawn)" /> + + + + + + + From f61aebe194cee28e5635382a2fe642f40d9ff03e Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 2 Apr 2024 11:00:48 +0900 Subject: [PATCH 145/233] feat(mrm_handler): enable comfortable stop Signed-off-by: Tomohito Ando --- .../config/system/mrm_handler/mrm_handler.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml index e82ee36a78..2370259cbe 100644 --- a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml +++ b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml @@ -10,7 +10,7 @@ timeout_emergency_recovery: 5.0 use_parking_after_stopped: false use_pull_over: false - use_comfortable_stop: false + use_comfortable_stop: true # setting whether to turn hazard lamp on for each situation turning_hazard_on: From 6bd885913e22ef0a0a0acda65b4c8b6607db65cf Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 3 Apr 2024 15:35:10 +0900 Subject: [PATCH 146/233] chore(diagnostic_graph_aggregator): add autonomous emergency braking (#609) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/control.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml index 2188a5bc09..19d5671924 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml @@ -26,6 +26,7 @@ nodes: - { type: link, link: /control/001-topic_status/control_command-error } - { type: link, link: /control/004-lane_departure-error } - { type: link, link: /control/005-trajectory_deviation-error } + - { type: link, link: /control/009-aeb_emergency_stop } - { type: link, link: /control/010-max_distance_deviation-error } - { type: link, link: /control/011-slip_detection } @@ -94,6 +95,11 @@ nodes: type: diag timeout: 1.0 + - path: /control/009-aeb_emergency_stop + diag: "autonomous_emergency_braking: aeb_emergency_stop" + type: diag + timeout: 1.0 + - path: /control/010-max_distance_deviation diag: "control_validator: control_validation_max_distance_deviation" type: diag From bdc41be9e25d4201f9419063c6d9d517c2c65396 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 9 Apr 2024 16:51:13 +0900 Subject: [PATCH 147/233] chore(diagnotisc_graph_aggregator): update config (#615) * fix(diagnostic_graph_aggregator): localization accuracy Signed-off-by: Tomohito Ando * fix: visibility validation Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- .../diagnostic_graph_aggregator/localization.yaml | 13 +------------ .../system/diagnostic_graph_aggregator/others.yaml | 11 +++-------- 2 files changed, 4 insertions(+), 20 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml index f8b6db43a6..fd3c75c85f 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml @@ -28,7 +28,6 @@ nodes: - { type: link, link: /localization/002-tf-error } - { type: link, link: /localization/003-matching_score-error } - { type: link, link: /localization/004-accuracy-error } - - { type: link, link: /localization/005-accuracy_lateral-error } - path: /localization/comfortable_stop type: and @@ -65,11 +64,6 @@ nodes: list: - { type: link, link: /localization/004-accuracy } - - path: /localization/005-accuracy_lateral-error - type: warn-to-ok - list: - - { type: link, link: /localization/005-accuracy_lateral } - # Diagnostics paths - path: /localization/001-topic_status/initialpose diag: "topic_state_monitor_initialpose3d: localization_topic_status" @@ -92,11 +86,6 @@ nodes: timeout: 1.0 - path: /localization/004-accuracy - diag: "localization_error_monitor: localization_accuracy" - type: diag - timeout: 1.0 - - - path: /localization/005-accuracy_lateral - diag: "localization_error_monitor: localization_accuracy_lateral_direction" + diag: "localization: localization_error_monitor" type: diag timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 552bde17e9..33568d5c7a 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -56,8 +56,7 @@ nodes: - path: /others/005-visibility_validation type: and list: - - { type: link, link: /others/005-visibility_validation/left_upper } - - { type: link, link: /others/005-visibility_validation/right_upper } + - { type: link, link: /others/005-visibility_validation/front_lower } - path: /others/002-blockage_validation-error type: warn-to-ok @@ -113,12 +112,8 @@ nodes: type: diag timeout: 1.0 - - path: /others/005-visibility_validation/left_upper - diag: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" - type: diag - timeout: 1.0 - - path: /others/005-visibility_validation/right_upper - diag: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" + - path: /others/005-visibility_validation/front_lower + diag: "dual_return_filter: /sensing/lidar/front_lower: visibility_validation" type: diag timeout: 1.0 From 27ef7cad9c90d80eb6c6bf7bb621ddfac0165ec9 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 28 Feb 2024 08:56:12 +0900 Subject: [PATCH 148/233] feat(avoidance): wait next shift approval until the ego reaches shift length threshold (#891) * feat(avoidance): wait next shift approval until the ego reaches shift length threshold Signed-off-by: satoshi-ota * fix(avoidance): param description Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 7101df72c2..8df32d7e5d 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -153,6 +153,10 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 0.0. + ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] From fca26e739b1c00d39ae95c104a0f382109113955 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 7 Mar 2024 10:17:50 +0900 Subject: [PATCH 149/233] feat(avoidance): change lateral margin based on if it's parked vehicle (#894) * feat(avoidance): change lateral margin based on if it's parked vehicle Signed-off-by: satoshi-ota * fix(AbLC): update values Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 44 ++++++++++++++----- 1 file changed, 32 insertions(+), 12 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 8df32d7e5d..ae53de2d8b 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -36,16 +36,22 @@ enable: true # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] + lateral_margin: + soft_margin: 1.0 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.1 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: enable: true moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 1.0 # [m] + hard_margin: 0.4 # [m] + hard_margin_for_parked_vehicle: 0.4 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 avoid_margin_lateral: 0.0 @@ -56,26 +62,34 @@ enable: true moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.8 # [m] + hard_margin_for_parked_vehicle: 0.8 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: false trailer: enable: true moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.8 # [m] + hard_margin_for_parked_vehicle: 0.8 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.8 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: false unknown: enable: false moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 1.0 # [m] + hard_margin: 0.7 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 safety_buffer_lateral: 0.7 @@ -85,30 +99,36 @@ enable: false moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: false motorcycle: enable: false moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: false pedestrian: enable: false moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: false lower_distance_for_polygon_expansion: 30.0 # [m] From 3c5e0fb7b6db417ac933e4d8825521f33ffb8d80 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Feb 2024 21:32:27 +0900 Subject: [PATCH 150/233] feat: define common max_vel (#870) Signed-off-by: Takayuki Murooka --- .../config/planning/scenario_planning/common/common.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 1704ae82de..9f1c261562 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - max_vel: 11.1 # max velocity limit [m/s] + max_vel: 9.72 # max velocity limit [m/s] # constraints param for normal driving normal: From fcc6f9787954fb7fb8aa32914727d4634faad0f1 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 9 Apr 2024 15:34:17 +0900 Subject: [PATCH 151/233] fix(avoidance): tuning avoidance parameters for J6 Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index ae53de2d8b..b131e60dd0 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -37,11 +37,11 @@ moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] lateral_margin: - soft_margin: 1.0 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.2 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] + envelope_buffer_margin: 0.1 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: @@ -49,9 +49,9 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 lateral_margin: - soft_margin: 1.0 # [m] - hard_margin: 0.4 # [m] - hard_margin_for_parked_vehicle: 0.4 # [m] + soft_margin: 0.2 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 avoid_margin_lateral: 0.0 @@ -63,11 +63,11 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 0.8 # [m] - hard_margin_for_parked_vehicle: 0.8 # [m] + soft_margin: 0.2 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: false trailer: @@ -75,11 +75,11 @@ moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 0.8 # [m] - hard_margin_for_parked_vehicle: 0.8 # [m] + soft_margin: 0.2 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: false unknown: From cee3388a940f63e27220560a1a140e68ad6bf6aa Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 10 Apr 2024 09:48:10 +0900 Subject: [PATCH 152/233] fix(obstacle_cruise_planner): decrease slow down speed Signed-off-by: satoshi-ota --- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index de3be97df4..bbc303c002 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -191,7 +191,7 @@ min_lat_margin: 0.2 max_lat_margin: 0.7 min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + max_ego_velocity: 5.56 static: min_lat_margin: 0.2 max_lat_margin: 1.0 From da996176af7343e264fc1a58a1de02f3a46b3be7 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 16 Apr 2024 15:20:25 +0900 Subject: [PATCH 153/233] =?UTF-8?q?chore(diagnostic=5Fgraph=5Faggregator):?= =?UTF-8?q?=20enable=20emergency=20stop=20diag=20on=20pla=E2=80=A6=20(#630?= =?UTF-8?q?)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit chore(diagnostic_graph_aggregator): enable emergency stop diag on planning simulator Signed-off-by: Tomohito Ando --- .../autoware.planning_simulator.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml index 72b4c30aa0..c212f19ede 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml @@ -2,6 +2,7 @@ files: - { path: $(dirname)/map.yaml } - { path: $(dirname)/planning.yaml } - { path: $(dirname)/control.yaml } + - { path: $(dirname)/system.yaml } # Exclude paths that does not need on planning simulator edits: @@ -9,6 +10,14 @@ edits: type: remove - path: /map/001-topic_status/pointcloud_map type: remove + - path: /system/003-bagpacker_status + type: remove + - path: /system/004-bagpacker_disk_space + type: remove + - path: /system/005-fms_connection + type: remove + - path: /system/005-fms_connection-error + type: remove nodes: - path: /autoware/modes/local @@ -27,6 +36,7 @@ nodes: - { type: link, link: /map/autonomous_available } - { type: link, link: /planning/autonomous_available } - { type: link, link: /control/autonomous_available } + - { type: link, link: /system/autonomous_available } - path: /autoware/modes/pull_over type: and @@ -34,6 +44,7 @@ nodes: - { type: link, link: /map/pull_over_available } - { type: link, link: /planning/pull_over_available } - { type: link, link: /control/pull_over_available } + - { type: link, link: /system/pull_over_available } - path: /autoware/modes/comfortable_stop type: and @@ -41,6 +52,7 @@ nodes: - { type: link, link: /map/comfortable_stop_available } - { type: link, link: /planning/comfortable_stop_available } - { type: link, link: /control/comfortable_stop_available } + - { type: link, link: /system/comfortable_stop_available } # Emergency stop is always available - path: /autoware/modes/emergency_stop From ca373eda541cdf72b1d33b6a304274b9a0244683 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 16 Apr 2024 17:10:28 +0900 Subject: [PATCH 154/233] chore(diagnostic_graph_aggregator): add heartbeat of MOT, signage and vehicle voice to config (#629) * chore(diagnostic_graph_aggregator): add heartbeat of MOT and vehicle voice system to config Signed-off-by: Tomohito Ando * add signage Signed-off-by: Tomohito Ando * update timeout Signed-off-by: Tomohito Ando --------- Signed-off-by: Tomohito Ando --- .../diagnostic_graph_aggregator/others.yaml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 33568d5c7a..ec8aa22d67 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -36,6 +36,9 @@ nodes: type: and list: - { type: link, link: /others/011-daytime_monitor } + - { type: link, link: /others/012-mot_hearbeat } + - { type: link, link: /others/013-signage_heartbeat } + - { type: link, link: /others/014-vehicle_voice_heartbeat } - path: /others/none type: and @@ -126,3 +129,19 @@ nodes: diag: "daytime_monitor: daytime_status" type: diag timeout: 1.0 + + # We assume these diagnostics become only OK or STALE + - path: /others/012-mot_hearbeat + diag: "mot: /system/mot_connection : mot heartbeat" + type: diag + timeout: 5.0 + + - path: /others/013-signage_heartbeat + diag: "signage: /system/signage_connection : signage heartbeat" + type: diag + timeout: 5.0 + + - path: /others/014-vehicle_voice_heartbeat + diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voirce alert system heartbeat" + type: diag + timeout: 5.0 From d219141bd0fa0dcf7559ecaf713696485b1a8ffe Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Wed, 17 Apr 2024 15:58:59 +0900 Subject: [PATCH 155/233] feat(fault injection): update params (#627) * feat(fault_injection): update params to adapt diagnostic_graph_aggregator Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * update event name Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --- .../simulator/fault_injection.param.yaml | 236 +++++++++++++----- 1 file changed, 176 insertions(+), 60 deletions(-) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index 899307ebb1..9fcdd18134 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -1,63 +1,179 @@ /**: ros__parameters: event_diag_list: - vehicle_is_out_of_lane: "lane_departure" - trajectory_deviation_is_high: "trajectory_deviation" - localization_matching_score_is_low: "scan_matching_status" - localization_accuracy_is_low: "localization_error_ellipse" - map_version_is_different: "map_version" - trajectory_is_invalid: "trajectory_point_validation" - cpu_temperature_is_high: "CPU Temperature" - cpu_usage_is_high: "CPU Usage" - cpu_is_in_thermal_throttling: "CPU Thermal Throttling" - storage_temperature_is_high: "HDD Temperature" - storage_usage_is_high: "HDD Usage" - network_usage_is_high: "Network Usage" - clock_error_is_large: "NTP Offset" - gpu_temperature_is_high: "GPU Temperature" - gpu_usage_is_high: "GPU Usage" - gpu_memory_usage_is_high: "GPU Memory Usage" - gpu_is_in_thermal_throttling: "GPU Thermal Throttling" - driving_recorder_storage_error: "driving_recorder" - debug_data_logger_storage_error: "bagpacker" - emergency_stop_operation: "emergency_stop_operation" - vehicle_error_occurred: "vehicle_errors" - vehicle_ecu_connection_is_lost: "can_bus_connection" - obstacle_crash_sensor_is_activated: "obstacle_crash" - /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" - /control/autonomous_driving/node_alive_monitoring: "control_topic_status" - /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" - /localization/node_alive_monitoring: "localization_topic_status" - /map/node_alive_monitoring: "map_topic_status" - /planning/node_alive_monitoring: "planning_topic_status" - /sensing/lidar/node_alive_monitoring: "lidar_topic_status" - /sensing/imu/node_alive_monitoring: "imu_connection" - /sensing/gnss/node_alive_monitoring: "gnss_connection" - /system/node_alive_monitoring: "system_topic_status" - /vehicle/node_alive_monitoring: "vehicle_topic_status" - cpu_frequency: "CPU Frequency" - cpu_load_average: "CPU Load Average" - gpu_frequency: "GPU Frequency" - lidar_blockage_validation: "blockage_validation" - lidar_visibility: "left_upper: visibility_validation" - memory_usage: "Memory Usage" - network_crc_error: "Network CRC Error" - network_ip_packet_reassembles_failed: "IP Packet Reassembles Failed" - network_traffic: "Network Traffic" - perception_topic_status: "perception_topic_status" - process_high_load: "High-load" - process_high_mem: "High-mem" - process_tasks_summary: "Tasks Summary" - remote_control_topic_status: "remote_control_topic_status" - sensing_topic_status: "sensing_topic_status" - storage_connection: "HDD Connection" - storage_power_on_hours: "HDD PowerOnHours" - storage_read_data_rate: "HDD ReadDataRate" - storage_read_iops: "HDD ReadIOPS" - storage_recovered_error: "HDD RecoveredError" - storage_total_data_written: "HDD TotalDataWritten" - storage_write_data_rate: "HDD WriteDataRate" - storage_write_iops: "HDD WriteIOPS" - trajectory_curvature_validation: "trajectory_curvature_validation" - trajectory_interval_validation : "trajectory_interval_validation" - trajectory_relative_angle_validation: "trajectory_relative_angle_validation" + # Hardware + # CPU + CPU-001: "cpu_monitor: CPU Temperature" + CPU-002: "cpu_monitor: CPU Usage" + CPU-003: "cpu_monitor: CPU Load Average" + CPU-004: "cpu_monitor: CPU Thermal Throttling" + CPU-005: "cpu_monitor: CPU Frequency" + + # HDD + HDD-001: "hdd_monitor: HDD Temperature" + HDD-002: "hdd_monitor: HDD Usage" + HDD-003: "hdd_monitor: HDD Connection" + HDD-004: "hdd_monitor: HDD TotalDataWritten" + HDD-005: "hdd_monitor: HDD PowerOnHours" + HDD-006: "hdd_monitor: HDD RecoveredError" + HDD-007-1: "hdd_monitor: HDD ReadDataRate" + HDD-007-2: "hdd_monitor: HDD WriteDataRate" + HDD-007-3: "hdd_monitor: HDD ReadIOPS" + HDD-007-4: "hdd_monitor: HDD WriteIOPS" + + # Memory + MEM-001: "mem_monitor: Memory Usage" + MEM-002: "mem_monitor: Memory ECC" + + # Network + NET-001: "net_monitor: Network Usage" + NET-002: "net_monitor: Network Traffic" + NET-003: "net_monitor: Network CRC Error" + NET-004: "net_monitor: IP Packet Reassembles Failed" + + # NTP + NTP-001: "ntp_monitor: NTP Offset" + + # Process + PRO-001: "process_monitor: Tasks Summary" + PRO-002-0: "process_monitor: High-load Proc[0]" + PRO-002-1: "process_monitor: High-load Proc[1]" + PRO-002-2: "process_monitor: High-load Proc[2]" + PRO-002-3: "process_monitor: High-load Proc[3]" + PRO-002-4: "process_monitor: High-load Proc[4]" + PRO-003-0: "process_monitor: High-mem Proc[0]" + PRO-003-1: "process_monitor: High-mem Proc[1]" + PRO-003-2: "process_monitor: High-mem Proc[2]" + PRO-003-3: "process_monitor: High-mem Proc[3]" + PRO-003-4: "process_monitor: High-mem Proc[4]" + + # GPU + GPU-001: "gpu_monitor: GPU Temperature" + GPU-002: "gpu_monitor: GPU Usage" + GPU-003: "gpu_monitor: GPU Memory Usage" + GPU-004: "gpu_monitor: GPU Thermal Throttling" + GPU-005: "gpu_monitor: GPU Frequency" + + # BIOS + BIOS-001: "voltage_monitor: CMOS Battery Status" + + # IMU + IMU-001: "imu_monitor: yaw_rate_status" + IMU-003: "gyro_bias_estimator: gyro_bias_validator" + + # LiDAR + PNDR-001/front_lower: "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" + PNDR-001/front_upper: "pandar_monitor: /sensing/lidar/front_upper: pandar_connection" + PNDR-001/left_lower: "pandar_monitor: /sensing/lidar/left_lower: pandar_connection" + PNDR-001/left_upper: "pandar_monitor: /sensing/lidar/left_upper: pandar_connection" + PNDR-001/right_lower: "pandar_monitor: /sensing/lidar/right_lower: pandar_connection" + PNDR-001/right_upper: "pandar_monitor: /sensing/lidar/right_upper: pandar_connection" + PNDR-001/rear_lower: "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection" + PNDR-001/rear_upper: "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection" + PNDR-002/front_lower: "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" + PNDR-002/front_upper: "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature" + PNDR-002/left_lower: "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature" + PNDR-002/left_upper: "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature" + PNDR-002/right_lower: "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature" + PNDR-002/right_upper: "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature" + PNDR-002/rear_lower: "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature" + PNDR-002/rear_upper: "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature" + PNDR-003/front_lower: "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp" + PNDR-003/front_upper: "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp" + PNDR-003/left_lower: "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp" + PNDR-003/left_upper: "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp" + PNDR-003/right_lower: "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp" + PNDR-003/right_upper: "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp" + PNDR-003/rear_lower: "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp" + PNDR-003/rear_upper: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" + + # Software + # Map + MAP-001-1: "topic_state_monitor_vector_map: map_topic_status" + MAP-001-2: "topic_state_monitor_pointcloud_map: map_topic_status" + + # Localization + LOCALIZATION-001-1: "topic_state_monitor_initialpose3d: localization_topic_status" + LOCALIZATION-001-2: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" + LOCALIZATION-001-3: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" + LOCALIZATION-004: "localization_error_monitor: localization_accuracy" + LOCALIZATION-005: "localization_error_monitor: localization_accuracy_lateral_direction" + + # Perception + PERCEPTION-001-1: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" + PERCEPTION-001-2: "topic_state_monitor_object_recognition_objects: perception_topic_status" + PERCEPTION-001-3: "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status" + + # Planning + PLANNING-001-1: "topic_state_monitor_mission_planning_route: planning_topic_status" + PLANNING-001-2: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" + PLANNING-003: "planning_validator: trajectory_validation_finite" + PLANNING-004: "planning_validator: trajectory_validation_interval" + PLANNING-005: "planning_validator: trajectory_validation_curvature" + PLANNING-006: "planning_validator: trajectory_validation_relative_angle" + PLANNING-007: "planning_validator: trajectory_validation_lateral_acceleration" + PLANNING-008: "planning_validator: trajectory_validation_acceleration" + PLANNING-009: "planning_validator: trajectory_validation_deceleration" + PLANNING-010: "planning_validator: trajectory_validation_steering" + PLANNING-011: "planning_validator: trajectory_validation_steering_rate" + PLANNING-012: "planning_validator: trajectory_validation_velocity_deviation" + PLANNING-013: "collision_checker: collision_check" + + # Control + CONTROL-001: "topic_state_monitor_control_command_control_cmd: control_topic_status" + CONTROL-002: "external_cmd_converter: remote_control_topic_status" + CONTROL-003: "vehicle_cmd_gate: heartbeat" + CONTROL-004: "lane_departure_checker_node: lane_departure" + CONTROL-005: "lane_departure_checker_node: trajectory_deviation" + CONTROL-008: "external_cmd_selector: heartbeat" + CONTROL-009: "autonomous_emergency_braking: aeb_emergency_stop" + CONTROL-010: "control_validator: control_validation_max_distance_deviation" + + # Vehicle + VEHICLE-001-1: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" + VEHICLE-001-2: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" + VEHICLE-005: "j6_interface: vehicle_heartbeat_errors" + VEHICLE-006: "j6_interface: vehicle_errors" + + # System + SYSTEM-001: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" + SYSTEM-002: "vehicle_cmd_gate: emergency_stop_operation" + SYSTEM-003: "bagpacker_state" + SYSTEM-005: "edge_core_internet_connection" + + # Others + OTHERS-002/front_lower: "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation" + OTHERS-002/front_upper: "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation" + OTHERS-002/left_lower: "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation" + OTHERS-002/left_upper: "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation" + OTHERS-002/right_lower: "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation" + OTHERS-002/right_upper: "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation" + OTHERS-002/rear_lower: "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation" + OTHERS-002/rear_upper: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" + OTHERS-004: "concatenate_data: concat_status" + OTHERS-005/left_upper: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" + OTHERS-005/right_upper: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" + OTHERS-010: "emergency_vehicle_detector: emergency_vehicle" + + # Not in System_Hazard_and_Fail_Safe + debug_data_logger_disk_usage_error: "bagpacker_state/disk_usage" + slip_status: "slip_detector: slip_status" + daytime_status: "daytime_monitor: daytime_status" + perception_delay_check: "multi_object_tracker: Perception delay check from original header stamp" + imu_topic_status: "topic_state_monitor_imu_data: imu_topic_status" + gnss_topic_status: "topic_state_monitor_gnss_pose: gnss_topic_status" + septentrio_status: "septentrio_driver: Quality indicators" + /sensing/camera/0/connection: "topic_state_monitor_camera0: camera0_topic_status" + /sensing/camera/1/connection: "topic_state_monitor_camera0: camera1_topic_status" + /sensing/camera/2/connection: "topic_state_monitor_camera0: camera2_topic_status" + /sensing/camera/3/connection: "topic_state_monitor_camera0: camera3_topic_status" + /sensing/camera/4/connection: "topic_state_monitor_camera0: camera4_topic_status" + /sensing/camera/5/connection: "topic_state_monitor_camera0: camera5_topic_status" + /sensing/camera/6/connection: "topic_state_monitor_camera0: camera6_topic_status" + /sensing/camera/7/connection: "topic_state_monitor_camera0: camera7_topic_status" + /sensing/radar/front_center/connection: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" + /sensing/radar/front_left/connection: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" + /sensing/radar/front_right/connection: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" + /sensing/radar/rear_center/connection: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" + /sensing/radar/rear_left/connection: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" + /sensing/radar/rear_right/connection: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" From d9f71f5f2052a7d6f837f15c01866915b6245c24 Mon Sep 17 00:00:00 2001 From: ito-san Date: Wed, 17 Apr 2024 11:28:59 +0900 Subject: [PATCH 156/233] fix(diagnostic_graph_aggregator): fix bagpacker status stale Signed-off-by: ito-san --- .../config/system/diagnostic_graph_aggregator/system.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml index 23c5a6467e..f1405b1f36 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml @@ -68,12 +68,12 @@ nodes: timeout: 1.0 - path: /system/003-bagpacker_status - diag: bagpacker_state + diag: rosbag_status type: diag timeout: 3.0 - path: /system/004-bagpacker_disk_space - diag: bagpacker_state/disk_usage + diag: disk_status type: diag timeout: 3.0 From 110ac584f791b9a9767065e6a28ec3fcf97c7199 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 18 Apr 2024 09:09:32 +0900 Subject: [PATCH 157/233] =?UTF-8?q?chore(diagnostic=5Fgraph=5Faggregator):?= =?UTF-8?q?=20move=20MOT,=20signage=20and=20voice=20to=20ha=E2=80=A6=20(#6?= =?UTF-8?q?33)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit chore(diagnostic_graph_aggregator): move MOT, signage and voice to hardware Signed-off-by: Tomohito Ando --- .../diagnostic_graph_aggregator/hardware.yaml | 19 +++++++++++++++++++ .../diagnostic_graph_aggregator/others.yaml | 19 ------------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml index 8854a4b6bc..6ca18ea993 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml @@ -43,6 +43,9 @@ nodes: type: and list: - { type: link, link: /hardware/hdd/002-usage-error } + - { type: link, link: /hardware/mot/001-connection } + - { type: link, link: /hardware/signage/001-connection } + - { type: link, link: /hardware/voice/001-connection } - path: /hardware/comfortable_stop type: and @@ -334,3 +337,19 @@ nodes: diag: "voltage_monitor: CMOS Battery Status" type: diag timeout: 3.0 + + # We assume these diagnostics become only OK or STALE + - path: /hardware/mot/001-connection + diag: "mot: /system/mot_connection : mot heartbeat" + type: diag + timeout: 5.0 + + - path: /hardware/signage/001-connection + diag: "signage: /system/signage_connection : signage heartbeat" + type: diag + timeout: 5.0 + + - path: /hardware/voice/001-connection + diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voirce alert system heartbeat" + type: diag + timeout: 5.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index ec8aa22d67..33568d5c7a 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -36,9 +36,6 @@ nodes: type: and list: - { type: link, link: /others/011-daytime_monitor } - - { type: link, link: /others/012-mot_hearbeat } - - { type: link, link: /others/013-signage_heartbeat } - - { type: link, link: /others/014-vehicle_voice_heartbeat } - path: /others/none type: and @@ -129,19 +126,3 @@ nodes: diag: "daytime_monitor: daytime_status" type: diag timeout: 1.0 - - # We assume these diagnostics become only OK or STALE - - path: /others/012-mot_hearbeat - diag: "mot: /system/mot_connection : mot heartbeat" - type: diag - timeout: 5.0 - - - path: /others/013-signage_heartbeat - diag: "signage: /system/signage_connection : signage heartbeat" - type: diag - timeout: 5.0 - - - path: /others/014-vehicle_voice_heartbeat - diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voirce alert system heartbeat" - type: diag - timeout: 5.0 From 233d93a968fe7d9dd263ef704b62654f68404514 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 31 Jan 2024 19:28:58 +0900 Subject: [PATCH 158/233] chore(ground_segmentation): add default params (#831) Signed-off-by: badai-nguyen Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../ground_segmentation.param.yaml | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 37edca19e0..72f497b93a 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -29,7 +29,10 @@ gnd_grid_buffer_size: 5 detection_range_z_max: 3.2 elevation_grid_mode: true + use_recheck_ground_cluster: true + low_priority_region_x: -20.0 center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 # common_ground_filter: # plugin: "ground_segmentation::RayGroundFilterComponent" @@ -61,8 +64,11 @@ grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 - center_pcl_shift: 0.0 elevation_grid_mode: true + use_recheck_ground_cluster: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 front_lower_crop_box_filter: parameters: @@ -87,6 +93,8 @@ grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 - center_pcl_shift: 0.0 elevation_grid_mode: false use_recheck_ground_cluster: false + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 From f95ea80dd1b8a70a17e1062a0d286337a3ddccf6 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 8 Apr 2024 18:41:50 +0900 Subject: [PATCH 159/233] chore(ground_segmentation): add tuning param (#946) Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 72f497b93a..5be908a5c3 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -30,6 +30,7 @@ detection_range_z_max: 3.2 elevation_grid_mode: true use_recheck_ground_cluster: true + use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 From d421e59964108396bd94c7a90ca86ba9ca9170d5 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 10 Apr 2024 10:08:44 +0900 Subject: [PATCH 160/233] fix: add use_lowest_point for other addtional lidar Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 5be908a5c3..18e0cb7df5 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -67,6 +67,7 @@ detection_range_z_max: 3.2 elevation_grid_mode: true use_recheck_ground_cluster: true + use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 @@ -96,6 +97,7 @@ detection_range_z_max: 3.2 elevation_grid_mode: false use_recheck_ground_cluster: false + use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 radial_divider_angle_deg: 1.0 From c23ea59f3dd107397e0a9fe6d20e86d45ea77055 Mon Sep 17 00:00:00 2001 From: TakahiroNISHIOKA <75649342+TakahiroNISHIOKA@users.noreply.github.com> Date: Fri, 19 Apr 2024 14:58:56 +0900 Subject: [PATCH 161/233] chore: update fault injection params (#634) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * chore: update fault injection params * Update fault_injection.param.yaml diag側にtypoがあるため、そちらに合わせてtypo * Update fault_injection.param.yaml 元にしているdiagのtypoが修正されていたので、こちらも修正 --- .../simulator/fault_injection.param.yaml | 68 +++++++++++-------- 1 file changed, 41 insertions(+), 27 deletions(-) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index 9fcdd18134..39e9a64a4c 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -57,10 +57,25 @@ # BIOS BIOS-001: "voltage_monitor: CMOS Battery Status" + # MOT + MOT-001: "mot: /system/mot_connection : mot heartbeat" + + # SIGNAGE + SIGNAGE-001: "signage: /system/signage_connection : signage heartbeat" + + # VOICE + VOICE-001: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voice alert system heartbeat" + + # Sensing # IMU IMU-001: "imu_monitor: yaw_rate_status" + IMU-002: "topic_state_monitor_imu_data: imu_topic_status" IMU-003: "gyro_bias_estimator: gyro_bias_validator" + # GNS + GNS-001: "topic_state_monitor_gnss_pose: gnss_topic_status" + GNS-002: "septentrio_driver: Quality indicators" + # LiDAR PNDR-001/front_lower: "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" PNDR-001/front_upper: "pandar_monitor: /sensing/lidar/front_upper: pandar_connection" @@ -87,6 +102,24 @@ PNDR-003/rear_lower: "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp" PNDR-003/rear_upper: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" + # CAMERA + LPD-001/camera0: "topic_state_monitor_camera0: camera0_topic_status" + LPD-001/camera1: "topic_state_monitor_camera1: camera1_topic_status" + LPD-001/camera2: "topic_state_monitor_camera2: camera2_topic_status" + LPD-001/camera3: "topic_state_monitor_camera3: camera3_topic_status" + LPD-001/camera4: "topic_state_monitor_camera4: camera4_topic_status" + LPD-001/camera5: "topic_state_monitor_camera5: camera5_topic_status" + LPD-001/camera6: "topic_state_monitor_camera6: camera6_topic_status" + LPD-001/camera7: "topic_state_monitor_camera7: camera7_topic_status" + + # RADAR + SENSING-001/front_center: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" + SENSING-001/front_left: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" + SENSING-001/front_right: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" + SENSING-001/rear_center: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" + SENSING-001/rear_left: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" + SENSING-001/rear_right: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" + # Software # Map MAP-001-1: "topic_state_monitor_vector_map: map_topic_status" @@ -95,14 +128,15 @@ # Localization LOCALIZATION-001-1: "topic_state_monitor_initialpose3d: localization_topic_status" LOCALIZATION-001-2: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" - LOCALIZATION-001-3: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" + LOCALIZATION-002: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" + LOCALIZATION-003: "ndt_scan_matcher" LOCALIZATION-004: "localization_error_monitor: localization_accuracy" - LOCALIZATION-005: "localization_error_monitor: localization_accuracy_lateral_direction" # Perception PERCEPTION-001-1: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" PERCEPTION-001-2: "topic_state_monitor_object_recognition_objects: perception_topic_status" PERCEPTION-001-3: "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status" + PERCEPTION-002: "multi_object_tracker: Perception delay check from original header stamp" # Planning PLANNING-001-1: "topic_state_monitor_mission_planning_route: planning_topic_status" @@ -121,13 +155,14 @@ # Control CONTROL-001: "topic_state_monitor_control_command_control_cmd: control_topic_status" - CONTROL-002: "external_cmd_converter: remote_control_topic_status" CONTROL-003: "vehicle_cmd_gate: heartbeat" CONTROL-004: "lane_departure_checker_node: lane_departure" CONTROL-005: "lane_departure_checker_node: trajectory_deviation" + CONTROL-007: "external_cmd_converter: remote_control_topic_status" CONTROL-008: "external_cmd_selector: heartbeat" CONTROL-009: "autonomous_emergency_braking: aeb_emergency_stop" CONTROL-010: "control_validator: control_validation_max_distance_deviation" + CONTROL-011: "slip_detector: slip_status" # Vehicle VEHICLE-001-1: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" @@ -138,7 +173,8 @@ # System SYSTEM-001: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" SYSTEM-002: "vehicle_cmd_gate: emergency_stop_operation" - SYSTEM-003: "bagpacker_state" + SYSTEM-003: "rosbag_status" + SYSTEM-004: "disk_status" SYSTEM-005: "edge_core_internet_connection" # Others @@ -154,26 +190,4 @@ OTHERS-005/left_upper: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" OTHERS-005/right_upper: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" OTHERS-010: "emergency_vehicle_detector: emergency_vehicle" - - # Not in System_Hazard_and_Fail_Safe - debug_data_logger_disk_usage_error: "bagpacker_state/disk_usage" - slip_status: "slip_detector: slip_status" - daytime_status: "daytime_monitor: daytime_status" - perception_delay_check: "multi_object_tracker: Perception delay check from original header stamp" - imu_topic_status: "topic_state_monitor_imu_data: imu_topic_status" - gnss_topic_status: "topic_state_monitor_gnss_pose: gnss_topic_status" - septentrio_status: "septentrio_driver: Quality indicators" - /sensing/camera/0/connection: "topic_state_monitor_camera0: camera0_topic_status" - /sensing/camera/1/connection: "topic_state_monitor_camera0: camera1_topic_status" - /sensing/camera/2/connection: "topic_state_monitor_camera0: camera2_topic_status" - /sensing/camera/3/connection: "topic_state_monitor_camera0: camera3_topic_status" - /sensing/camera/4/connection: "topic_state_monitor_camera0: camera4_topic_status" - /sensing/camera/5/connection: "topic_state_monitor_camera0: camera5_topic_status" - /sensing/camera/6/connection: "topic_state_monitor_camera0: camera6_topic_status" - /sensing/camera/7/connection: "topic_state_monitor_camera0: camera7_topic_status" - /sensing/radar/front_center/connection: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" - /sensing/radar/front_left/connection: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" - /sensing/radar/front_right/connection: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" - /sensing/radar/rear_center/connection: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" - /sensing/radar/rear_left/connection: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" - /sensing/radar/rear_right/connection: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" + OTHERS-011: "daytime_monitor: daytime_status" From 273a55f08452acb76425a5dfd79e5a9a7a238994 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 22 Apr 2024 16:23:31 +0900 Subject: [PATCH 162/233] fix(diagnostic_graph_aggregator): fix slip detection diag timeout (#637) Signed-off-by: Makoto Kurihara --- .../config/system/diagnostic_graph_aggregator/control.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml index 19d5671924..432781f0ba 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml @@ -108,4 +108,4 @@ nodes: - path: /control/011-slip_detection diag: "slip_detector: slip_status" type: diag - timeout: 1.0 + timeout: 3.0 From 68be65d4381ab5867efb317e2fc3818ab4f9f26a Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Tue, 23 Apr 2024 08:47:57 +0900 Subject: [PATCH 163/233] fix(system_monitor): update temperature threshold (#639) --- .../config/system/system_monitor/hdd_monitor.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml b/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml index d818d848be..fdfbcb3271 100644 --- a/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml @@ -6,8 +6,8 @@ disk0: name: / temp_attribute_id: 0xC2 - temp_warn: 55.0 - temp_error: 70.0 + temp_warn: 70.0 + temp_error: 85.0 power_on_hours_attribute_id: 0x09 power_on_hours_warn: 3000000 total_data_written_attribute_id: 0xF1 From 690ce7afec98a71e53fc7754eb80604dbae5d7d4 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 23 Apr 2024 13:11:42 +0900 Subject: [PATCH 164/233] feat: apply new acceleration settings (#612) * feat: apply acceleration for emergency deceleration (-0.6G) Signed-off-by: Makoto Kurihara * feat: apply behavior deceleration (-0.4G) Signed-off-by: Makoto Kurihara * feat: apply comfortable stop/launch acceleration settings Signed-off-by: Makoto Kurihara --------- Signed-off-by: Makoto Kurihara --- .../longitudinal/pid.param.yaml | 4 ++-- .../vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 6 +++--- .../scenario_planning/common/common.param.yaml | 8 ++++---- .../behavior_velocity_planner.param.yaml | 2 +- .../behavior_velocity_planner/crosswalk.param.yaml | 14 +++++++------- .../intersection.param.yaml | 2 +- .../mrm_comfortable_stop_operator.param.yaml | 4 ++-- .../mrm_emergency_stop_operator.param.yaml | 4 ++-- 8 files changed, 22 insertions(+), 22 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 0e1c2ba075..fd83953182 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -63,11 +63,11 @@ # acceleration limit max_acc: 1.86 - min_acc: -3.36 + min_acc: -6.00 # jerk limit max_jerk: 2.0 - min_jerk: -5.0 + min_jerk: -30.0 # slope compensation # pitch 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 e451ea24f9..6e89a18e06 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 @@ -10,7 +10,7 @@ filter_activated_velocity_threshold: 1.0 external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 - emergency_acceleration: -2.4 + emergency_acceleration: -4.0 moderate_stop_service_acceleration: -1.5 stopped_state_entry_duration_time: 0.1 stop_check_duration: 1.0 @@ -19,8 +19,8 @@ reference_speed_points: [20.0, 30.0] steer_lim: [1.0, 0.8] steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] + lon_acc_lim: [6.0, 6.0] + lon_jerk_lim: [30.0, 30.0] lat_acc_lim: [5.0, 4.0] lat_jerk_lim: [7.0, 6.0] actual_steer_diff_lim: [1.0, 0.8] diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 9f1c261562..857c40cdf2 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,12 +6,12 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.3 # min jerk [m/sss] + min_jerk: -0.6 # min jerk [m/sss] max_jerk: 0.6 # max jerk [m/sss] # constraints to be observed limit: - min_acc: -2.5 # min deceleration limit [m/ss] + min_acc: -6.0 # min deceleration limit [m/ss] max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] + min_jerk: -20.0 # min jerk limit [m/sss] + max_jerk: 20.0 # max jerk limit [m/sss] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index b31506918a..cb4e8acdbd 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -4,7 +4,7 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -2.8 + max_accel: -4.0 max_jerk: -5.0 system_delay: 0.5 delay_response_time: 0.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 9db75db825..54ef4ff402 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -30,10 +30,10 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. + min_acc: -4.0 # min acceleration [m/ss] + min_jerk: -5.0 # min jerk [m/sss] + max_jerk: 5.0 # max jerk [m/sss] # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: @@ -47,9 +47,9 @@ no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. - min_acc: -1.0 # min acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + min_acc: -4.0 # min acceleration [m/ss] + min_jerk: -5.0 # min jerk [m/sss] + max_jerk: 5.0 # max jerk [m/sss] stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.25 m/s = 0.9 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index e9ace39e26..08bed1714c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -9,7 +9,7 @@ default_stopline_margin: 3.0 stopline_overshoot_margin: 0.5 path_interpolation_ds: 0.1 - max_accel: -2.8 + max_accel: -4.0 max_jerk: -5.0 delay_response_time: 0.5 enable_pass_judge_before_default_stopline: false diff --git a/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml b/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml index 81bc9b9c0b..7868246909 100644 --- a/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml +++ b/autoware_launch/config/system/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml @@ -2,5 +2,5 @@ ros__parameters: update_rate: 10 min_acceleration: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 + max_jerk: 0.6 + min_jerk: -0.6 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..d3f0d41fd7 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: -4.0 + target_jerk: -5.0 From 33ef529164ce0f366d29306c113b481fe3b3083a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Tue, 23 Apr 2024 13:39:34 +0900 Subject: [PATCH 165/233] feat(mrm_handler): use mrm pull over (#640) Signed-off-by: Makoto Kurihara --- .../config/system/mrm_handler/mrm_handler.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml index 2370259cbe..f4fda15af6 100644 --- a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml +++ b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml @@ -9,7 +9,7 @@ use_emergency_holding: false timeout_emergency_recovery: 5.0 use_parking_after_stopped: false - use_pull_over: false + use_pull_over: true use_comfortable_stop: true # setting whether to turn hazard lamp on for each situation From 05e3e49cf9981b180e10167df9f3f1a624f08365 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 23 Apr 2024 16:56:22 +0900 Subject: [PATCH 166/233] chore(diagnostic_graph_aggregator): fix typo in voice alert system (#635) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/hardware.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml index 6ca18ea993..54e909c426 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml @@ -350,6 +350,6 @@ nodes: timeout: 5.0 - path: /hardware/voice/001-connection - diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voirce alert system heartbeat" + diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voice alert system heartbeat" type: diag timeout: 5.0 From 521144012349888fff45cbb60e24d69740391156 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 25 Apr 2024 13:25:56 +0900 Subject: [PATCH 167/233] tune parameter to improve dynamic collision check Signed-off-by: Daniel Sanchez --- .../start_planner/start_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 75b20bbbfe..073daba72b 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -139,7 +139,7 @@ rss_params: rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 + lateral_distance_max_threshold: 3.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon From 60691ad7cd6a1348acfc72bbf2354c8d88021833 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 8 May 2024 17:18:26 +0800 Subject: [PATCH 168/233] feat: add dummy diag (#641) * tmp: dummy diag * add dummy diags * dummy diag publisher is true --- .../dummy_diag_publisher.param.yaml | 166 +++++++++++++++++- .../tier4_system_component.launch.xml | 12 +- .../launch/planning_simulator.launch.xml | 2 +- 3 files changed, 169 insertions(+), 11 deletions(-) diff --git a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml index 43edd109b5..02c6d305eb 100644 --- a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml +++ b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml @@ -14,4 +14,168 @@ /**: ros__parameters: required_diags: - dummy_diag_empty: default + # hardware + + ## /hardware/cpu/001-temperature + "cpu_monitor: CPU Temperature": default + + ## /hardware/cpu/002-usage + "cpu_monitor: CPU Usage": default + + ## /hardware/cpu/003-load_average + "cpu_monitor: CPU Load Average": default + + ## /hardware/cpu/004-throttling + "cpu_monitor: CPU Thermal Throttling": default + + ## /hardware/cpu/005-frequency + "cpu_monitor: CPU Frequency": default + + ## /hardware/hdd/001-temperature + "hdd_monitor: HDD Temperature": default + + ## /hardware/hdd/002-usage + "hdd_monitor: HDD Usage": default + + ## /hardware/hdd/003-connection + "hdd_monitor: HDD Connection": default + + ## /hardware/hdd/004-total_written + "hdd_monitor: HDD TotalDataWritten": default + + ## /hardware/hdd/005-warranty_period + "hdd_monitor: HDD PowerOnHours": default + + ## /hardware/hdd/006-soft_error + "hdd_monitor: HDD RecoveredError": default + + ## /hardware/hdd/007-load + "hdd_monitor: HDD ReadDataRate": default + "hdd_monitor: HDD WriteDataRate": default + "hdd_monitor: HDD ReadIOPS": default + "hdd_monitor: HDD WriteIOPS": default + + ## /hardware/memory/001-usage + "mem_monitor: Memory Usage": default + + ## /hardware/memory/002-ecc + "mem_monitor: Memory ECC": default + + ## /hardware/network/001-usage + "net_monitor: Network Usage": default + + ## /hardware/network/002-traffic + "net_monitor: Network Traffic": default + + ## /hardware/network/003-crc + "net_monitor: Network CRC Error": default + + ## /hardware/network/004-packet_reassembles + "net_monitor: IP Packet Reassembles Failed": default + + ## /hardware/ntp/001-sync + "ntp_monitor: NTP Offset": default + + ## /hardware/process/001-summary + "process_monitor: Tasks Summary": default + + ## /hardware/process/002-load + "process_monitor: High-load Proc[0]": default + "process_monitor: High-load Proc[1]": default + "process_monitor: High-load Proc[2]": default + "process_monitor: High-load Proc[3]": default + "process_monitor: High-load Proc[4]": default + + ## /hardware/process/003-memory + "process_monitor: High-mem Proc[0]": default + "process_monitor: High-mem Proc[1]": default + "process_monitor: High-mem Proc[2]": default + "process_monitor: High-mem Proc[3]": default + "process_monitor: High-mem Proc[4]": default + + ## /hardware/bios/001-battery + "voltage_monitor: CMOS Battery Status": default + + ## /hardware/gpu/001-temperature + "gpu_monitor: GPU Temperature": default + + ## /hardware/gpu/002-usage + "gpu_monitor: GPU Usage": default + + ## /hardware/gpu/003-memory + "gpu_monitor: GPU Memory Usage": default + + ## /hardware/gpu/004-throttling + "gpu_monitor: GPU Thermal Throttling": default + + ## /hardware/gpu/005-frequency + "gpu_monitor: GPU Frequency": default + + ## /hardware/mot/001-connection + "mot: /system/mot_connection : mot heartbeat": default + + ## /hardware/signage/001-connection + "signage: /system/signage_connection : signage heartbeat": default + + ## /hardware/voice/001-connection + "vehicle_voice_alert_system: /system/voice_alert_system_connection : voice alert system heartbeat": default + + # localization + ## /localization/001-topic_status/initialpose + "topic_state_monitor_initialpose3d: localization_topic_status": default + + ## /localization/001-topic_status/pose_twist_fusion_filter + "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status": default + + ## /localization/002-tf + "topic_state_monitor_transform_map_to_base_link: localization_topic_status": default + + ## /localization/003-matching_score + ndt_scan_matcher: default + + ## /localization/004-accuracy + "localization: localization_error_monitor": default + + # map + ## /map/001-topic_status/pointcloud_map + "topic_state_monitor_pointcloud_map: map_topic_status": default + + # perception + ## /perception/001-topic_status/traffic_signals + "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status": default + + ## /perception/001-topic_status/objects + "topic_state_monitor_object_recognition_objects: perception_topic_status": default + + ## /perception/001-topic_status/pointcloud + "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status": default + + ## /perception/002-detection_delay + "multi_object_tracker: Perception delay check from original header stamp": default + + # system + ## /system/003-bagpacker_status + rosbag_status: default + + ## /system/004-bagpacker_disk_space + disk_status: default + + ## /system/005-fms_connection + edge_core_internet_connection: default + + # vehicle + ## /vehicle/001-topic_status/velocity + "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status": default + + ## /vehicle/001-topic_status/steering + "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status": default + + ## /vehicle/005-vehicle_heartbeat + "j6_interface: vehicle_heartbeat_errors": default + + ## /vehicle/006-vehicle_errors + "j6_interface: vehicle_errors": default + + # others + ## /others/010-emergency_vehicle + "emergency_vehicle_detector: emergency_vehicle": default diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml index b6dea867fe..eee2bf2f0d 100644 --- a/autoware_launch/launch/components/tier4_system_component.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.launch.xml @@ -31,14 +31,8 @@ - - - - - - - - - + + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 051ad53af5..366f1d8ef8 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -18,7 +18,7 @@ - + From ea26d424c90f87d664c375171302ac1b48f58a7d Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 10 May 2024 10:28:07 +0900 Subject: [PATCH 169/233] chore(diagnostic_graph_aggregator): update timeout of daytime monitor (#644) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/others.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 33568d5c7a..9824ea17c2 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -125,4 +125,4 @@ nodes: - path: /others/011-daytime_monitor diag: "daytime_monitor: daytime_status" type: diag - timeout: 1.0 + timeout: 3.0 From a1f49f42df2c474fcfdc92af267a96ffeb7bf5d6 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 10 May 2024 13:53:43 +0900 Subject: [PATCH 170/233] feat(obstacle_cruise): pseudo occlusion (#628) Signed-off-by: Yuki Takagi Co-authored-by: Makoto Kurihara --- .../config/planning/preset/x2_preset.yaml | 3 +- .../obstacle_cruise_planner.param.yaml | 4 + ...cruise_planner_pseudo_occlusion.param.yaml | 215 ++++++++++++++++++ 3 files changed, 221 insertions(+), 1 deletion(-) create mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml diff --git a/autoware_launch/config/planning/preset/x2_preset.yaml b/autoware_launch/config/planning/preset/x2_preset.yaml index f74be4906e..21378f9d97 100644 --- a/autoware_launch/config/planning/preset/x2_preset.yaml +++ b/autoware_launch/config/planning/preset/x2_preset.yaml @@ -94,9 +94,10 @@ launch: - arg: name: motion_stop_planner_type - default: obstacle_cruise_planner + default: obstacle_cruise_planner_with_pseudo_occlusion # option: obstacle_stop_planner # obstacle_cruise_planner + # obstacle_cruise_planner_with_pseudo_occlusion # none - arg: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index bbc303c002..57322fd487 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -107,6 +107,10 @@ slow_down: max_lat_margin: 0.8 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 + pseudo_occlusion: + enable_function: false # If true, the slow down function behave as pseudo_occlusion + max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function + focus_intersections: [0] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml new file mode 100644 index 0000000000..327fb41c11 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml @@ -0,0 +1,215 @@ +/**: + ros__parameters: + common: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + + enable_debug_info: false + enable_calculation_time_info: false + + enable_slow_down_planning: true + + # longitudinal info + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] + min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] + min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] + safe_distance_margin : 6.0 # This is also used as a stop margin [m] + terminal_safe_distance_margin : 6.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index + min_behavior_stop_margin: 6.0 # [m] + stop_on_curve: + enable_approaching: true + additional_safe_distance_margin: 2.0 # [m] + min_safe_distance_margin: 3.5 # [m] + suppress_sudden_obstacle_stop: true + + stop_obstacle_type: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + cruise_obstacle_type: + inside: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + outside: + unknown: false + car: false + truck: false + bus: false + trailer: false + motorcycle: false + bicycle: false + pedestrian: false + + slow_down_obstacle_type: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: false + bicycle: false + pedestrian: false + + behavior_determination: + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking + + prediction_resampling_time_interval: 0.1 + prediction_resampling_time_horizon: 10.0 + + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + # hysteresis for cruise and stop + obstacle_velocity_threshold_from_cruise_to_stop : 4.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 4.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + + # if crossing vehicle is determined as target obstacles or not + crossing_obstacle: + obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + + stop: + max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + crossing_obstacle: + collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + cruise: + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + outside_obstacle: + obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + + slow_down: + max_lat_margin: 1.5 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 + pseudo_occlusion: + enable_function: true # If true, the slow down function behave as pseudo_occlusion + max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function + focus_intersections: [0, 452] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. + + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 + + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: true + time_to_convergence: 1.5 #[s] + + cruise: + pid_based_planner: + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic + + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 + + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false + + disable_target_acceleration: true + + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 + + lpf_normalized_error_cruise_dist_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + replan_vel_deviation: 5.0 # 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] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 + + slow_down: + # parameters to calculate slow down velocity by linear interpolation + labels: + - "default" + - "pedestrian" + default: + moving: + min_lat_margin: 0.7 + max_lat_margin: 1.5 + min_ego_velocity: 2.78 + max_ego_velocity: 9.72 + static: + min_lat_margin: 0.7 + max_lat_margin: 1.5 + min_ego_velocity: 2.78 + max_ego_velocity: 9.72 + pedestrian: + moving: + min_lat_margin: 0.5 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 4.0 + static: + min_lat_margin: 0.5 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 4.0 + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold + + time_margin_on_target_velocity: 1.5 # [s] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start From 92c06d3ebe0b96dd2b1350c3c3a1dda001f6120b Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Sun, 12 May 2024 23:49:05 +0900 Subject: [PATCH 171/233] feat(motion_velocity_smoother): set 5kph for min velocity of lat acc filter (#646) feat(motion_velocity_smoother): set 5km/h for min velocity of lat acc filter Signed-off-by: Makoto Kurihara --- .../autoware_velocity_smoother/velocity_smoother.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml index e08d1d02e1..728f56a858 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml @@ -12,7 +12,7 @@ # lateral acceleration limit parameters enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. max_lateral_accel: 0.6 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + min_curve_velocity: 1.39 # min velocity at lateral acceleration limit and steering angle rate 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 min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] From fa8afb89fad013d2fc637e5d4219ffe02fe99325 Mon Sep 17 00:00:00 2001 From: TakahiroNISHIOKA <75649342+TakahiroNISHIOKA@users.noreply.github.com> Date: Sun, 12 May 2024 23:50:27 +0900 Subject: [PATCH 172/233] chore: update fault injection config (#645) chore: update faultinjection config --- autoware_launch/config/simulator/fault_injection.param.yaml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index 39e9a64a4c..1efa912b85 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -130,7 +130,7 @@ LOCALIZATION-001-2: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" LOCALIZATION-002: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" LOCALIZATION-003: "ndt_scan_matcher" - LOCALIZATION-004: "localization_error_monitor: localization_accuracy" + LOCALIZATION-004: "localization: localization_error_monitor" # Perception PERCEPTION-001-1: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" @@ -187,7 +187,6 @@ OTHERS-002/rear_lower: "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation" OTHERS-002/rear_upper: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" OTHERS-004: "concatenate_data: concat_status" - OTHERS-005/left_upper: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" - OTHERS-005/right_upper: "dual_return_filter: /sensing/lidar/right_upper: visibility_validation" + OTHERS-005/front_lower: "dual_return_filter: /sensing/lidar/front_lower: visibility_validation" OTHERS-010: "emergency_vehicle_detector: emergency_vehicle" OTHERS-011: "daytime_monitor: daytime_status" From 5512e30b2597ad2a5b27ab668ff77070ea5d5b82 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Fri, 17 May 2024 19:38:54 +0900 Subject: [PATCH 173/233] feat: update camera diagnostics (#652) * test * restore timeout --- .../diagnostic_graph_aggregator/sensing.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml index 08caf57125..e46f8499a6 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml @@ -248,35 +248,35 @@ nodes: timeout: 5.0 - path: /sensing/camera/0/connection - diag: "topic_state_monitor_camera0: camera0_topic_status" + diag: "v4l2_camera_camera0: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/1/connection - diag: "topic_state_monitor_camera1: camera1_topic_status" + diag: "v4l2_camera_camera1: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/2/connection - diag: "topic_state_monitor_camera2: camera2_topic_status" + diag: "v4l2_camera_camera2: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/3/connection - diag: "topic_state_monitor_camera3: camera3_topic_status" + diag: "v4l2_camera_camera3: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/4/connection - diag: "topic_state_monitor_camera4: camera4_topic_status" + diag: "v4l2_camera_camera4: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/5/connection - diag: "topic_state_monitor_camera5: camera5_topic_status" + diag: "v4l2_camera_camera5: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/6/connection - diag: "topic_state_monitor_camera6: camera6_topic_status" + diag: "v4l2_camera_camera6: capture_status" type: diag timeout: 1.0 - path: /sensing/camera/7/connection - diag: "topic_state_monitor_camera7: camera7_topic_status" + diag: "v4l2_camera_camera7: capture_status" type: diag timeout: 1.0 From 3aacc89e5cb7fced34ea0c404c24366934ec8925 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 17 May 2024 20:02:15 +0900 Subject: [PATCH 174/233] chore(ground_segmentation): reduce ground_segmentation range (#649) Signed-off-by: badai-nguyen --- .../ground_segmentation.param.yaml | 40 ++++++++++++++++--- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 18e0cb7df5..689ada5e47 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -8,7 +8,7 @@ common_crop_box_filter: parameters: min_x: -70.0 - max_x: 120.0 + max_x: 100.0 min_y: -75.0 max_y: 75.0 max_z: 3.2 @@ -44,7 +44,7 @@ front_upper_crop_box_filter: parameters: - min_x: -50.0 + min_x: 5.8 max_x: 100.0 min_y: -50.0 max_y: 50.0 @@ -74,7 +74,7 @@ front_lower_crop_box_filter: parameters: - min_x: -50.0 + min_x: 5.8 max_x: 100.0 min_y: -50.0 max_y: 50.0 @@ -95,8 +95,38 @@ grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 - elevation_grid_mode: false - use_recheck_ground_cluster: false + elevation_grid_mode: true + use_recheck_ground_cluster: true + use_lowest_point: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + + left_upper_crop_box_filter: + parameters: + min_x: 5.8 + max_x: 40.0 + min_y: -10.0 + max_y: 10.0 + max_z: 1. # recommended 2.5 for non elevation_grid_mode + min_z: -1.0 # recommended 0.0 for non elevation_grid_mode + negative: False + + left_upper_ground_filter: + plugin: "ground_segmentation::ScanGroundFilterComponent" + parameters: + global_slope_max_angle_deg: 6.0 + local_slope_max_angle_deg: 6.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode + use_virtual_ground_point: False + non_ground_height_threshold: 0.05 + grid_size_m: 0.2 + grid_mode_switch_radius: 10.0 + gnd_grid_buffer_size: 3 + detection_range_z_max: 3.2 + elevation_grid_mode: true + use_recheck_ground_cluster: true use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 From 9a06fef32213fa23487dcc18cb77ef86f7c640fd Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 21 May 2024 13:32:01 +0900 Subject: [PATCH 175/233] chore: add vehicle_stuck_checker as emergency stop MRM (#654) Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/others.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 9824ea17c2..646cee3e99 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -26,6 +26,7 @@ nodes: - { type: link, link: /others/002-blockage_validation-error } - { type: link, link: /others/004-concat_status } - { type: link, link: /others/005-visibility_validation-error } + - { type: link, link: /others/012-vehicle_stuck_checker } - path: /others/comfortable_stop type: and @@ -126,3 +127,8 @@ nodes: diag: "daytime_monitor: daytime_status" type: diag timeout: 3.0 + + - path: /others/012-vehicle_stuck_checker + diag: "vehicle_stuck_checker: vehicle_stuck_check" + type: diag + timeout: 3.0 From 123595d1f127e7585d1b26af0d4106c9eb154124 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Wed, 22 May 2024 19:08:20 +0900 Subject: [PATCH 176/233] =?UTF-8?q?revert:=20"feat(motion=5Fvelocity=5Fsmo?= =?UTF-8?q?other):=20set=205kph=20for=20min=20velocity=20of=20=E2=80=A6=20?= =?UTF-8?q?(#653)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Revert "feat(motion_velocity_smoother): set 5kph for min velocity of lat acc filter (#646)" This reverts commit 1e4372d573e54ed2192297dfd5e6a912e8e62037. --- .../autoware_velocity_smoother/velocity_smoother.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml index 728f56a858..e08d1d02e1 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/velocity_smoother.param.yaml @@ -12,7 +12,7 @@ # lateral acceleration limit parameters enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. max_lateral_accel: 0.6 # max lateral acceleration limit [m/ss] - min_curve_velocity: 1.39 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate 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 min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] From a295d2af6500de18c39ef19b6565b0eb6aa32538 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 23 May 2024 16:42:04 +0900 Subject: [PATCH 177/233] fix(diagnostics_graph_aggregator): update timeout (#655) fix: update timeout of emergency_vehicle_detector --- .../config/system/diagnostic_graph_aggregator/others.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 646cee3e99..8f8b4d98bb 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -121,7 +121,7 @@ nodes: - path: /others/010-emergency_vehicle diag: "emergency_vehicle_detector: emergency_vehicle" type: diag - timeout: 1.0 + timeout: 3.0 - path: /others/011-daytime_monitor diag: "daytime_monitor: daytime_status" From 1dcce2886c8651bb4f5f074dcd2891c39a4247ce Mon Sep 17 00:00:00 2001 From: TakahiroNISHIOKA <75649342+TakahiroNISHIOKA@users.noreply.github.com> Date: Fri, 24 May 2024 08:16:26 +0900 Subject: [PATCH 178/233] chore: update camera diag of fault-injection config (#656) chore: update camera diagnostics of fault injection --- .../config/simulator/fault_injection.param.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index 1efa912b85..a7a22e7416 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -103,14 +103,14 @@ PNDR-003/rear_upper: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" # CAMERA - LPD-001/camera0: "topic_state_monitor_camera0: camera0_topic_status" - LPD-001/camera1: "topic_state_monitor_camera1: camera1_topic_status" - LPD-001/camera2: "topic_state_monitor_camera2: camera2_topic_status" - LPD-001/camera3: "topic_state_monitor_camera3: camera3_topic_status" - LPD-001/camera4: "topic_state_monitor_camera4: camera4_topic_status" - LPD-001/camera5: "topic_state_monitor_camera5: camera5_topic_status" - LPD-001/camera6: "topic_state_monitor_camera6: camera6_topic_status" - LPD-001/camera7: "topic_state_monitor_camera7: camera7_topic_status" + LPD-001/camera0: "v4l2_camera_camera0: capture_status" + LPD-001/camera1: "v4l2_camera_camera1: capture_status" + LPD-001/camera2: "v4l2_camera_camera2: capture_status" + LPD-001/camera3: "v4l2_camera_camera3: capture_status" + LPD-001/camera4: "v4l2_camera_camera4: capture_status" + LPD-001/camera5: "v4l2_camera_camera5: capture_status" + LPD-001/camera6: "v4l2_camera_camera6: capture_status" + LPD-001/camera7: "v4l2_camera_camera7: capture_status" # RADAR SENSING-001/front_center: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" From 8dc1e14b4a9a82ee24b60f9344118e1be2286e97 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Sun, 26 May 2024 08:48:19 +0900 Subject: [PATCH 179/233] chore: visibility tuning (#657) --- .../config/system/diagnostic_graph_aggregator/others.yaml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml index 8f8b4d98bb..e47df545e5 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml @@ -58,6 +58,7 @@ nodes: type: and list: - { type: link, link: /others/005-visibility_validation/front_lower } + - { type: link, link: /others/005-visibility_validation/left_upper } - path: /others/002-blockage_validation-error type: warn-to-ok @@ -118,6 +119,11 @@ nodes: type: diag timeout: 1.0 + - path: /others/005-visibility_validation/left_upper + diag: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" + type: diag + timeout: 1.0 + - path: /others/010-emergency_vehicle diag: "emergency_vehicle_detector: emergency_vehicle" type: diag From 2d422c4afe7c8a13ed20b50afc11f7dc6639ab2e Mon Sep 17 00:00:00 2001 From: TakahiroNISHIOKA <75649342+TakahiroNISHIOKA@users.noreply.github.com> Date: Mon, 27 May 2024 11:00:46 +0900 Subject: [PATCH 180/233] chore: update left_upper visibility validation diag of fault-injection config (#658) chore: update visibility diag of fault-injection config --- autoware_launch/config/simulator/fault_injection.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index a7a22e7416..2f4dfab0a4 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -188,5 +188,6 @@ OTHERS-002/rear_upper: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" OTHERS-004: "concatenate_data: concat_status" OTHERS-005/front_lower: "dual_return_filter: /sensing/lidar/front_lower: visibility_validation" + OTHERS-005/left_upper: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" OTHERS-010: "emergency_vehicle_detector: emergency_vehicle" OTHERS-011: "daytime_monitor: daytime_status" From f582a70ebf1d6c533e719646328f344a26aba3fa Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 30 May 2024 18:03:29 +0900 Subject: [PATCH 181/233] feat(behavior_path_planner): add drivable_area linestrings for dynamic (#660) drivable area expansion Signed-off-by: Makoto Kurihara --- .../behavior_path_planner/drivable_area_expansion.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 2f8676e1bf..7226876c75 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -44,6 +44,7 @@ types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border - curbstone + - drivable_area distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid compensate: enable: false # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction From 2698017e3ab1b0f8d9df48ba20de26a191f55442 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 30 May 2024 18:28:51 +0900 Subject: [PATCH 182/233] feat(behavior_path_planner): disable backward parking (#661) Signed-off-by: Makoto Kurihara --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index eea6d0acdc..c34dc04dd3 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -76,7 +76,7 @@ forward_parking_path_interval: 1.0 forward_parking_max_steer_angle: 0.35 # 20deg backward: - enable_arc_backward_parking: true + enable_arc_backward_parking: false after_backward_parking_straight_distance: 2.0 backward_parking_velocity: -1.38 backward_parking_lane_departure_margin: 0.0 From 5dc46f56004ffa1e141967ca058766890fb49490 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 4 Jun 2024 10:30:06 +0900 Subject: [PATCH 183/233] feat(behavior_path_planner_common): add general method for calculating turn signal for bpp modules (#662) * feat(behavior_path_planner_common,turn_signal_decider): add turn_signal_remaining_shift_length_threshold (#1007) add turn_signal_remaining_shift_length_threshold Signed-off-by: Daniel Sanchez * Turn off the turn signal sooner --------- Signed-off-by: Daniel Sanchez Co-authored-by: danielsanchezaran --- .../behavior_path_planner/behavior_path_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index cb1c2b9e30..96ecac2aaf 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -19,7 +19,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 - turn_signal_remaining_shift_length_threshold: 0.1 + turn_signal_remaining_shift_length_threshold: 1.0 turn_signal_on_swerving: true enable_akima_spline_first: false From 9601f59306e58fc79d68be857c9ac7811401fa10 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 5 Jun 2024 15:25:37 +0900 Subject: [PATCH 184/233] feat(goal_planner): set longitudinal_margin: 6.0 (#666) Signed-off-by: kosuke55 --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index c34dc04dd3..645294a68e 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -17,7 +17,7 @@ forward_goal_search_length: 40.0 backward_goal_search_length: 20.0 goal_search_interval: 2.0 - longitudinal_margin: 3.0 + longitudinal_margin: 6.0 max_lateral_offset: 1.0 lateral_offset_interval: 0.5 ignore_distance_from_lane_start: 0.0 From b6400b5250145573d33c7e648e54f7c9f32b21b3 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 6 Jun 2024 18:06:03 +0900 Subject: [PATCH 185/233] feat(planning_validator): relax the threshold for min acceleration (#670) validation Signed-off-by: Makoto Kurihara --- .../common/planning_validator/planning_validator.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml b/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml index da337d70b1..189066a562 100644 --- a/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml @@ -21,7 +21,7 @@ curvature: 1.0 lateral_acc: 9.8 longitudinal_max_acc: 9.8 - longitudinal_min_acc: -9.8 + longitudinal_min_acc: -98.0 steering: 1.414 steering_rate: 10.0 velocity_deviation: 100.0 From 531e19de86fcea0d80fae50b150c877cb8651e7b Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Fri, 7 Jun 2024 08:25:07 +0900 Subject: [PATCH 186/233] chore(diagnostic_graph_aggregator): remove camera6-7 (#671) --- .../system/diagnostic_graph_aggregator/sensing.yaml | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml index e46f8499a6..67ad0d43d8 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml @@ -107,8 +107,6 @@ nodes: - { type: link, link: /sensing/camera/3/connection } - { type: link, link: /sensing/camera/4/connection } - { type: link, link: /sensing/camera/5/connection } - - { type: link, link: /sensing/camera/6/connection } - - { type: link, link: /sensing/camera/7/connection } - path: /sensing/radar/001-connection type: and @@ -271,14 +269,6 @@ nodes: diag: "v4l2_camera_camera5: capture_status" type: diag timeout: 1.0 - - path: /sensing/camera/6/connection - diag: "v4l2_camera_camera6: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/7/connection - diag: "v4l2_camera_camera7: capture_status" - type: diag - timeout: 1.0 - path: /sensing/radar/front_center/connection diag: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" From 80628df719dde026344051c28c382796d20643af Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 14 Jun 2024 16:09:28 +0900 Subject: [PATCH 187/233] feat(obstacle_cruise)!: type specified stop deccel limit and enabling abandon to stop (#669) * abandon_to_stop Signed-off-by: Yuki Takagi * sync to pseudo_occlusion Signed-off-by: Yuki Takagi * adonban to stop for sudden unknown object --------- Signed-off-by: Yuki Takagi Co-authored-by: Shohei Sakai --- ...stacle_cruise_planner_pseudo_occlusion.param.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml index 327fb41c11..e01cd3c99d 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml @@ -213,3 +213,15 @@ lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + stop: + type_specified_params: + labels: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. From 7c3b66f01e6fb0f339cdde7a8a279d28ceaa38e7 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 17 Jun 2024 13:44:09 +0900 Subject: [PATCH 188/233] feat(goal_planner): tune goal planner (#673) Signed-off-by: Makoto Kurihara --- .../goal_planner/goal_planner.param.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index 645294a68e..6eeb085791 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -14,14 +14,14 @@ lateral_weight: 40.0 prioritize_goals_before_objects: true parking_policy: "left_side" # "left_side" or "right_side" - forward_goal_search_length: 40.0 + forward_goal_search_length: 0.5 backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 6.0 - max_lateral_offset: 1.0 + max_lateral_offset: 0.0 lateral_offset_interval: 0.5 ignore_distance_from_lane_start: 0.0 - margin_from_boundary: 0.75 + margin_from_boundary: 0.50 # occupancy grid map occupancy_grid: @@ -59,8 +59,8 @@ shift_parking: enable_shift_parking: true shift_sampling_num: 4 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 + maximum_lateral_jerk: 0.5 + minimum_lateral_jerk: 0.1 deceleration_interval: 15.0 after_shift_straight_distance: 1.0 From 568edc6db5cc41c1193b3655902de512e8931905 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 17 Jun 2024 22:30:32 +0900 Subject: [PATCH 189/233] feat(control_validator): add min velocity for max distance deviation (#675) checking Signed-off-by: Makoto Kurihara --- .../control/control_validator/control_validator.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index ffd259e5e5..3910360424 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -10,3 +10,4 @@ thresholds: max_distance_deviation: 1.0 + min_velocity_for_checking: 1.0 # m/s From 000c7b42d2fbdcea560189e6190eefec501c72e3 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 20 Jun 2024 16:34:33 +0900 Subject: [PATCH 190/233] =?UTF-8?q?chore(diagnostic=5Fgraph=5Faggregator):?= =?UTF-8?q?=20enable=20comfortable=20stop=20when=20vehi=E2=80=A6=20(#677)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit chore(diagnostic_graph_aggregator): enable comfortable stop when vehicle errors become WARN Signed-off-by: Tomohito Ando --- .../config/system/diagnostic_graph_aggregator/vehicle.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml index 4f3190d572..19f34ac02e 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml @@ -30,6 +30,8 @@ nodes: - path: /vehicle/comfortable_stop type: and + list: + - { type: link, link: /vehicle/006-vehicle_errors } - path: /vehicle/pull_over type: and From 8c52fd27824131f1a5fa4a401c118ad8dcbd82b7 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 26 Jun 2024 13:26:12 +0900 Subject: [PATCH 191/233] Rebase all x2 changes to tier4/main fix parameter namespace remove old parameter file remove old style parameter file remove unused parameter remove duplicated parameter remove old style parameter remove old style parameter pick new style parameter remove duplicated parameter remove unnecessary parameter remove old parameter Revert "feat: sp tuning for low obstacle (#578)" This reverts commit f00363f012cfcfe8c244d153e281796c25a9d7cf. Revert "chore(ground_segmentation): reduce ground_segmentation range (#649)" This reverts commit 3aacc89e5cb7fced34ea0c404c24366934ec8925. fix ekf parameter remove temporary parameter disable pull over of mrm change initial_selector_mode to local reset analytical smoother parameter remove hazard lights selector remove l4toolkit enable stuck vehicle detection in left turn chore diagnostic_graph_aggregator params fix module name chore x2_preset.yaml fix(static_obstacle_avoidance): parameter tuning Signed-off-by: satoshi-ota fix(slow_down): parameter tuning Signed-off-by: satoshi-ota fix typo remove duplicated parameter remove old node revert common planning param for safety reason. use mrm_handler as default relax emergency stop acceleration for safety reason modify rviz setting remove l4toolkit from simulation fix lane change priority fix behavior velocity planner parameter make same mpt param as main update mlmodel parameter style fix old parameter name --- .../control_validator.param.yaml | 1 - .../external_cmd_selector.param.yaml | 2 +- .../longitudinal/pid.param.yaml | 3 +- .../localization/ekf_localizer.param.yaml | 16 +- .../localization/ndt_scan_matcher.param.yaml | 84 ------- ...el_grid_based_euclidean_cluster.param.yaml | 2 +- .../lidar_model/centerpoint_x2.param.yaml | 20 ++ .../pointcloud_map_filter.param.yaml | 4 +- .../data_association_matrix.param.yaml | 2 +- .../ground_segmentation.param.yaml | 66 ++--- .../config/planning/preset/x2_preset.yaml | 30 ++- .../Analytical.param.yaml | 4 +- .../common/common.param.yaml | 8 +- .../static_obstacle_avoidance.param.yaml | 28 +-- .../avoidance/avoidance.param.yaml | 222 ----------------- .../drivable_area_expansion.param.yaml | 18 -- .../lane_change/lane_change.param.yaml | 20 +- .../scene_module_manager.param.yaml | 4 +- .../start_planner/start_planner.param.yaml | 3 +- .../behavior_velocity_planner.param.yaml | 2 +- .../crosswalk.param.yaml | 6 +- .../intersection.param.yaml | 24 +- .../run_out.param.yaml | 6 - .../path_optimizer.param.yaml | 2 +- .../obstacle_cruise_planner.param.yaml | 11 +- ...cruise_planner_pseudo_occlusion.param.yaml | 227 ------------------ .../component_state_monitor/topics.yaml | 13 - .../hazard_lights_selector.param.yaml | 3 - .../mrm_emergency_stop_operator.param.yaml | 4 +- .../system/mrm_handler/mrm_handler.param.yaml | 2 +- autoware_launch/launch/autoware.launch.xml | 16 -- .../tier4_localization_component.launch.xml | 2 +- .../tier4_perception_component.launch.xml | 3 +- .../tier4_system_component.launch.xml | 17 +- .../launch/logging_simulator.launch.xml | 2 - .../launch/planning_simulator.launch.xml | 2 - autoware_launch/rviz/autoware.rviz | 4 +- 37 files changed, 137 insertions(+), 746 deletions(-) delete mode 100644 autoware_launch/config/localization/ndt_scan_matcher.param.yaml create mode 100644 autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml delete mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml delete mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml delete mode 100644 autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index 3910360424..ffd259e5e5 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -10,4 +10,3 @@ thresholds: max_distance_deviation: 1.0 - min_velocity_for_checking: 1.0 # m/s diff --git a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml index 3de702aa00..6556656cc8 100644 --- a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml +++ b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: update_rate: 10.0 - initial_selector_mode: "remote" # ["local", "remote"] + initial_selector_mode: "local" # ["local", "remote"] diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index fd83953182..688e650de1 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -71,9 +71,8 @@ # slope compensation # pitch - use_trajectory_for_pitch_calculation: true lpf_pitch_gain: 0.95 - slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + slope_source: "trajectory_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 096db82bdd..6df24435cc 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -1,10 +1,12 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: false - predict_frequency: 50.0 - tf_rate: 50.0 - extend_state_step: 50 + node: + show_debug_info: false + enable_yaw_bias_estimation: false + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 pose_measurement: # for Pose measurement @@ -40,7 +42,5 @@ misc: # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + threshold_observable_velocity_mps: 0.5 # [m/s] pose_frame_id: "map" - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.5 # [m/s] diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml deleted file mode 100644 index 4c29059581..0000000000 --- a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml +++ /dev/null @@ -1,84 +0,0 @@ -/**: - ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - - # Vehicle reference frame - base_frame: "base_link" - - # Subscriber queue size - input_sensor_points_queue_size: 1 - - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 - - # The newton line search maximum step length - step_size: 0.1 - - # The ND voxel grid resolution - resolution: 2.0 - - # The number of iterations required to calculate alignment - max_iterations: 30 - - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 - - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 - - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 - - # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 - - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 - - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - - # Number of threads used for parallel computing - num_threads: 4 - - # The covariance of output pose - # Do note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] - - # Regularization switch - regularization_enabled: false - - # Regularization scale factor - regularization_scale_factor: 0.01 - - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 - - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 - - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 - - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false - - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml index b99612ab8f..26b027f007 100644 --- a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -3,7 +3,7 @@ tolerance: 0.7 voxel_leaf_size: 0.3 min_points_number_per_voxel: 1 - min_cluster_size: 3 + min_cluster_size: 10 max_cluster_size: 3000 use_height: false input_frame: "base_link" diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml new file mode 100644 index 0000000000..0777ddb92b --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" + trt_precision: fp16 + post_process_params: + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + score_threshold: 0.35 + densification_params: + world_frame_id: map + num_past_frames: 1 diff --git a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml index 5b9ad199fa..62b3074c15 100644 --- a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml @@ -2,13 +2,13 @@ ros__parameters: # voxel size for downsample filter - down_sample_voxel_size: 0.05 + down_sample_voxel_size: 0.1 # distance threshold for compare compare distance_threshold: 0.5 # ratio to reduce voxel_leaf_size and neighbor points distance threshold in z axis - downsize_ratio_z_axis: 0.3 + downsize_ratio_z_axis: 0.6 # publish voxelized map pointcloud for debug publish_debug_pcd: False diff --git a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 9743a9ce59..13f2220655 100644 --- a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -56,7 +56,7 @@ min_iou_matrix: # If value is negative, it will be ignored. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 689ada5e47..ec966ecc45 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - additional_lidars: ["front_lower"] + additional_lidars: [] ransac_input_topics: [] use_single_frame_filter: False use_time_series_filter: True @@ -8,11 +8,11 @@ common_crop_box_filter: parameters: min_x: -70.0 - max_x: 100.0 + max_x: 120.0 min_y: -75.0 max_y: 75.0 - max_z: 3.2 - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False common_ground_filter: @@ -20,10 +20,10 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 25.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.15 + split_points_distance_tolerance: 0.2 use_virtual_ground_point: True split_height_distance: 0.2 - non_ground_height_threshold: 0.12 + non_ground_height_threshold: 0.20 grid_size_m: 0.2 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 5 @@ -44,12 +44,12 @@ front_upper_crop_box_filter: parameters: - min_x: 5.8 + min_x: -50.0 max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False front_upper_ground_filter: @@ -58,7 +58,7 @@ global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode - split_height_distance: 0.15 # recommended 0.15 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode use_virtual_ground_point: False non_ground_height_threshold: 0.1 grid_size_m: 0.1 @@ -74,12 +74,12 @@ front_lower_crop_box_filter: parameters: - min_x: 5.8 + min_x: -50.0 max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False front_lower_ground_filter: @@ -87,46 +87,16 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.10 # recommended 0.1 for non elevation_grid_mode - split_height_distance: 0.05 # recommended 0.05 for non elevation_grid_mode - use_virtual_ground_point: true + split_points_distance_tolerance: 0.20 # recommended 0.1 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.05 for non elevation_grid_mode + use_virtual_ground_point: False non_ground_height_threshold: 0.1 grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 - elevation_grid_mode: true - use_recheck_ground_cluster: true - use_lowest_point: true - low_priority_region_x: -20.0 - center_pcl_shift: 0.0 - radial_divider_angle_deg: 1.0 - - left_upper_crop_box_filter: - parameters: - min_x: 5.8 - max_x: 40.0 - min_y: -10.0 - max_y: 10.0 - max_z: 1. # recommended 2.5 for non elevation_grid_mode - min_z: -1.0 # recommended 0.0 for non elevation_grid_mode - negative: False - - left_upper_ground_filter: - plugin: "ground_segmentation::ScanGroundFilterComponent" - parameters: - global_slope_max_angle_deg: 6.0 - local_slope_max_angle_deg: 6.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode - split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode - use_virtual_ground_point: False - non_ground_height_threshold: 0.05 - grid_size_m: 0.2 - grid_mode_switch_radius: 10.0 - gnd_grid_buffer_size: 3 - detection_range_z_max: 3.2 - elevation_grid_mode: true - use_recheck_ground_cluster: true + elevation_grid_mode: false + use_recheck_ground_cluster: false use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 diff --git a/autoware_launch/config/planning/preset/x2_preset.yaml b/autoware_launch/config/planning/preset/x2_preset.yaml index 21378f9d97..12c1bcadf5 100644 --- a/autoware_launch/config/planning/preset/x2_preset.yaml +++ b/autoware_launch/config/planning/preset/x2_preset.yaml @@ -1,14 +1,17 @@ launch: # behavior path modules - arg: - name: launch_avoidance_module + name: launch_static_obstacle_avoidance default: "true" - arg: name: launch_avoidance_by_lane_change_module default: "false" - arg: - name: launch_dynamic_avoidance_module + name: launch_dynamic_obstacle_avoidance default: "false" + - arg: + name: launch_sampling_planner_module + default: "false" # Warning, experimental module, use only in simulations - arg: name: launch_lane_change_right_module default: "true" @@ -71,9 +74,6 @@ launch: - arg: name: launch_speed_bump_module default: "false" - - arg: - name: launch_out_of_lane_module - default: "true" - arg: name: launch_no_drivable_lane_module default: "false" @@ -87,21 +87,31 @@ launch: - arg: name: motion_path_planner_type - default: obstacle_avoidance_planner - # option: obstacle_avoidance_planner + default: path_optimizer + # option: path_optimizer # path_sampler # none + # motion velocity planner modules + - arg: + name: launch_dynamic_obstacle_stop_module + default: "false" + - arg: + name: launch_out_of_lane_module + default: "true" + - arg: + name: launch_obstacle_velocity_limiter_module + default: "true" + - arg: name: motion_stop_planner_type - default: obstacle_cruise_planner_with_pseudo_occlusion + default: obstacle_cruise_planner # option: obstacle_stop_planner # obstacle_cruise_planner - # obstacle_cruise_planner_with_pseudo_occlusion # none - arg: - name: motion_velocity_smoother_type + name: velocity_smoother_type default: JerkFiltered # option: JerkFiltered # L2 diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml index 232610713f..329714e3d3 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml @@ -17,8 +17,8 @@ kp: 0.3 backward: - start_jerk: -0.3 - min_jerk_mild_stop: -0.5 + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 min_jerk: -1.5 min_acc_mild_stop: -1.0 min_acc: -2.5 diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 857c40cdf2..9f1c261562 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,12 +6,12 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.6 # min jerk [m/sss] + min_jerk: -0.3 # min jerk [m/sss] max_jerk: 0.6 # max jerk [m/sss] # constraints to be observed limit: - min_acc: -6.0 # min deceleration limit [m/ss] + min_acc: -2.5 # min deceleration limit [m/ss] max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -20.0 # min jerk limit [m/sss] - max_jerk: 20.0 # max jerk limit [m/sss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] 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 24e88c8ed6..7e03b1c45f 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 @@ -23,41 +23,41 @@ th_moving_time: 2.0 # [s] longitudinal_margin: 0.0 # [m] lateral_margin: - soft_margin: 0.3 # [m] + soft_margin: 0.2 # [m] hard_margin: 0.2 # [m] hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] FOR DEVELOPER - envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER + envelope_buffer_margin: 0.1 # [m] FOR DEVELOPER truck: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 bus: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 trailer: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 unknown: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -214,7 +214,7 @@ longitudinal: min_prepare_time: 1.0 # [s] max_prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] + min_prepare_distance: 4.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER @@ -253,7 +253,7 @@ # but there is a possibility that the vehicle can't stop in front of the vehicle. # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. - deceleration: "best_effort" # [-] + deceleration: "reliable" # [-] # policy for avoidance lateral margin. select "best_effort" or "reliable". # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal # margin to avoid. @@ -261,7 +261,7 @@ # with expected lateral margin. lateral_margin: "best_effort" # [-] # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. - use_shorten_margin_immediately: true # [-] + use_shorten_margin_immediately: false # [-] # -------------------------------------- # FOLLOWING PARAMETERS ARE FOR DEVELOPER @@ -270,10 +270,10 @@ constraints: # lateral constraints lateral: - velocity: [1.39, 4.17, 11.1] # [m/s] + velocity: [2.78, 8.33, 11.1] # [m/s] max_accel_values: [0.5, 0.5, 0.5] # [m/ss] min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + max_jerk_values: [0.5, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: @@ -281,7 +281,7 @@ nominal_jerk: 0.5 # [m/sss] max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] - max_acceleration: 0.5 # [m/ss] + max_acceleration: 0.3 # [m/ss] min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] shift_line_pipeline: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml deleted file mode 100644 index b131e60dd0..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ /dev/null @@ -1,222 +0,0 @@ -# see AvoidanceParameters description in avoidance_module_data.hpp for description. -/**: - ros__parameters: - avoidance: - 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] - - # 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_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true - enable_yield_maneuver: true - enable_yield_maneuver_during_shifting: false - disable_path_update: false - - # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true - use_intersection_areas: true - use_hatched_road_markings: true - - # for debug - publish_debug_marker: false - print_debug_info: false - - # avoidance is performed for the object type with true - target_object: - car: - enable: true # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.1 # [m] - safety_buffer_longitudinal: 0.0 # [m] - use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. - truck: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.8 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - bus: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - trailer: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - unknown: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 1.0 # [m] - hard_margin: 0.7 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - bicycle: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - motorcycle: - enable: false - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - pedestrian: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - lower_distance_for_polygon_expansion: 30.0 # [m] - upper_distance_for_polygon_expansion: 100.0 # [m] - - # For target object filtering - target_filtering: - # filtering moving objects - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] - # detection range - object_ignore_distance_traffic_light: 30.0 # [m] - object_ignore_distance_crosswalk_forward: 30.0 # [m] - object_ignore_distance_crosswalk_backward: 30.0 # [m] - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 10.0 # [m] - object_check_goal_distance: 0.0 # [m] - # filtering parking objects - threshold_distance_object_is_on_center: 0.5 # [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: 100.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_execution_threshold: 0.499 # [m] - lateral_small_shift_threshold: 0.101 # [m] - lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.3 # [m] - hard_road_shoulder_margin: 0.3 # [m] - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - max_deviation_from_lane: 0.5 # [m] - # approve the next shift line after reaching this percentage of the current shift line length. - # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. - # this feature can be disabled by setting this parameter to 0.0. - ratio_for_return_shift_approval: 0.5 # [-] - # avoidance distance parameters - longitudinal: - 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] - - # 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] - - shift_line_pipeline: - trim: - quantize_filter_threshold: 0.2 - same_grad_filter_1_threshold: 0.1 - same_grad_filter_2_threshold: 0.2 - same_grad_filter_3_threshold: 0.5 - sharp_shift_filter_threshold: 0.2 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 7226876c75..46082e2659 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -18,11 +18,6 @@ extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase extra_front_overhang: 0.5 # [m] extra length to add to the front overhang extra_width: 1.0 # [m] extra length to add to the width - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint dynamic_objects: avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -34,22 +29,9 @@ max_arc_length: 100.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border - curbstone - drivable_area distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: false # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation - replan_checker: - enable: true # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly - # not compatible with dynamic_objects.avoid - max_deviation: 1.0 # [m] full replan is only done if the path changes by more than this distance 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 2605bfd563..7adb3fe776 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 @@ -46,14 +46,6 @@ lateral_distance_max_threshold: 0.75 longitudinal_distance_min_threshold: 2.0 longitudinal_velocity_delta_time: 0.3 - stuck: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -2.0 - rear_vehicle_reaction_time: 1.5 - rear_vehicle_safety_time_margin: 0.8 - lateral_distance_max_threshold: 1.0 - longitudinal_distance_min_threshold: 2.5 - longitudinal_velocity_delta_time: 0.6 stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -68,11 +60,6 @@ left_offset: 1.0 # [m] right_offset: 1.0 # [m] - # lane expansion for object filtering - lane_expansion: - left_offset: 1.0 # [m] - right_offset: 1.0 # [m] - # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] @@ -88,7 +75,7 @@ unknown: false bicycle: true motorcycle: true - pedestrian: false + pedestrian: true # collision check enable_collision_check_for_prepare_phase: @@ -111,11 +98,6 @@ velocity: 0.5 # [m/s] stop_time: 3.0 # [s] - # ego vehicle stuck detection - stuck_detection: - velocity: 0.5 # [m/s] - stop_time: 3.0 # [s] - # lane change cancel cancel: enable_on_prepare_phase: true 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 d54c1b210f..c62c866492 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 @@ -24,7 +24,7 @@ enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 4 + priority: 5 max_module_size: 1 lane_change_right: @@ -32,7 +32,7 @@ enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 4 + priority: 5 max_module_size: 1 start_planner: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 073daba72b..9d7054487f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -26,11 +26,12 @@ allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 - deceleration_interval: 15.0 + maximum_longitudinal_deviation: 1.0 # Note, should be the same or less than planning validator's "longitudinal_distance_deviation" lateral_jerk: 0.5 lateral_acceleration_sampling_num: 3 minimum_lateral_acc: 0.15 maximum_lateral_acc: 0.5 + maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true geometric_collision_check_distance_from_end: 0.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index cb4e8acdbd..b31506918a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -4,7 +4,7 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -4.0 + max_accel: -2.8 max_jerk: -5.0 system_delay: 0.5 delay_response_time: 0.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 54ef4ff402..88d730f654 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -30,8 +30,8 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. - min_acc: -4.0 # min acceleration [m/ss] + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle + min_acc: -2.8 # min acceleration [m/ss] min_jerk: -5.0 # min jerk [m/sss] max_jerk: 5.0 # max jerk [m/sss] @@ -47,7 +47,7 @@ no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. - min_acc: -4.0 # min acceleration [m/ss] + min_acc: -2.8 # min acceleration [m/ss] min_jerk: -5.0 # min jerk [m/sss] max_jerk: 5.0 # max jerk [m/sss] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 08bed1714c..7820a5db31 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -9,7 +9,7 @@ default_stopline_margin: 3.0 stopline_overshoot_margin: 0.5 path_interpolation_ds: 0.1 - max_accel: -4.0 + max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 enable_pass_judge_before_default_stopline: false @@ -24,7 +24,7 @@ bicycle: false unknown: false turn_direction: - left: false + left: true right: true straight: true use_stuck_stopline: true @@ -75,17 +75,18 @@ not_prioritized: collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr - use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module - minimum_upstream_velocity: 1.38 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 duration: 8.0 object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: - object_expected_deceleration: 2.0 # [m/ss] + object_expected_deceleration: + car: 2.0 + bike: 5.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: true @@ -105,11 +106,12 @@ occlusion_detection_hold_time: 1.5 temporal_stop_time_before_peeking: 0.1 temporal_stop_before_attention_area: false - absence_traffic_light: - creep_velocity: 1.388 # [m/s] - maximum_peeking_distance: -0.5 # [m] - attention_lane_crop_curvature_threshold: 0.25 - attention_lane_curvature_calculation_ds: 0.5 + creep_velocity_without_traffic_light: 1.388 + static_occlusion_with_traffic_light_timeout: 0.5 + + debug: + ttc: [0] + enable_rtc: intersection: false diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index e5fdabef3a..c8905f66da 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -67,12 +67,6 @@ margin_behind: 0.5 # [m] ahead margin for detection area length margin_ahead: 1.0 # [m] behind margin for detection area length - # parameter to avoid sudden stopping - slow_down_limit: - enable: false - max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. - max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # Only used when "detection_method" is set to Points # Points in this area are detected even if it is in the no obstacle segmentation area # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index 1f78eecb02..93cc644040 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -44,7 +44,7 @@ enable_optimization_validation: false common: - num_points: 50 # number of points for optimization [-] + num_points: 100 # number of points for optimization [-] delta_arc_length: 1.0 # delta arc length for optimization [m] kinematics: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 57322fd487..70dfbaec9c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -107,11 +107,6 @@ slow_down: max_lat_margin: 0.8 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 - pseudo_occlusion: - enable_function: false # If true, the slow down function behave as pseudo_occlusion - max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function - focus_intersections: [0] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. - successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 @@ -198,9 +193,9 @@ max_ego_velocity: 5.56 static: min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 4.0 - max_ego_velocity: 8.0 + max_lat_margin: 0.7 + min_ego_velocity: 2.0 + max_ego_velocity: 5.56 moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml deleted file mode 100644 index e01cd3c99d..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml +++ /dev/null @@ -1,227 +0,0 @@ -/**: - ros__parameters: - common: - planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - - enable_debug_info: false - enable_calculation_time_info: false - - enable_slow_down_planning: true - - # longitudinal info - idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] - min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] - min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] - safe_distance_margin : 6.0 # This is also used as a stop margin [m] - terminal_safe_distance_margin : 6.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] - hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] - hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] - - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index - nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index - min_behavior_stop_margin: 6.0 # [m] - stop_on_curve: - enable_approaching: true - additional_safe_distance_margin: 2.0 # [m] - min_safe_distance_margin: 3.5 # [m] - suppress_sudden_obstacle_stop: true - - stop_obstacle_type: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - cruise_obstacle_type: - inside: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - outside: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - slow_down_obstacle_type: - unknown: false - car: true - truck: true - bus: true - trailer: true - motorcycle: false - bicycle: false - pedestrian: false - - behavior_determination: - decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - - prediction_resampling_time_interval: 0.1 - prediction_resampling_time_horizon: 10.0 - - stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle - - # hysteresis for cruise and stop - obstacle_velocity_threshold_from_cruise_to_stop : 4.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - obstacle_velocity_threshold_from_stop_to_cruise : 4.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - - # if crossing vehicle is determined as target obstacles or not - crossing_obstacle: - obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop - - stop: - max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width - crossing_obstacle: - collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] - - cruise: - max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width - outside_obstacle: - obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] - max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - - slow_down: - max_lat_margin: 1.5 # lateral margin between obstacle and trajectory band with ego's width - lat_hysteresis_margin: 0.2 - pseudo_occlusion: - enable_function: true # If true, the slow down function behave as pseudo_occlusion - max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function - focus_intersections: [0, 452] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. - - successive_num_to_entry_slow_down_condition: 5 - successive_num_to_exit_slow_down_condition: 5 - - # consider the current ego pose (it is not the nearest pose on the reference trajectory) - # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" - # The both errors decrease with constant rates against the time. - consider_current_pose: - enable_to_consider_current_pose: true - time_to_convergence: 1.5 #[s] - - cruise: - pid_based_planner: - use_velocity_limit_based_planner: true - error_function_type: quadratic # choose from linear, quadratic - - velocity_limit_based_planner: - # PID gains to keep safe distance with the front vehicle - kp: 10.0 - ki: 0.0 - kd: 2.0 - - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] - - enable_jerk_limit_to_output_acc: false - - disable_target_acceleration: true - - velocity_insertion_based_planner: - kp_acc: 6.0 - ki_acc: 0.0 - kd_acc: 2.0 - - kp_jerk: 20.0 - ki_jerk: 0.0 - kd_jerk: 0.0 - - output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - - enable_jerk_limit_to_output_acc: true - - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] - time_to_evaluate_rss: 0.0 - - lpf_normalized_error_cruise_dist_gain: 0.2 - - optimization_based_planner: - dense_resampling_time_interval: 0.2 - sparse_resampling_time_interval: 2.0 - dense_time_horizon: 5.0 - max_time_horizon: 25.0 - velocity_margin: 0.2 #[m/s] - - # Parameters for safe distance - t_dangerous: 0.5 - - # For initial Motion - replan_vel_deviation: 5.0 # 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] - - # Weights for optimization - max_s_weight: 100.0 - max_v_weight: 1.0 - over_s_safety_weight: 1000000.0 - over_s_ideal_weight: 50.0 - over_v_weight: 500000.0 - over_a_weight: 5000.0 - over_j_weight: 10000.0 - - slow_down: - # parameters to calculate slow down velocity by linear interpolation - labels: - - "default" - - "pedestrian" - default: - moving: - min_lat_margin: 0.7 - max_lat_margin: 1.5 - min_ego_velocity: 2.78 - max_ego_velocity: 9.72 - static: - min_lat_margin: 0.7 - max_lat_margin: 1.5 - min_ego_velocity: 2.78 - max_ego_velocity: 9.72 - pedestrian: - moving: - min_lat_margin: 0.5 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 4.0 - static: - min_lat_margin: 0.5 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 4.0 - moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" - moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold - - time_margin_on_target_velocity: 1.5 # [s] - - lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity - lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path - lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start - stop: - type_specified_params: - labels: # For the listed types, the node try to read the following type specified values - - "default" - - "unknown" - # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. - # limit_min_acc: common_param.yaml/limit.min_acc - unknown: - limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. - sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". - sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". - abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index 9e81adf3e3..a9b18001d7 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -50,19 +50,6 @@ error_rate: 1.0 timeout: 1.0 -- module: localization - mode: [online, logging_simulation] - type: autonomous - args: - node_name_suffix: pose_estimator_pose - topic: /localization/pose_estimator/pose_with_covariance - topic_type: geometry_msgs/msg/PoseWithCovarianceStamped - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - - module: perception mode: [online, logging_simulation] type: launch diff --git a/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml b/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml deleted file mode 100644 index 68273fd990..0000000000 --- a/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - update_rate: 10 # hazard lights command publish rate [Hz] 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 d3f0d41fd7..1ee2699a23 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: -4.0 - target_jerk: -5.0 + target_acceleration: -2.5 + target_jerk: -1.5 diff --git a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml index f4fda15af6..2370259cbe 100644 --- a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml +++ b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml @@ -9,7 +9,7 @@ use_emergency_holding: false timeout_emergency_recovery: 5.0 use_parking_after_stopped: false - use_pull_over: true + use_pull_over: false use_comfortable_stop: true # setting whether to turn hazard lamp on for each situation diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index b96bfdcbc0..5cf27f274f 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -23,7 +23,6 @@ - @@ -47,9 +46,6 @@ - - - @@ -136,11 +132,6 @@ - - - - - - - - - - - - diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index b6e2f8f18a..1b92977330 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 5d9ecebe1f..fe24e836b0 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -25,8 +25,7 @@ - - + diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml index eee2bf2f0d..027837196d 100644 --- a/autoware_launch/launch/components/tier4_system_component.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.launch.xml @@ -15,10 +15,10 @@ - + + - @@ -29,10 +29,17 @@ - + - - + + + + + + + + + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 12f9cab3be..fa33147824 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -75,8 +75,6 @@ - - diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 366f1d8ef8..88712916b1 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -76,8 +76,6 @@ - - diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 96b17995c2..fc757b8156 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2215,7 +2215,7 @@ Visualization Manager: Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: DebugMarker Namespaces: {} @@ -2226,7 +2226,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker Value: true - Enabled: true + Enabled: false Name: ObstacleCruise - Class: rviz_default_plugins/MarkerArray Enabled: false From c4afb6727ebcd2c32416bf9d887fcc09a296561f Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 1 Jul 2024 09:06:47 +0900 Subject: [PATCH 192/233] chore: change max_z of cropbox filter to vehicle_height (#585) * chore: change max_z of cropbox filter to vehicle_height Signed-off-by: badai-nguyen * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: badai-nguyen --- .../ground_segmentation.param.yaml | 12 ++++++------ .../tier4_localization_component.launch.xml | 6 +++++- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index ec966ecc45..83a02d9454 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -11,8 +11,8 @@ max_x: 120.0 min_y: -75.0 max_y: 75.0 - margin_max_z: 3.2 - margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_min_z: -2.5 # to extend the crop box min_z from ground negative: False common_ground_filter: @@ -48,8 +48,8 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 - margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_min_z: -2.5 # to extend the crop box min_z from ground negative: False front_upper_ground_filter: @@ -78,8 +78,8 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 - margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_min_z: -2.5 # to extend the crop box min_z from ground negative: False front_lower_ground_filter: diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 1b92977330..c421fb9ae1 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -4,7 +4,11 @@ - + From 719d973638d39114d7ed69142341f6e64bea945e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 27 Jun 2024 10:15:32 +0900 Subject: [PATCH 193/233] feat(behavior_path_planner): add yaw threshold param (#1040) add yaw threshold param Signed-off-by: Daniel Sanchez --- .../static_obstacle_avoidance.param.yaml | 1 + .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 1 + .../behavior_path_planner/lane_change/lane_change.param.yaml | 1 + .../start_planner/start_planner.param.yaml | 2 ++ 4 files changed, 5 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 7e03b1c45f..05ed862539 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 @@ -178,6 +178,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index 6eeb085791..e1f8198a81 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -182,6 +182,7 @@ time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 3.1416 # temporary backward_path_length: 30.0 forward_path_length: 100.0 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 7adb3fe776..e9bbb65d3f 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 @@ -30,6 +30,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml index 9d7054487f..7b0587bd26 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 3.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 From 8680d519983b43526f397a0716e32a086b491a64 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 2 Jul 2024 11:03:03 +0900 Subject: [PATCH 194/233] chore(traffic_light_arbiter): disable signal matching Signed-off-by: Tomohito Ando --- .../traffic_light_arbiter/traffic_light_arbiter.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml index 0c23c7b81d..8af55b824d 100644 --- a/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml +++ b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml @@ -3,4 +3,4 @@ external_time_tolerance: 5.0 perception_time_tolerance: 1.0 external_priority: true - enable_signal_matching: true + enable_signal_matching: false From 4a1d744a29281b654acbbb61f4e2fcc2fafbb03a Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 5 Jul 2024 14:20:02 +0900 Subject: [PATCH 195/233] chore(tier4_perception_component): set fusion_only to true (#691) Signed-off-by: Tomohito Ando --- .../launch/components/tier4_perception_component.launch.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index fe24e836b0..5ef8ca8ca2 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -27,7 +27,6 @@ - @@ -179,7 +178,7 @@ - + From 36b01d7101e51dde1dad42ed3d25d874f32b96db Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Fri, 5 Jul 2024 15:31:43 +0900 Subject: [PATCH 196/233] feat: enable camera6 to object recognition (#692) * fix camera input offset as same as v3.0.0, and add offset for camera6 Signed-off-by: Zhi Wang * in order to use camera6, set image number as 7. Signed-off-by: Zhi Wang --------- Signed-off-by: Zhi Wang --- .../image_projection_based_fusion/fusion_common.param.yaml | 2 +- .../launch/components/tier4_perception_component.launch.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/fusion_common.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/fusion_common.param.yaml index 99d85089be..952dc434ff 100644 --- a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/fusion_common.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/fusion_common.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + input_offset_ms: [50.0, 66.67, 83.33, 0.0, 16.67, 33.33, 50.0] timeout_ms: 70.0 match_threshold_ms: 50.0 image_buffer_size: 15 diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 5ef8ca8ca2..c98357d429 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -32,6 +32,7 @@ + Date: Tue, 9 Jul 2024 11:44:47 +0900 Subject: [PATCH 197/233] feat(x2_gen1_nebula): fixed topic and container names of pointcloud subscribed by localization module (#693) * fix topic name for localization_component Signed-off-by: kotaro-hihara * fix localization pointcloud container name --------- Signed-off-by: kotaro-hihara Co-authored-by: Shohei Sakai --- .../launch/components/tier4_localization_component.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index c421fb9ae1..b6e7a6fb61 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -3,10 +3,10 @@ - + From b41c77dfef74e9d73457955a2234dffdfbe3f884 Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Wed, 10 Jul 2024 19:25:21 +0900 Subject: [PATCH 198/233] Use concat pointcloud for ndt --- .../components/tier4_localization_component.launch.xml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index b6e7a6fb61..ed0542cc77 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -3,10 +3,8 @@ - - + From ce46326f08e4d399272dfa80be5c1463da0a93c4 Mon Sep 17 00:00:00 2001 From: saka1-s Date: Wed, 10 Jul 2024 10:30:12 +0000 Subject: [PATCH 199/233] style(pre-commit): autofix --- .../launch/components/tier4_localization_component.launch.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index ed0542cc77..7dfe0a3dea 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -4,9 +4,7 @@ - + From ca01723047505a63178e34c3752597bfa0cad05c Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Wed, 10 Jul 2024 19:33:06 +0900 Subject: [PATCH 200/233] launch l4toolkit by default --- autoware_launch/launch/autoware.launch.xml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 5cf27f274f..3376d6f700 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -23,6 +23,7 @@ + @@ -132,6 +133,11 @@ + + + + + Date: Thu, 11 Jul 2024 17:24:47 +0900 Subject: [PATCH 201/233] ci: add sync-beta-upstream for v3.1.0 (#459) (#697) * ci: add sync-beta-upstream (#459) * ci: add sync-beta-upstream Signed-off-by: Shumpei Wakabayashi * style(pre-commit): autofix --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix for v3.1.0 Signed-off-by: Shumpei Wakabayashi --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/workflows/sync-beta-upstream.yaml | 32 +++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 .github/workflows/sync-beta-upstream.yaml diff --git a/.github/workflows/sync-beta-upstream.yaml b/.github/workflows/sync-beta-upstream.yaml new file mode 100644 index 0000000000..677b98804e --- /dev/null +++ b/.github/workflows/sync-beta-upstream.yaml @@ -0,0 +1,32 @@ +# This workflow is intended for the use in the repositories created by forking tier4/autoware_launch. +name: sync-beta-upstream + +on: + schedule: + - cron: 0 20 * * * + workflow_dispatch: + +jobs: + sync-tier4-upstream: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-branches + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: beta/v3.1.0 + sync-pr-branch: sync-beta-upstream + sync-target-repository: https://github.com/tier4/autoware_launch.git + sync-target-branch: beta/v0.29.0 + pr-title: "chore: sync beta upstream" + pr-labels: | + bot + sync-tier4-upstream + auto-merge-method: merge From af26983d897bd1ad5864b5b5b506aaf2e05c7105 Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Tue, 9 Jul 2024 13:01:24 +0900 Subject: [PATCH 202/233] add system_diagnotic_monitor config --- .../autoware-main.yaml | 70 ++++++++++ .../autoware-psim.yaml | 11 ++ .../system_diagnostic_monitor/control.yaml | 73 +++++++++++ .../system_diagnostic_monitor/hardware.yaml | 121 ++++++++++++++++++ .../localization.yaml | 43 +++++++ .../system/system_diagnostic_monitor/map.yaml | 16 +++ .../system_diagnostic_monitor/perception.yaml | 16 +++ .../system_diagnostic_monitor/planning.yaml | 90 +++++++++++++ .../system_diagnostic_monitor/system.yaml | 27 ++++ .../system_diagnostic_monitor/vehicle.yaml | 16 +++ 10 files changed, 483 insertions(+) create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/control.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/localization.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/map.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/perception.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/planning.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/system.yaml create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml new file mode 100644 index 0000000000..30b6ce9bad --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -0,0 +1,70 @@ +files: + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/localization.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/perception.yaml } + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/vehicle.yaml } + - { path: $(dirname)/system.yaml } + +units: + - path: /autoware/modes/stop + type: ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/local + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + - { type: link, link: /autoware/control/local } + + - path: /autoware/modes/remote + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + - { type: link, link: /autoware/control/remote } + + - path: /autoware/modes/emergency_stop + type: and + list: + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/comfortable_stop + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/modes/pull_over + type: and + list: + - { type: link, link: /autoware/map } + - { type: link, link: /autoware/localization } + - { type: link, link: /autoware/planning } + - { type: link, link: /autoware/perception } + - { type: link, link: /autoware/control } + - { type: link, link: /autoware/vehicle } + - { type: link, link: /autoware/system } + + - path: /autoware/debug/tools + type: and + list: + - { type: link, link: /autoware/system/service_log_checker } diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml new file mode 100644 index 0000000000..3d55079229 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml @@ -0,0 +1,11 @@ +files: + - { path: $(find-pkg-share system_diagnostic_monitor)/config/autoware-main.yaml } + +edits: + - { type: remove, path: /autoware/map/topic_rate_check/pointcloud_map } + - { type: remove, path: /autoware/localization/scan_matching_status } + - { type: remove, path: /autoware/localization/accuracy } + - { type: remove, path: /autoware/localization/sensor_fusion_status } + - { type: remove, path: /autoware/localization/topic_rate_check/pose_twist_fusion } + - { type: remove, path: /autoware/perception/topic_rate_check/pointcloud } + - { type: remove, path: /autoware/control/emergency_braking } diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml new file mode 100644 index 0000000000..57bf1b86c2 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -0,0 +1,73 @@ +units: + - path: /autoware/control + type: and + list: + - { type: link, link: /autoware/control/topic_rate_check/trajectory_follower } + - { type: link, link: /autoware/control/topic_rate_check/control_command } + - { type: link, link: /autoware/control/node_alive_monitoring/vehicle_cmd_gate } + - { type: link, link: /autoware/control/emergency_braking } + - { type: link, link: /autoware/control/performance_monitoring/lane_departure } + - { type: link, link: /autoware/control/performance_monitoring/trajectory_deviation } + - { type: link, link: /autoware/control/performance_monitoring/control_state } + + - path: /autoware/control/local + type: and + list: + - { type: link, link: /autoware/control/topic_rate_check/selector } + - { type: link, link: /autoware/control/topic_rate_check/local } + + - path: /autoware/control/remote + type: and + list: + - { type: link, link: /autoware/control/topic_rate_check/selector } + - { type: link, link: /autoware/control/topic_rate_check/remote } + + - path: /autoware/control/topic_rate_check/trajectory_follower + type: diag + node: topic_state_monitor_trajectory_follower_control_cmd + name: control_topic_status + + - path: /autoware/control/topic_rate_check/control_command + type: diag + node: topic_state_monitor_control_command_control_cmd + name: control_topic_status + + - path: /autoware/control/node_alive_monitoring/vehicle_cmd_gate + type: diag + node: vehicle_cmd_gate + name: heartbeat + + - path: /autoware/control/emergency_braking + type: diag + node: autonomous_emergency_braking + name: aeb_emergency_stop + + - path: /autoware/control/performance_monitoring/lane_departure + type: diag + node: lane_departure_checker_node + name: lane_departure + + - path: /autoware/control/performance_monitoring/trajectory_deviation + type: diag + node: lane_departure_checker_node + name: trajectory_deviation + + - path: /autoware/control/performance_monitoring/control_state + type: diag + node: controller_node_exe + name: control_state + + - path: /autoware/control/topic_rate_check/selector + type: diag + node: external_cmd_selector + name: heartbeat + + - path: /autoware/control/topic_rate_check/local + type: diag + node: joy_controller + name: joy_controller_connection + + - path: /autoware/control/topic_rate_check/remote + type: diag + node: external_cmd_converter + name: remote_control_topic_status diff --git a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml new file mode 100644 index 0000000000..d73f723670 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml @@ -0,0 +1,121 @@ +# TODO(Takagi, Isamu): This file is under construction. +units: + - path: /autoware/system/resources/clock/offset + diag: ": NTP Offset" + timeout: 10.0 + + - path: /autoware/system/resources/cpu/offset + diag: ": CPU Temperature" + timeout: 3.0 + + - path: /autoware/system/resources/cpu/usage + diag: ": CPU Usage" + timeout: 3.0 + + - path: /autoware/system/resources/cpu/thermal_throttling + diag: ": CPU Thermal Throttling" + timeout: 3.0 + + - path: /autoware/system/resources/cpu/frequency + diag: ": CPU Frequency" + timeout: 3.0 + + - path: /autoware/system/resources/cpu/load_average + diag: ": CPU Load Average" + timeout: 3.0 + + - path: /autoware/system/resources/gpu/temperature + diag: ": GPU Temperature" + timeout: 3.0 + + - path: /autoware/system/resources/gpu/usage + diag: ": GPU Usage" + timeout: 3.0 + + - path: /autoware/system/resources/gpu/memory_usage + diag: ": GPU Memory Usage" + timeout: 3.0 + + - path: /autoware/system/resources/gpu/thermal_throttling + diag: ": GPU Thermal Throttling" + timeout: 3.0 + + - path: /autoware/system/resources/gpu/frequency + diag: ": GPU Frequency" + timeout: 3.0 + + - path: /autoware/system/resources/memory/usage + diag: ": Memory Usage" + timeout: 3.0 + + - path: /autoware/system/resources/network/usage + diag: ": Network Usage" + timeout: 3.0 + + - path: /autoware/system/resources/network/traffic + diag: ": Network Traffic" + timeout: 3.0 + + - path: /autoware/system/resources/network/crc + diag: ": Network CRC Error" + timeout: 3.0 + + - path: /autoware/system/resources/network/packet_reassembles + diag: ": IP Packet Reassembles Failed" + timeout: 3.0 + + - path: /autoware/system/resources/storage/temperature + diag: ": HDD Temperature" + timeout: 3.0 + + - path: /autoware/system/resources/storage/recovered_error + diag: ": HDD RecoveredError" + timeout: 3.0 + + - path: /autoware/system/resources/storage/read_data_rate + diag: ": HDD ReadDataRate" + timeout: 3.0 + + - path: /autoware/system/resources/storage/write_data_rate + diag: ": HDD WriteDataRate" + timeout: 3.0 + + - path: /autoware/system/resources/storage/read_iops + diag: ": HDD ReadIOPS" + timeout: 3.0 + + - path: /autoware/system/resources/storage/write_iops + diag: ": HDD WriteIOPS" + timeout: 3.0 + + - path: /autoware/system/resources/storage/usage + diag: ": HDD Usage" + timeout: 3.0 + + - path: /autoware/system/resources/storage/power_on_hours + diag: ": HDD PowerOnHours" + timeout: 3.0 + + - path: /autoware/system/resources/storage/total_data_written + diag: ": HDD TotalDataWritten" + timeout: 3.0 + + - path: /autoware/system/resources/storage/connection + diag: ": HDD Connection" + timeout: 3.0 + + - path: /autoware/system/resources/process/high_load + diag: ": High-load" + timeout: 3.0 + + - path: /autoware/system/resources/process/high_mem + diag: ": High-mem" + timeout: 3.0 + + - path: /autoware/system/resources/process/tasks_summary + diag: ": Tasks Summary" + timeout: 3.0 + + - path: /autoware/system/resources/voltage/battery + diag: ": CMOS Battery Status" + timeout: 3.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml new file mode 100644 index 0000000000..62bf17775c --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml @@ -0,0 +1,43 @@ +units: + - path: /autoware/localization + type: short-circuit-and + list: + - type: link + link: /autoware/localization/state + - type: and + list: + - { type: link, link: /autoware/localization/topic_rate_check/transform } + - { type: link, link: /autoware/localization/topic_rate_check/pose_twist_fusion } + - { type: link, link: /autoware/localization/scan_matching_status } + - { type: link, link: /autoware/localization/accuracy } + - { type: link, link: /autoware/localization/sensor_fusion_status } + + - path: /autoware/localization/state + type: diag + node: component_state_diagnostics + name: localization_state + + - path: /autoware/localization/topic_rate_check/transform + type: diag + node: topic_state_monitor_transform_map_to_base_link + name: localization_topic_status + + - path: /autoware/localization/topic_rate_check/pose_twist_fusion + type: diag + node: topic_state_monitor_pose_twist_fusion_filter_pose + name: localization_topic_status + + - path: /autoware/localization/scan_matching_status + type: diag + node: ndt_scan_matcher + name: scan_matching_status + + - path: /autoware/localization/accuracy + type: diag + node: localization + name: localization_error_monitor + + - path: /autoware/localization/sensor_fusion_status + type: diag + node: localization + name: ekf_localizer diff --git a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml new file mode 100644 index 0000000000..231ac6eb5f --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml @@ -0,0 +1,16 @@ +units: + - path: /autoware/map + type: and + list: + - { type: link, link: /autoware/map/topic_rate_check/vector_map } + - { type: link, link: /autoware/map/topic_rate_check/pointcloud_map } + + - path: /autoware/map/topic_rate_check/vector_map + type: diag + node: topic_state_monitor_vector_map + name: map_topic_status + + - path: /autoware/map/topic_rate_check/pointcloud_map + type: diag + node: topic_state_monitor_pointcloud_map + name: map_topic_status diff --git a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml new file mode 100644 index 0000000000..24e3c4eed5 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml @@ -0,0 +1,16 @@ +units: + - path: /autoware/perception + type: and + list: + - { type: link, link: /autoware/perception/topic_rate_check/objects } + - { type: link, link: /autoware/perception/topic_rate_check/pointcloud } + + - path: /autoware/perception/topic_rate_check/objects + type: diag + node: topic_state_monitor_object_recognition_objects + name: perception_topic_status + + - path: /autoware/perception/topic_rate_check/pointcloud + type: diag + node: topic_state_monitor_obstacle_segmentation_pointcloud + name: perception_topic_status diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml new file mode 100644 index 0000000000..c403fec237 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml @@ -0,0 +1,90 @@ +units: + - path: /autoware/planning + type: short-circuit-and + list: + - type: link + link: /autoware/planning/routing/state + - type: and + list: + - { type: link, link: /autoware/planning/topic_rate_check/route } + - { type: link, link: /autoware/planning/topic_rate_check/trajectory } + - { type: link, link: /autoware/planning/trajectory_validation } + + - path: /autoware/planning/trajectory_validation + type: and + list: + - { type: link, link: /autoware/planning/trajectory_validation/finite } + - { type: link, link: /autoware/planning/trajectory_validation/interval } + - { type: link, link: /autoware/planning/trajectory_validation/curvature } + - { type: link, link: /autoware/planning/trajectory_validation/angle } + - { type: link, link: /autoware/planning/trajectory_validation/lateral_acceleration } + - { type: link, link: /autoware/planning/trajectory_validation/acceleration } + - { type: link, link: /autoware/planning/trajectory_validation/deceleration } + - { type: link, link: /autoware/planning/trajectory_validation/steering } + - { type: link, link: /autoware/planning/trajectory_validation/steering_rate } + - { type: link, link: /autoware/planning/trajectory_validation/velocity_deviation } + + - path: /autoware/planning/routing/state + type: diag + node: component_state_diagnostics + name: route_state + + - path: /autoware/planning/topic_rate_check/route + type: diag + node: topic_state_monitor_mission_planning_route + name: planning_topic_status + + - path: /autoware/planning/topic_rate_check/trajectory + type: diag + node: topic_state_monitor_scenario_planning_trajectory + name: planning_topic_status + + - path: /autoware/planning/trajectory_validation/finite + type: diag + node: planning_validator + name: trajectory_validation_finite + + - path: /autoware/planning/trajectory_validation/interval + type: diag + node: planning_validator + name: trajectory_validation_interval + + - path: /autoware/planning/trajectory_validation/curvature + type: diag + node: planning_validator + name: trajectory_validation_curvature + + - path: /autoware/planning/trajectory_validation/angle + type: diag + node: planning_validator + name: trajectory_validation_relative_angle + + - path: /autoware/planning/trajectory_validation/lateral_acceleration + type: diag + node: planning_validator + name: trajectory_validation_lateral_acceleration + + - path: /autoware/planning/trajectory_validation/acceleration + type: diag + node: planning_validator + name: trajectory_validation_acceleration + + - path: /autoware/planning/trajectory_validation/deceleration + type: diag + node: planning_validator + name: trajectory_validation_deceleration + + - path: /autoware/planning/trajectory_validation/steering + type: diag + node: planning_validator + name: trajectory_validation_steering + + - path: /autoware/planning/trajectory_validation/steering_rate + type: diag + node: planning_validator + name: trajectory_validation_steering_rate + + - path: /autoware/planning/trajectory_validation/velocity_deviation + type: diag + node: planning_validator + name: trajectory_validation_velocity_deviation diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml new file mode 100644 index 0000000000..cb96c2cd7f --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -0,0 +1,27 @@ +units: + - path: /autoware/system + type: and + list: + - { type: link, link: /autoware/system/duplicated_node_checker } + - { type: link, link: /autoware/system/topic_rate_check/emergency_control_command } + - { type: link, link: /autoware/system/emergency_stop_operation } + + - path: /autoware/system/duplicated_node_checker + type: diag + node: duplicated_node_checker + name: duplicated_node_checker + + - path: /autoware/system/service_log_checker + type: diag + node: service_log_checker + name: response_status + + - path: /autoware/system/topic_rate_check/emergency_control_command + type: diag + node: topic_state_monitor_system_emergency_control_cmd + name: system_topic_status + + - path: /autoware/system/emergency_stop_operation + type: diag + node: vehicle_cmd_gate + name: emergency_stop_operation diff --git a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml new file mode 100644 index 0000000000..e040e3c3c3 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml @@ -0,0 +1,16 @@ +units: + - path: /autoware/vehicle + type: and + list: + - { type: link, link: /autoware/vehicle/topic_rate_check/velocity } + - { type: link, link: /autoware/vehicle/topic_rate_check/steering } + + - path: /autoware/vehicle/topic_rate_check/velocity + type: diag + node: topic_state_monitor_vehicle_status_velocity_status + name: vehicle_topic_status + + - path: /autoware/vehicle/topic_rate_check/steering + type: diag + node: topic_state_monitor_vehicle_status_steering_status + name: vehicle_topic_status From cec8faa17678a61729fa046ae3343b7b20ab8396 Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Wed, 10 Jul 2024 00:39:39 +0900 Subject: [PATCH 203/233] update system diagnostic monitor config --- .../autoware-main.yaml | 91 ++-- .../system_diagnostic_monitor/control.yaml | 126 ++++-- .../system_diagnostic_monitor/hardware.yaml | 424 ++++++++++++++---- .../localization.yaml | 108 +++-- .../system/system_diagnostic_monitor/map.yaml | 54 ++- .../system_diagnostic_monitor/others.yaml | 162 +++++++ .../system_diagnostic_monitor/perception.yaml | 80 +++- .../system_diagnostic_monitor/planning.yaml | 189 ++++++-- .../system_diagnostic_monitor/system.yaml | 90 +++- .../system_diagnostic_monitor/vehicle.yaml | 82 +++- 10 files changed, 1136 insertions(+), 270 deletions(-) create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/others.yaml diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml index 30b6ce9bad..3f3b855640 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -1,70 +1,63 @@ files: - - { path: $(dirname)/map.yaml } + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/hardware.yaml } - { path: $(dirname)/localization.yaml } - - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/map.yaml } - { path: $(dirname)/perception.yaml } - - { path: $(dirname)/control.yaml } - - { path: $(dirname)/vehicle.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/others.yaml } - { path: $(dirname)/system.yaml } + - { path: $(dirname)/vehicle.yaml } units: + - path: /autoware/modes/local + type: stale + + - path: /autoware/modes/remote + type: stale + - path: /autoware/modes/stop type: ok - path: /autoware/modes/autonomous type: and list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } + - { type: link, link: /control/autonomous_available } + - { type: link, link: /hardware/autonomous_available } + - { type: link, link: /localization/autonomous_available } + - { type: link, link: /map/autonomous_available } + - { type: link, link: /perception/autonomous_available } + - { type: link, link: /planning/autonomous_available } + - { type: link, link: /others/autonomous_available } + - { type: link, link: /system/autonomous_available } + - { type: link, link: /vehicle/autonomous_available } - - path: /autoware/modes/local - type: and - list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - { type: link, link: /autoware/control/local } - - - path: /autoware/modes/remote - type: and - list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } - - { type: link, link: /autoware/control/remote } - - - path: /autoware/modes/emergency_stop + - path: /autoware/modes/pull_over type: and list: - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } + - { type: link, link: /control/pull_over_available } + - { type: link, link: /hardware/pull_over_available } + - { type: link, link: /localization/pull_over_available } + - { type: link, link: /map/pull_over_available } + - { type: link, link: /perception/pull_over_available } + - { type: link, link: /planning/pull_over_available } + - { type: link, link: /others/pull_over_available } + - { type: link, link: /system/pull_over_available } + - { type: link, link: /vehicle/pull_over_available } - path: /autoware/modes/comfortable_stop type: and list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } + - { type: link, link: /control/comfortable_stop_available } + - { type: link, link: /hardware/comfortable_stop_available } + - { type: link, link: /localization/comfortable_stop_available } + - { type: link, link: /map/comfortable_stop_available } + - { type: link, link: /perception/comfortable_stop_available } + - { type: link, link: /planning/comfortable_stop_available } + - { type: link, link: /others/comfortable_stop_available } + - { type: link, link: /system/comfortable_stop_available } + - { type: link, link: /vehicle/comfortable_stop_available } - - path: /autoware/modes/pull_over - type: and - list: - - { type: link, link: /autoware/map } - - { type: link, link: /autoware/localization } - - { type: link, link: /autoware/planning } - - { type: link, link: /autoware/perception } - - { type: link, link: /autoware/control } - - { type: link, link: /autoware/vehicle } - - { type: link, link: /autoware/system } + - path: /autoware/modes/emergency_stop + type: ok - - path: /autoware/debug/tools - type: and - list: - - { type: link, link: /autoware/system/service_log_checker } diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml index 57bf1b86c2..3fb89182fa 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -1,73 +1,123 @@ units: - - path: /autoware/control + - path: /control/autonomous_available type: and list: - - { type: link, link: /autoware/control/topic_rate_check/trajectory_follower } - - { type: link, link: /autoware/control/topic_rate_check/control_command } - - { type: link, link: /autoware/control/node_alive_monitoring/vehicle_cmd_gate } - - { type: link, link: /autoware/control/emergency_braking } - - { type: link, link: /autoware/control/performance_monitoring/lane_departure } - - { type: link, link: /autoware/control/performance_monitoring/trajectory_deviation } - - { type: link, link: /autoware/control/performance_monitoring/control_state } - - - path: /autoware/control/local + - { type: link, link: /control/emergency_stop } + - { type: link, link: /control/pull_over } + - { type: link, link: /control/comfortable_stop } + + - path: /control/pull_over_available type: and list: - - { type: link, link: /autoware/control/topic_rate_check/selector } - - { type: link, link: /autoware/control/topic_rate_check/local } + - { type: link, link: /control/emergency_stop } + - { type: link, link: /control/comfortable_stop } - - path: /autoware/control/remote + - path: /control/comfortable_stop_available type: and list: - - { type: link, link: /autoware/control/topic_rate_check/selector } - - { type: link, link: /autoware/control/topic_rate_check/remote } + - { type: link, link: /control/emergency_stop } - - path: /autoware/control/topic_rate_check/trajectory_follower - type: diag - node: topic_state_monitor_trajectory_follower_control_cmd - name: control_topic_status + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /control/emergency_stop + type: and + list: + - { type: link, link: /control/001-topic_status/control_command-error } + - { type: link, link: /control/004-lane_departure-error } + - { type: link, link: /control/005-trajectory_deviation-error } + - { type: link, link: /control/009-aeb_emergency_stop } + - { type: link, link: /control/010-max_distance_deviation-error } + - { type: link, link: /control/011-slip_detection } + + - path: /control/comfortable_stop + type: and + + - path: /control/pull_over + type: and + + - path: /control/none + type: and + list: + - { type: link, link: /control/003-gate_heartbeat } + - { type: link, link: /control/007-external_command_converter_heartbeat } + - { type: link, link: /control/008-external_command_selector_heartbeat } + + # Intermediate paths + - path: /control/001-topic_status/control_command-error + type: warn-to-ok + item: + type: link + link: /control/001-topic_status/control_command + + - path: /control/004-lane_departure-error + type: warn-to-ok + item: + type: link + link: /control/004-lane_departure - - path: /autoware/control/topic_rate_check/control_command + - path: /control/005-trajectory_deviation-error + type: warn-to-ok + item: + type: link + link: /control/005-trajectory_deviation + + - path: /control/010-max_distance_deviation-error + type: warn-to-ok + item: + type: link + link: /control/010-max_distance_deviation + + - path: /control/001-topic_status/control_command type: diag node: topic_state_monitor_control_command_control_cmd name: control_topic_status + timeout: 1.0 - - path: /autoware/control/node_alive_monitoring/vehicle_cmd_gate + - path: /control/003-gate_heartbeat type: diag node: vehicle_cmd_gate name: heartbeat + timeout: 1.0 - - path: /autoware/control/emergency_braking - type: diag - node: autonomous_emergency_braking - name: aeb_emergency_stop - - - path: /autoware/control/performance_monitoring/lane_departure + - path: /control/004-lane_departure type: diag node: lane_departure_checker_node name: lane_departure + timeout: 1.0 - - path: /autoware/control/performance_monitoring/trajectory_deviation + - path: /control/005-trajectory_deviation type: diag node: lane_departure_checker_node name: trajectory_deviation + timeout: 1.0 - - path: /autoware/control/performance_monitoring/control_state + - path: /control/007-external_command_converter_heartbeat type: diag - node: controller_node_exe - name: control_state + node: external_cmd_converter + name: remote_control_topic_status + timeout: 1.0 - - path: /autoware/control/topic_rate_check/selector + - path: /control/008-external_command_selector_heartbeat type: diag node: external_cmd_selector name: heartbeat + timeout: 1.0 - - path: /autoware/control/topic_rate_check/local + - path: /control/009-aeb_emergency_stop type: diag - node: joy_controller - name: joy_controller_connection + node: autonomous_emergency_braking + name: aeb_emergency_stop + timeout: 1.0 - - path: /autoware/control/topic_rate_check/remote + - path: /control/010-max_distance_deviation type: diag - node: external_cmd_converter - name: remote_control_topic_status + node: control_validator + name: control_validation_max_distance_deviation + timeout: 1.0 + + - path: /control/011-slip_detection + type: diag + node: slip_detector + name: slip_status + timeout: 3.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml index d73f723670..241d90bd30 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml @@ -1,121 +1,365 @@ -# TODO(Takagi, Isamu): This file is under construction. units: - - path: /autoware/system/resources/clock/offset - diag: ": NTP Offset" - timeout: 10.0 + - path: /hardware/autonomous_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } + - { type: link, link: /hardware/pull_over } + - { type: link, link: /hardware/comfortable_stop } - - path: /autoware/system/resources/cpu/offset - diag: ": CPU Temperature" - timeout: 3.0 + - path: /hardware/pull_over_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } + - { type: link, link: /hardware/comfortable_stop } - - path: /autoware/system/resources/cpu/usage - diag: ": CPU Usage" - timeout: 3.0 + - path: /hardware/comfortable_stop_available + type: and + list: + - { type: link, link: /hardware/emergency_stop } - - path: /autoware/system/resources/cpu/thermal_throttling - diag: ": CPU Thermal Throttling" - timeout: 3.0 + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /hardware/emergency_stop + type: and + list: + - { type: link, link: /hardware/cpu/002-usage-error } + - { type: link, link: /hardware/cpu/004-throttling-error } + - { type: link, link: /hardware/hdd/001-temperature-error } + - { type: link, link: /hardware/hdd/003-connection-error } + - { type: link, link: /hardware/hdd/006-soft_error-error } + - { type: link, link: /hardware/hdd/007-load-error } + - { type: link, link: /hardware/memory/001-usage-error } + - { type: link, link: /hardware/memory/002-ecc-error } + - { type: link, link: /hardware/network/003-crc-error } + - { type: link, link: /hardware/network/004-packet_reassembles-error } + - { type: link, link: /hardware/gpu/001-temperature-error } + - { type: link, link: /hardware/gpu/002-usage-error } + - { type: link, link: /hardware/gpu/003-memory-error } + - { type: link, link: /hardware/gpu/004-throttling-error } + - { type: link, link: /hardware/gpu/005-frequency-error } - - path: /autoware/system/resources/cpu/frequency - diag: ": CPU Frequency" - timeout: 3.0 + - path: /hardware/pull_over + type: and + list: + - { type: link, link: /hardware/hdd/002-usage-error } - - path: /autoware/system/resources/cpu/load_average - diag: ": CPU Load Average" - timeout: 3.0 + - path: /hardware/comfortable_stop + type: and - - path: /autoware/system/resources/gpu/temperature - diag: ": GPU Temperature" - timeout: 3.0 + - path: /hardware/none + type: and + list: + - { type: link, link: /hardware/cpu/001-temperature } + - { type: link, link: /hardware/cpu/003-load_average } + - { type: link, link: /hardware/cpu/005-frequency } + - { type: link, link: /hardware/hdd/004-total_written } + - { type: link, link: /hardware/hdd/005-warranty_period } + - { type: link, link: /hardware/network/001-usage } + - { type: link, link: /hardware/network/002-traffic } + - { type: link, link: /hardware/ntp/001-sync } + - { type: link, link: /hardware/process/001-summary } + - { type: link, link: /hardware/process/002-load } + - { type: link, link: /hardware/process/003-memory } + - { type: link, link: /hardware/bios/001-battery } + - path: /hardware/hdd/007-load + type: and + list: + - { type: link, link: /hardware/hdd/007-load/read_rate } + - { type: link, link: /hardware/hdd/007-load/read_iops } + - { type: link, link: /hardware/hdd/007-load/write_rate } + - { type: link, link: /hardware/hdd/007-load/write_iops } - - path: /autoware/system/resources/gpu/usage - diag: ": GPU Usage" - timeout: 3.0 + - path: /hardware/process/002-load + type: and + list: + - { type: link, link: /hardware/process/002-load/proc-0 } + - { type: link, link: /hardware/process/002-load/proc-1 } + - { type: link, link: /hardware/process/002-load/proc-2 } + - { type: link, link: /hardware/process/002-load/proc-3 } + - { type: link, link: /hardware/process/002-load/proc-4 } - - path: /autoware/system/resources/gpu/memory_usage - diag: ": GPU Memory Usage" - timeout: 3.0 + - path: /hardware/process/003-memory + type: and + list: + - { type: link, link: /hardware/process/003-memory/proc-0 } + - { type: link, link: /hardware/process/003-memory/proc-1 } + - { type: link, link: /hardware/process/003-memory/proc-2 } + - { type: link, link: /hardware/process/003-memory/proc-3 } + - { type: link, link: /hardware/process/003-memory/proc-4 } - - path: /autoware/system/resources/gpu/thermal_throttling - diag: ": GPU Thermal Throttling" + - path: /hardware/cpu/002-usage-error + type: warn-to-ok + item: + type: link + link: /hardware/cpu/002-usage + - path: /hardware/cpu/004-throttling-error + type: warn-to-ok + item: + type: link + link: /hardware/cpu/004-throttling + - path: /hardware/hdd/001-temperature-error + type: warn-to-ok + item: + type: link + link: /hardware/hdd/001-temperature + - path: /hardware/hdd/002-usage-error + type: warn-to-ok + item: + type: link + link: /hardware/hdd/002-usage + - path: /hardware/hdd/003-connection-error + type: warn-to-ok + item: + type: link + link: /hardware/hdd/003-connection + - path: /hardware/hdd/006-soft_error-error + type: warn-to-ok + item: + type: link + link: /hardware/hdd/006-soft_error + - path: /hardware/hdd/007-load-error + type: warn-to-ok + item: + type: link + link: /hardware/hdd/007-load + - path: /hardware/memory/001-usage-error + type: warn-to-ok + item: + type: link + link: /hardware/memory/001-usage + - path: /hardware/memory/002-ecc-error + type: warn-to-ok + item: + type: link + link: /hardware/memory/002-ecc + - path: /hardware/network/003-crc-error + type: warn-to-ok + item: + type: link + link: /hardware/network/003-crc + - path: /hardware/gpu/001-temperature-error + type: warn-to-ok + item: + type: link + link: /hardware/gpu/001-temperature + - path: /hardware/gpu/002-usage-error + type: warn-to-ok + item: + type: link + link: /hardware/gpu/002-usage + - path: /hardware/gpu/003-memory-error + type: warn-to-ok + item: + type: link + link: /hardware/gpu/003-memory + - path: /hardware/gpu/004-throttling-error + type: warn-to-ok + item: + type: link + link: /hardware/gpu/004-throttling + - path: /hardware/gpu/005-frequency-error + type: warn-to-ok + item: + type: link + link: /hardware/gpu/005-frequency + - path: /hardware/network/004-packet_reassembles-error + type: warn-to-ok + item: + type: link + link: /hardware/network/004-packet_reassembles + - path: /hardware/cpu/001-temperature + type: diag + node: cpu_monitor + name: CPU Temperature timeout: 3.0 - - - path: /autoware/system/resources/gpu/frequency - diag: ": GPU Frequency" + - path: /hardware/cpu/002-usage + type: diag + node: cpu_monitor + name: CPU Usage timeout: 3.0 - - - path: /autoware/system/resources/memory/usage - diag: ": Memory Usage" + - path: /hardware/cpu/003-load_average + type: diag + node: cpu_monitor + name: CPU Load Average timeout: 3.0 - - - path: /autoware/system/resources/network/usage - diag: ": Network Usage" + - path: /hardware/cpu/004-throttling + type: diag + node: cpu_monitor + name: CPU Thermal Throttling timeout: 3.0 - - - path: /autoware/system/resources/network/traffic - diag: ": Network Traffic" + - path: /hardware/cpu/005-frequency + type: diag + node: cpu_monitor + name: CPU Frequency timeout: 3.0 - - - path: /autoware/system/resources/network/crc - diag: ": Network CRC Error" + - path: /hardware/hdd/001-temperature + type: diag + node: hdd_monitor + name: HDD Temperature timeout: 3.0 - - - path: /autoware/system/resources/network/packet_reassembles - diag: ": IP Packet Reassembles Failed" + - path: /hardware/hdd/002-usage + type: diag + node: hdd_monitor + name: HDD Usage timeout: 3.0 - - - path: /autoware/system/resources/storage/temperature - diag: ": HDD Temperature" + - path: /hardware/hdd/003-connection + type: diag + node: hdd_monitor + name: HDD Connection timeout: 3.0 - - - path: /autoware/system/resources/storage/recovered_error - diag: ": HDD RecoveredError" + - path: /hardware/hdd/004-total_written + type: diag + node: hdd_monitor + name: HDD TotalDataWritten timeout: 3.0 - - - path: /autoware/system/resources/storage/read_data_rate - diag: ": HDD ReadDataRate" + - path: /hardware/hdd/005-warranty_period + type: diag + node: hdd_monitor + name: HDD PowerOnHours timeout: 3.0 - - - path: /autoware/system/resources/storage/write_data_rate - diag: ": HDD WriteDataRate" + - path: /hardware/hdd/006-soft_error + type: diag + node: hdd_monitor + name: HDD RecoveredError timeout: 3.0 - - - path: /autoware/system/resources/storage/read_iops - diag: ": HDD ReadIOPS" + - path: /hardware/hdd/007-load/read_rate + type: diag + node: hdd_monitor + name: HDD ReadDataRate timeout: 3.0 - - - path: /autoware/system/resources/storage/write_iops - diag: ": HDD WriteIOPS" + - path: /hardware/hdd/007-load/write_rate + type: diag + node: hdd_monitor + name: HDD WriteDataRate timeout: 3.0 - - - path: /autoware/system/resources/storage/usage - diag: ": HDD Usage" + - path: /hardware/hdd/007-load/read_iops + type: diag + node: hdd_monitor + name: HDD ReadIOPS timeout: 3.0 - - - path: /autoware/system/resources/storage/power_on_hours - diag: ": HDD PowerOnHours" + - path: /hardware/hdd/007-load/write_iops + type: diag + node: hdd_monitor + name: HDD WriteIOPS timeout: 3.0 - - - path: /autoware/system/resources/storage/total_data_written - diag: ": HDD TotalDataWritten" + - path: /hardware/memory/001-usage + type: diag + node: mem_monitor + name: Memory Usage timeout: 3.0 - - - path: /autoware/system/resources/storage/connection - diag: ": HDD Connection" + - path: /hardware/memory/002-ecc + type: diag + node: mem_monitor + name: Memory ECC timeout: 3.0 - - - path: /autoware/system/resources/process/high_load - diag: ": High-load" + - path: /hardware/network/001-usage + type: diag + node: net_monitor + name: Network Usage timeout: 3.0 - - - path: /autoware/system/resources/process/high_mem - diag: ": High-mem" + - path: /hardware/network/002-traffic + type: diag + node: net_monitor + name: Network Traffic timeout: 3.0 - - - path: /autoware/system/resources/process/tasks_summary - diag: ": Tasks Summary" + - path: /hardware/network/003-crc + type: diag + node: net_monitor + name: Network CRC Error timeout: 3.0 - - - path: /autoware/system/resources/voltage/battery - diag: ": CMOS Battery Status" + - path: /hardware/network/004-packet_reassembles + type: diag + node: net_monitor + name: IP Packet Reassembles Failed + timeout: 3.0 + - path: /hardware/ntp/001-sync + type: diag + node: ntp_monitor + name: NTP Offset + timeout: 10.0 + - path: /hardware/process/001-summary + type: diag + node: process_monitor + name: Tasks Summary + timeout: 3.0 + - path: /hardware/process/002-load/proc-0 + type: diag + node: process_monitor + name: High-load Proc[0] + timeout: 3.0 + - path: /hardware/process/002-load/proc-1 + type: diag + node: process_monitor + name: High-load Proc[1] + timeout: 3.0 + - path: /hardware/process/002-load/proc-2 + type: diag + node: process_monitor + name: High-load Proc[2] + timeout: 3.0 + - path: /hardware/process/002-load/proc-3 + type: diag + node: process_monitor + name: High-load Proc[3] + timeout: 3.0 + - path: /hardware/process/002-load/proc-4 + type: diag + node: process_monitor + name: High-load Proc[4] + timeout: 3.0 + - path: /hardware/process/003-memory/proc-0 + type: diag + node: process_monitor + name: High-mem Proc[0] + timeout: 3.0 + - path: /hardware/process/003-memory/proc-1 + type: diag + node: process_monitor + name: High-mem Proc[1] + timeout: 3.0 + - path: /hardware/process/003-memory/proc-2 + type: diag + node: process_monitor + name: High-mem Proc[2] + timeout: 3.0 + - path: /hardware/process/003-memory/proc-3 + type: diag + node: process_monitor + name: High-mem Proc[3] + timeout: 3.0 + - path: /hardware/process/003-memory/proc-4 + type: diag + node: process_monitor + name: High-mem Proc[4] + timeout: 3.0 + - path: /hardware/gpu/001-temperature + type: diag + node: gpu_monitor + name: GPU Temperature + timeout: 3.0 + - path: /hardware/gpu/002-usage + type: diag + node: gpu_monitor + name: GPU Usage + timeout: 3.0 + - path: /hardware/gpu/003-memory + type: diag + node: gpu_monitor + name: GPU Memory Usage + timeout: 3.0 + - path: /hardware/gpu/004-throttling + type: diag + node: gpu_monitor + name: GPU Thermal Throttling + timeout: 3.0 + - path: /hardware/gpu/005-frequency + type: diag + node: gpu_monitor + name: GPU Frequency timeout: 3.0 + - path: /hardware/bios/001-battery + type: diag + node: voltage_monitor + name: CMOS Battery Status + timeout: 3.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml index 62bf17775c..0cb725167f 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml @@ -1,43 +1,99 @@ units: - - path: /autoware/localization - type: short-circuit-and + - path: /localization/autonomous_available + type: and list: - - type: link - link: /autoware/localization/state - - type: and - list: - - { type: link, link: /autoware/localization/topic_rate_check/transform } - - { type: link, link: /autoware/localization/topic_rate_check/pose_twist_fusion } - - { type: link, link: /autoware/localization/scan_matching_status } - - { type: link, link: /autoware/localization/accuracy } - - { type: link, link: /autoware/localization/sensor_fusion_status } - - - path: /autoware/localization/state - type: diag - node: component_state_diagnostics - name: localization_state + - { type: link, link: /localization/emergency_stop } + - { type: link, link: /localization/pull_over } + - { type: link, link: /localization/comfortable_stop } + + - path: /localization/pull_over_available + type: and + list: + - { type: link, link: /localization/emergency_stop } + - { type: link, link: /localization/comfortable_stop } + + - path: /localization/comfortable_stop_available + type: and + list: + - { type: link, link: /localization/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /localization/emergency_stop + type: and + list: + - { type: link, link: /localization/001-topic_status/initialpose-error } + - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter-error } + - { type: link, link: /localization/002-tf-error } + - { type: link, link: /localization/003-matching_score-error } + - { type: link, link: /localization/004-accuracy-error } + + - path: /localization/comfortable_stop + type: and + + - path: /localization/pull_over + type: and + + - path: /localization/none + type: and - - path: /autoware/localization/topic_rate_check/transform + - path: /localization/001-topic_status/initialpose-error + type: warn-to-ok + item: + type: link + link: /localization/001-topic_status/initialpose + + - path: /localization/001-topic_status/pose_twist_fusion_filter-error + type: warn-to-ok + item: + type: link + link: /localization/001-topic_status/pose_twist_fusion_filter + + - path: /localization/002-tf-error + type: warn-to-ok + item: + type: link + link: /localization/002-tf + + - path: /localization/003-matching_score-error + type: warn-to-ok + item: + type: link + link: /localization/003-matching_score + + - path: /localization/004-accuracy-error + type: warn-to-ok + item: + type: link + link: /localization/004-accuracy + + - path: /localization/001-topic_status/initialpose type: diag - node: topic_state_monitor_transform_map_to_base_link + node: topic_state_monitor_initialpose3d name: localization_topic_status + timeout: 1.0 - - path: /autoware/localization/topic_rate_check/pose_twist_fusion + - path: /localization/001-topic_status/pose_twist_fusion_filter type: diag node: topic_state_monitor_pose_twist_fusion_filter_pose name: localization_topic_status + timeout: 1.0 - - path: /autoware/localization/scan_matching_status + - path: /localization/002-tf type: diag - node: ndt_scan_matcher - name: scan_matching_status + node: topic_state_monitor_transform_map_to_base_link + name: localization_topic_status + timeout: 1.0 - - path: /autoware/localization/accuracy + - path: /localization/003-matching_score type: diag node: localization - name: localization_error_monitor + name: ndt_scan_matcher + timeout: 1.0 - - path: /autoware/localization/sensor_fusion_status + - path: /localization/004-accuracy type: diag node: localization - name: ekf_localizer + name: localization_error_monitor + timeout: 1.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml index 231ac6eb5f..69e7638b65 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml @@ -1,16 +1,60 @@ units: - - path: /autoware/map + - path: /map/autonomous_available type: and list: - - { type: link, link: /autoware/map/topic_rate_check/vector_map } - - { type: link, link: /autoware/map/topic_rate_check/pointcloud_map } + - { type: link, link: /map/emergency_stop } + - { type: link, link: /map/pull_over } + - { type: link, link: /map/comfortable_stop } - - path: /autoware/map/topic_rate_check/vector_map + - path: /map/pull_over_available + type: and + list: + - { type: link, link: /map/emergency_stop } + - { type: link, link: /map/comfortable_stop } + + - path: /map/comfortable_stop_available + type: and + list: + - { type: link, link: /map/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /map/emergency_stop + type: and + list: + - { type: link, link: /map/001-topic_status/vector_map-error } + - { type: link, link: /map/001-topic_status/pointcloud_map-error } + + - path: /map/comfortable_stop + type: and + + - path: /map/pull_over + type: and + + - path: /map/none + type: and + + - path: /map/001-topic_status/vector_map-error + type: warn-to-ok + item: + type: link + link: /map/001-topic_status/vector_map + + - path: /map/001-topic_status/pointcloud_map-error + type: warn-to-ok + item: + type: link + link: /map/001-topic_status/pointcloud_map + + - path: /map/001-topic_status/vector_map type: diag node: topic_state_monitor_vector_map name: map_topic_status + timeout: 1.0 - - path: /autoware/map/topic_rate_check/pointcloud_map + - path: /map/001-topic_status/pointcloud_map type: diag node: topic_state_monitor_pointcloud_map name: map_topic_status + timeout: 1.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml new file mode 100644 index 0000000000..632d27c924 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -0,0 +1,162 @@ +units: + - path: /others/autonomous_available + type: and + list: + - { type: link, link: /others/emergency_stop } + - { type: link, link: /others/pull_over } + - { type: link, link: /others/comfortable_stop } + + - path: /others/pull_over_available + type: and + list: + - { type: link, link: /others/emergency_stop } + - { type: link, link: /others/comfortable_stop } + + - path: /others/comfortable_stop_available + type: and + list: + - { type: link, link: /others/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /others/emergency_stop + type: and + list: + - { type: link, link: /others/002-blockage_validation-error } + - { type: link, link: /others/004-concat_status } + - { type: link, link: /others/005-visibility_validation-error } + - { type: link, link: /others/012-vehicle_stuck_checker } + + - path: /others/comfortable_stop + type: and + list: + - { type: link, link: /others/010-emergency_vehicle-error } + + - path: /others/pull_over + type: and + list: + - { type: link, link: /others/011-daytime_monitor } + + - path: /others/none + type: and + + # Intermediate paths + - path: /others/002-blockage_validation + type: and + list: + - { type: link, link: /others/002-blockage_validation/front_lower } + - { type: link, link: /others/002-blockage_validation/front_upper } + - { type: link, link: /others/002-blockage_validation/left_lower } + - { type: link, link: /others/002-blockage_validation/left_upper } + - { type: link, link: /others/002-blockage_validation/right_lower } + - { type: link, link: /others/002-blockage_validation/right_upper } + - { type: link, link: /others/002-blockage_validation/rear_lower } + - { type: link, link: /others/002-blockage_validation/rear_upper } + + - path: /others/005-visibility_validation + type: and + list: + - { type: link, link: /others/005-visibility_validation/front_lower } + - { type: link, link: /others/005-visibility_validation/left_upper } + + - path: /others/002-blockage_validation-error + type: warn-to-ok + item: + type: link + link: /others/002-blockage_validation + + - path: /others/005-visibility_validation-error + type: warn-to-ok + item: + type: link + link: /others/005-visibility_validation + - path: /others/010-emergency_vehicle-error + type: warn-to-ok + item: + type: link + link: /others/010-emergency_vehicle + + - path: /others/002-blockage_validation/front_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/front_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/front_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/front_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/left_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/left_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/left_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/left_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/right_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/right_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/right_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/right_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/rear_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/rear_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/rear_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/rear_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/004-concat_status + type: diag + node: concatenate_data + name: concat_status + timeout: 1.0 + + - path: /others/005-visibility_validation/front_lower + type: diag + node: "dual_return_filter: /sensing/lidar/front_lower" + name: visibility_validation + timeout: 1.0 + + - path: /others/005-visibility_validation/left_upper + type: diag + node: "dual_return_filter: /sensing/lidar/left_upper" + name: visibility_validation + timeout: 1.0 + + - path: /others/010-emergency_vehicle + type: diag + node: emergency_vehicle_detector + name: emergency_vehicle + timeout: 3.0 + + - path: /others/011-daytime_monitor + type: diag + node: daytime_monitor + name: daytime_status + timeout: 3.0 + + - path: /others/012-vehicle_stuck_checker + type: diag + node: vehicle_stuck_checker + name: vehicle_stuck_check + timeout: 3.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml index 24e3c4eed5..5614d115c7 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml @@ -1,16 +1,86 @@ units: - - path: /autoware/perception + - path: /perception/autonomous_available type: and list: - - { type: link, link: /autoware/perception/topic_rate_check/objects } - - { type: link, link: /autoware/perception/topic_rate_check/pointcloud } + - { type: link, link: /perception/emergency_stop } + - { type: link, link: /perception/pull_over } + - { type: link, link: /perception/comfortable_stop } - - path: /autoware/perception/topic_rate_check/objects + - path: /perception/pull_over_available + type: and + list: + - { type: link, link: /perception/emergency_stop } + - { type: link, link: /perception/comfortable_stop } + + - path: /perception/comfortable_stop_available + type: and + list: + - { type: link, link: /perception/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /perception/emergency_stop + type: and + list: + - { type: link, link: /perception/001-topic_status/traffic_signals-error } + - { type: link, link: /perception/001-topic_status/objects-error } + - { type: link, link: /perception/001-topic_status/pointcloud-error } + - { type: link, link: /perception/002-detection_delay-error } + + - path: /perception/comfortable_stop + type: and + + - path: /perception/pull_over + type: and + + - path: /perception/none + type: and + + - path: /perception/001-topic_status/traffic_signals-error + type: warn-to-ok + item: + type: link + link: /perception/001-topic_status/traffic_signals + + - path: /perception/001-topic_status/objects-error + type: warn-to-ok + item: + type: link + link: /perception/001-topic_status/objects + + - path: /perception/001-topic_status/pointcloud-error + type: warn-to-ok + item: + type: link + link: /perception/001-topic_status/pointcloud + + - path: /perception/002-detection_delay-error + type: warn-to-ok + item: + type: link + link: /perception/002-detection_delay + + - path: /perception/001-topic_status/traffic_signals + type: diag + node: topic_state_monitor_traffic_light_recognition_traffic_signals + name: perception_topic_status + timeout: 1.0 + + - path: /perception/001-topic_status/objects type: diag node: topic_state_monitor_object_recognition_objects name: perception_topic_status + timeout: 1.0 - - path: /autoware/perception/topic_rate_check/pointcloud + - path: /perception/001-topic_status/pointcloud type: diag node: topic_state_monitor_obstacle_segmentation_pointcloud name: perception_topic_status + timeout: 1.0 + + - path: /perception/002-detection_delay + type: diag + node: multi_object_tracker + name: Perception delay check from original header stamp + timeout: 1.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml index c403fec237..9487fee709 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml @@ -1,90 +1,203 @@ units: - - path: /autoware/planning - type: short-circuit-and + - path: /planning/autonomous_available + type: and list: - - type: link - link: /autoware/planning/routing/state - - type: and - list: - - { type: link, link: /autoware/planning/topic_rate_check/route } - - { type: link, link: /autoware/planning/topic_rate_check/trajectory } - - { type: link, link: /autoware/planning/trajectory_validation } - - - path: /autoware/planning/trajectory_validation + - { type: link, link: /planning/emergency_stop } + - { type: link, link: /planning/pull_over } + - { type: link, link: /planning/comfortable_stop } + + - path: /planning/pull_over_available type: and list: - - { type: link, link: /autoware/planning/trajectory_validation/finite } - - { type: link, link: /autoware/planning/trajectory_validation/interval } - - { type: link, link: /autoware/planning/trajectory_validation/curvature } - - { type: link, link: /autoware/planning/trajectory_validation/angle } - - { type: link, link: /autoware/planning/trajectory_validation/lateral_acceleration } - - { type: link, link: /autoware/planning/trajectory_validation/acceleration } - - { type: link, link: /autoware/planning/trajectory_validation/deceleration } - - { type: link, link: /autoware/planning/trajectory_validation/steering } - - { type: link, link: /autoware/planning/trajectory_validation/steering_rate } - - { type: link, link: /autoware/planning/trajectory_validation/velocity_deviation } - - - path: /autoware/planning/routing/state - type: diag - node: component_state_diagnostics - name: route_state + - { type: link, link: /planning/emergency_stop } + - { type: link, link: /planning/comfortable_stop } + + - path: /planning/comfortable_stop_available + type: and + list: + - { type: link, link: /planning/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /planning/emergency_stop + type: and + list: + - { type: link, link: /planning/001-topic_status/route-error } + - { type: link, link: /planning/001-topic_status/trajectory-error } + - { type: link, link: /planning/003-trajectory_finite_validation-error } + - { type: link, link: /planning/004-trajectory_interval_validation-error } + - { type: link, link: /planning/005-trajectory_curvature_validation-error } + - { type: link, link: /planning/006-trajectory_relative_angle_validation-error } + - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation-error } + - { type: link, link: /planning/008-trajectory_acceleration_validation-error } + - { type: link, link: /planning/009-trajectory_deceleration_validation-error } + - { type: link, link: /planning/010-trajectory_steering_validation-error } + - { type: link, link: /planning/011-trajectory_steering_rate_validation-error } + - { type: link, link: /planning/012-trajectory_velocity_deviation_validation-error } + - { type: link, link: /planning/013-collision_checker-error } + + - path: /planning/comfortable_stop + type: and + + - path: /planning/pull_over + type: and + + - path: /planning/none + type: and + + - path: /planning/001-topic_status/route-error + type: warn-to-ok + item: + type: link + link: /planning/001-topic_status/route - - path: /autoware/planning/topic_rate_check/route + - path: /planning/001-topic_status/trajectory-error + type: warn-to-ok + item: + type: link + link: /planning/001-topic_status/trajectory + + - path: /planning/003-trajectory_finite_validation-error + type: warn-to-ok + item: + type: link + link: /planning/003-trajectory_finite_validation + + - path: /planning/004-trajectory_interval_validation-error + type: warn-to-ok + item: + type: link + link: /planning/004-trajectory_interval_validation + + - path: /planning/005-trajectory_curvature_validation-error + type: warn-to-ok + item: + type: link + link: /planning/005-trajectory_curvature_validation + + - path: /planning/006-trajectory_relative_angle_validation-error + type: warn-to-ok + item: + type: link + link: /planning/006-trajectory_relative_angle_validation + + - path: /planning/007-trajectory_lateral_acceleration_validation-error + type: warn-to-ok + item: + type: link + link: /planning/007-trajectory_lateral_acceleration_validation + + - path: /planning/008-trajectory_acceleration_validation-error + type: warn-to-ok + item: + type: link + link: /planning/008-trajectory_acceleration_validation + + - path: /planning/009-trajectory_deceleration_validation-error + type: warn-to-ok + item: + type: link + link: /planning/009-trajectory_deceleration_validation + + - path: /planning/010-trajectory_steering_validation-error + type: warn-to-ok + item: + type: link + link: /planning/010-trajectory_steering_validation + + - path: /planning/011-trajectory_steering_rate_validation-error + type: warn-to-ok + item: + type: link + link: /planning/011-trajectory_steering_rate_validation + + - path: /planning/012-trajectory_velocity_deviation_validation-error + type: warn-to-ok + item: + type: link + link: /planning/012-trajectory_velocity_deviation_validation + + - path: /planning/013-collision_checker-error + type: warn-to-ok + item: + type: link + link: /planning/013-collision_checker + + - path: /planning/001-topic_status/route type: diag node: topic_state_monitor_mission_planning_route name: planning_topic_status + timeout: 1.0 - - path: /autoware/planning/topic_rate_check/trajectory + - path: /planning/001-topic_status/trajectory type: diag node: topic_state_monitor_scenario_planning_trajectory name: planning_topic_status + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/finite + - path: /planning/003-trajectory_finite_validation type: diag node: planning_validator name: trajectory_validation_finite + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/interval + - path: /planning/004-trajectory_interval_validation type: diag node: planning_validator name: trajectory_validation_interval + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/curvature + - path: /planning/005-trajectory_curvature_validation type: diag node: planning_validator name: trajectory_validation_curvature + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/angle + - path: /planning/006-trajectory_relative_angle_validation type: diag node: planning_validator name: trajectory_validation_relative_angle + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/lateral_acceleration + - path: /planning/007-trajectory_lateral_acceleration_validation type: diag node: planning_validator name: trajectory_validation_lateral_acceleration + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/acceleration + - path: /planning/008-trajectory_acceleration_validation type: diag node: planning_validator name: trajectory_validation_acceleration + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/deceleration + - path: /planning/009-trajectory_deceleration_validation type: diag node: planning_validator name: trajectory_validation_deceleration + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/steering + - path: /planning/010-trajectory_steering_validation type: diag node: planning_validator name: trajectory_validation_steering + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/steering_rate + - path: /planning/011-trajectory_steering_rate_validation type: diag node: planning_validator name: trajectory_validation_steering_rate + timeout: 1.0 - - path: /autoware/planning/trajectory_validation/velocity_deviation + - path: /planning/012-trajectory_velocity_deviation_validation type: diag node: planning_validator name: trajectory_validation_velocity_deviation + timeout: 1.0 + + - path: /planning/013-collision_checker + type: diag + node: collision_checker + name: collision_check + timeout: 1.0 \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml index cb96c2cd7f..2d652d1de0 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -1,27 +1,89 @@ units: - - path: /autoware/system + - path: /system/autonomous_available type: and list: - - { type: link, link: /autoware/system/duplicated_node_checker } - - { type: link, link: /autoware/system/topic_rate_check/emergency_control_command } - - { type: link, link: /autoware/system/emergency_stop_operation } + - { type: link, link: /system/emergency_stop } + - { type: link, link: /system/pull_over } + - { type: link, link: /system/comfortable_stop } - - path: /autoware/system/duplicated_node_checker - type: diag - node: duplicated_node_checker - name: duplicated_node_checker + - path: /system/pull_over_available + type: and + list: + - { type: link, link: /system/emergency_stop } + - { type: link, link: /system/comfortable_stop } - - path: /autoware/system/service_log_checker - type: diag - node: service_log_checker - name: response_status + - path: /system/comfortable_stop_available + type: and + list: + - { type: link, link: /system/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /system/emergency_stop + type: and + list: + - { type: link, link: /system/001-topic_status-error } + - { type: link, link: /system/002-emergency_stop_operation-error } + + - path: /system/comfortable_stop + type: and + + - path: /system/pull_over + type: and + list: + - { type: link, link: /system/005-fms_connection-error } + + - path: /system/none + type: and + list: + - { type: link, link: /system/003-bagpacker_status } + - { type: link, link: /system/004-bagpacker_disk_space } + + - path: /system/001-topic_status-error + type: warn-to-ok + item: + type: link + link: /system/001-topic_status + + - path: /system/002-emergency_stop_operation-error + type: warn-to-ok + item: + type: link + link: /system/002-emergency_stop_operation + + - path: /system/005-fms_connection-error + type: warn-to-ok + item: + type: link + link: /system/005-fms_connection - - path: /autoware/system/topic_rate_check/emergency_control_command + - path: /system/001-topic_status type: diag node: topic_state_monitor_system_emergency_control_cmd name: system_topic_status + timeout: 1.0 - - path: /autoware/system/emergency_stop_operation + - path: /system/002-emergency_stop_operation type: diag node: vehicle_cmd_gate name: emergency_stop_operation + timeout: 1.0 + + - path: /system/003-bagpacker_status + type: diag + node: system + name: rosbag_status + timeout: 3.0 + + - path: /system/004-bagpacker_disk_space + type: diag + node: system + name: disk_status + timeout: 3.0 + + - path: /system/005-fms_connection + type: diag + node: system + name: edge_core_internet_connection + timeout: 10.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml index e040e3c3c3..69bd1e2ba0 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml @@ -1,16 +1,88 @@ units: - - path: /autoware/vehicle + - path: /vehicle/autonomous_available type: and list: - - { type: link, link: /autoware/vehicle/topic_rate_check/velocity } - - { type: link, link: /autoware/vehicle/topic_rate_check/steering } + - { type: link, link: /vehicle/emergency_stop } + - { type: link, link: /vehicle/pull_over } + - { type: link, link: /vehicle/comfortable_stop } - - path: /autoware/vehicle/topic_rate_check/velocity + - path: /vehicle/pull_over_available + type: and + list: + - { type: link, link: /vehicle/emergency_stop } + - { type: link, link: /vehicle/comfortable_stop } + + - path: /vehicle/comfortable_stop_available + type: and + list: + - { type: link, link: /vehicle/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /vehicle/emergency_stop + type: and + list: + - { type: link, link: /vehicle/001-topic_status/velocity-error } + - { type: link, link: /vehicle/001-topic_status/steering-error } + - { type: link, link: /vehicle/005-vehicle_heartbeat-error } + - { type: link, link: /vehicle/006-vehicle_errors-error } + + - path: /vehicle/comfortable_stop + type: and + list: + - { type: link, link: /vehicle/006-vehicle_errors } + + - path: /vehicle/pull_over + type: and + + - path: /vehicle/none + type: and + + - path: /vehicle/001-topic_status/velocity-error + type: warn-to-ok + item: + type: link + link: /vehicle/001-topic_status/velocity + + - path: /vehicle/001-topic_status/steering-error + type: warn-to-ok + item: + type: link + link: /vehicle/001-topic_status/steering + + - path: /vehicle/005-vehicle_heartbeat-error + type: warn-to-ok + item: + type: link + link: /vehicle/005-vehicle_heartbeat + + - path: /vehicle/006-vehicle_errors-error + type: warn-to-ok + item: + type: link + link: /vehicle/006-vehicle_errors + + - path: /vehicle/001-topic_status/velocity type: diag node: topic_state_monitor_vehicle_status_velocity_status name: vehicle_topic_status + timeout: 1.0 - - path: /autoware/vehicle/topic_rate_check/steering + - path: /vehicle/001-topic_status/steering type: diag node: topic_state_monitor_vehicle_status_steering_status name: vehicle_topic_status + timeout: 1.0 + + - path: /vehicle/005-vehicle_heartbeat + type: diag + node: j6_interface + name: vehicle_heartbeat_errors + timeout: 1.0 + + - path: /vehicle/006-vehicle_errors + type: diag + node: j6_interface + name: vehicle_errors + timeout: 1.0 \ No newline at end of file From dc2ec2c5ef323166270f09fd79acc5ca9b681226 Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Wed, 10 Jul 2024 22:10:33 +0900 Subject: [PATCH 204/233] remove not working diag --- .../autoware-main.yaml | 5 +- .../localization.yaml | 4 +- .../system_diagnostic_monitor/others.yaml | 159 +++++++++--------- .../system_diagnostic_monitor/system.yaml | 54 +++--- 4 files changed, 110 insertions(+), 112 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml index f35f219df6..c5f2a033fe 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml @@ -1,5 +1,2 @@ files: - - { path: $(find-pkg-share system_diagnostic_monitor)/config/autoware-main.yaml } - -edits: - - { type: remove, path: /autoware/control/emergency_braking } + - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-main.yaml } \ No newline at end of file diff --git a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml index 0cb725167f..4e47de5621 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml @@ -88,8 +88,8 @@ units: - path: /localization/003-matching_score type: diag - node: localization - name: ndt_scan_matcher + node: ndt_scan_matcher + name: scan_matching_status timeout: 1.0 - path: /localization/004-accuracy diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml index 632d27c924..42b46d8186 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -23,9 +23,9 @@ units: - path: /others/emergency_stop type: and list: - - { type: link, link: /others/002-blockage_validation-error } + # - { type: link, link: /others/002-blockage_validation-error } - { type: link, link: /others/004-concat_status } - - { type: link, link: /others/005-visibility_validation-error } + # - { type: link, link: /others/005-visibility_validation-error } - { type: link, link: /others/012-vehicle_stuck_checker } - path: /others/comfortable_stop @@ -41,89 +41,90 @@ units: - path: /others/none type: and - # Intermediate paths - - path: /others/002-blockage_validation - type: and - list: - - { type: link, link: /others/002-blockage_validation/front_lower } - - { type: link, link: /others/002-blockage_validation/front_upper } - - { type: link, link: /others/002-blockage_validation/left_lower } - - { type: link, link: /others/002-blockage_validation/left_upper } - - { type: link, link: /others/002-blockage_validation/right_lower } - - { type: link, link: /others/002-blockage_validation/right_upper } - - { type: link, link: /others/002-blockage_validation/rear_lower } - - { type: link, link: /others/002-blockage_validation/rear_upper } - - - path: /others/005-visibility_validation - type: and - list: - - { type: link, link: /others/005-visibility_validation/front_lower } - - { type: link, link: /others/005-visibility_validation/left_upper } - - - path: /others/002-blockage_validation-error - type: warn-to-ok - item: - type: link - link: /others/002-blockage_validation - - - path: /others/005-visibility_validation-error - type: warn-to-ok - item: - type: link - link: /others/005-visibility_validation - path: /others/010-emergency_vehicle-error type: warn-to-ok item: type: link link: /others/010-emergency_vehicle - - path: /others/002-blockage_validation/front_lower - type: diag - node: "blockage_return_diag: /sensing/lidar/front_lower" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/front_upper - type: diag - node: "blockage_return_diag: /sensing/lidar/front_upper" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/left_lower - type: diag - node: "blockage_return_diag: /sensing/lidar/left_lower" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/left_upper - type: diag - node: "blockage_return_diag: /sensing/lidar/left_upper" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/right_lower - type: diag - node: "blockage_return_diag: /sensing/lidar/right_lower" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/right_upper - type: diag - node: "blockage_return_diag: /sensing/lidar/right_upper" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/rear_lower - type: diag - node: "blockage_return_diag: /sensing/lidar/rear_lower" - name: blockage_validation - timeout: 1.0 - - - path: /others/002-blockage_validation/rear_upper - type: diag - node: "blockage_return_diag: /sensing/lidar/rear_upper" - name: blockage_validation - timeout: 1.0 + # # Intermediate paths + # - path: /others/002-blockage_validation-error + # type: warn-to-ok + # item: + # type: link + # link: /others/002-blockage_validation + + # - path: /others/002-blockage_validation + # type: and + # list: + # - { type: link, link: /others/002-blockage_validation/front_lower } + # - { type: link, link: /others/002-blockage_validation/front_upper } + # - { type: link, link: /others/002-blockage_validation/left_lower } + # - { type: link, link: /others/002-blockage_validation/left_upper } + # - { type: link, link: /others/002-blockage_validation/right_lower } + # - { type: link, link: /others/002-blockage_validation/right_upper } + # - { type: link, link: /others/002-blockage_validation/rear_lower } + # - { type: link, link: /others/002-blockage_validation/rear_upper } + + # - path: /others/005-visibility_validation + # type: and + # list: + # - { type: link, link: /others/005-visibility_validation/front_lower } + # - { type: link, link: /others/005-visibility_validation/left_upper } + + # - path: /others/005-visibility_validation-error + # type: warn-to-ok + # item: + # type: link + # link: /others/005-visibility_validation + + # - path: /others/002-blockage_validation/front_lower + # type: diag + # node: "blockage_return_diag: /sensing/lidar/front_lower" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/front_upper + # type: diag + # node: "blockage_return_diag: /sensing/lidar/front_upper" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/left_lower + # type: diag + # node: "blockage_return_diag: /sensing/lidar/left_lower" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/left_upper + # type: diag + # node: "blockage_return_diag: /sensing/lidar/left_upper" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/right_lower + # type: diag + # node: "blockage_return_diag: /sensing/lidar/right_lower" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/right_upper + # type: diag + # node: "blockage_return_diag: /sensing/lidar/right_upper" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/rear_lower + # type: diag + # node: "blockage_return_diag: /sensing/lidar/rear_lower" + # name: blockage_validation + # timeout: 1.0 + + # - path: /others/002-blockage_validation/rear_upper + # type: diag + # node: "blockage_return_diag: /sensing/lidar/rear_upper" + # name: blockage_validation + # timeout: 1.0 - path: /others/004-concat_status type: diag diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml index 2d652d1de0..0e15251542 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -31,14 +31,14 @@ units: - path: /system/pull_over type: and - list: - - { type: link, link: /system/005-fms_connection-error } + # list: + # - { type: link, link: /system/005-fms_connection-error } - - path: /system/none - type: and - list: - - { type: link, link: /system/003-bagpacker_status } - - { type: link, link: /system/004-bagpacker_disk_space } + # - path: /system/none + # type: and + # list: + # - { type: link, link: /system/003-bagpacker_status } + # - { type: link, link: /system/004-bagpacker_disk_space } - path: /system/001-topic_status-error type: warn-to-ok @@ -52,11 +52,11 @@ units: type: link link: /system/002-emergency_stop_operation - - path: /system/005-fms_connection-error - type: warn-to-ok - item: - type: link - link: /system/005-fms_connection + # - path: /system/005-fms_connection-error + # type: warn-to-ok + # item: + # type: link + # link: /system/005-fms_connection - path: /system/001-topic_status type: diag @@ -70,20 +70,20 @@ units: name: emergency_stop_operation timeout: 1.0 - - path: /system/003-bagpacker_status - type: diag - node: system - name: rosbag_status - timeout: 3.0 + # - path: /system/003-bagpacker_status + # type: diag + # node: system + # name: rosbag_status + # timeout: 3.0 - - path: /system/004-bagpacker_disk_space - type: diag - node: system - name: disk_status - timeout: 3.0 + # - path: /system/004-bagpacker_disk_space + # type: diag + # node: system + # name: disk_status + # timeout: 3.0 - - path: /system/005-fms_connection - type: diag - node: system - name: edge_core_internet_connection - timeout: 10.0 + # - path: /system/005-fms_connection + # type: diag + # node: system + # name: edge_core_internet_connection + # timeout: 10.0 From bd151bb5f1a05d40a3a689e6f788577690962b50 Mon Sep 17 00:00:00 2001 From: "shohei.sakai" Date: Thu, 11 Jul 2024 18:44:23 +0900 Subject: [PATCH 205/233] remove unsupported diag --- .../system_diagnostic_monitor/control.yaml | 12 ++--- .../system_diagnostic_monitor/others.yaml | 44 +++++++++---------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml index 3fb89182fa..06a3f77863 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -26,7 +26,7 @@ units: - { type: link, link: /control/001-topic_status/control_command-error } - { type: link, link: /control/004-lane_departure-error } - { type: link, link: /control/005-trajectory_deviation-error } - - { type: link, link: /control/009-aeb_emergency_stop } + # - { type: link, link: /control/009-aeb_emergency_stop } - { type: link, link: /control/010-max_distance_deviation-error } - { type: link, link: /control/011-slip_detection } @@ -104,11 +104,11 @@ units: name: heartbeat timeout: 1.0 - - path: /control/009-aeb_emergency_stop - type: diag - node: autonomous_emergency_braking - name: aeb_emergency_stop - timeout: 1.0 + # - path: /control/009-aeb_emergency_stop + # type: diag + # node: autonomous_emergency_braking + # name: aeb_emergency_stop + # timeout: 1.0 - path: /control/010-max_distance_deviation type: diag diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml index 42b46d8186..a54272fca2 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -30,8 +30,8 @@ units: - path: /others/comfortable_stop type: and - list: - - { type: link, link: /others/010-emergency_vehicle-error } + # list: + # - { type: link, link: /others/010-emergency_vehicle-error } - path: /others/pull_over type: and @@ -41,11 +41,11 @@ units: - path: /others/none type: and - - path: /others/010-emergency_vehicle-error - type: warn-to-ok - item: - type: link - link: /others/010-emergency_vehicle + # - path: /others/010-emergency_vehicle-error + # type: warn-to-ok + # item: + # type: link + # link: /others/010-emergency_vehicle # # Intermediate paths # - path: /others/002-blockage_validation-error @@ -132,23 +132,23 @@ units: name: concat_status timeout: 1.0 - - path: /others/005-visibility_validation/front_lower - type: diag - node: "dual_return_filter: /sensing/lidar/front_lower" - name: visibility_validation - timeout: 1.0 + # - path: /others/005-visibility_validation/front_lower + # type: diag + # node: "dual_return_filter: /sensing/lidar/front_lower" + # name: visibility_validation + # timeout: 1.0 - - path: /others/005-visibility_validation/left_upper - type: diag - node: "dual_return_filter: /sensing/lidar/left_upper" - name: visibility_validation - timeout: 1.0 + # - path: /others/005-visibility_validation/left_upper + # type: diag + # node: "dual_return_filter: /sensing/lidar/left_upper" + # name: visibility_validation + # timeout: 1.0 - - path: /others/010-emergency_vehicle - type: diag - node: emergency_vehicle_detector - name: emergency_vehicle - timeout: 3.0 + # - path: /others/010-emergency_vehicle + # type: diag + # node: emergency_vehicle_detector + # name: emergency_vehicle + # timeout: 3.0 - path: /others/011-daytime_monitor type: diag From 848f042dd158a2c0b4b105f8d8d091646086a69e Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 12 Jul 2024 14:05:11 +0900 Subject: [PATCH 206/233] remove unused parameter file in diagnostic_graph_aggregator --- .../autoware-awsim.yaml | 1 - .../autoware-psim.yaml | 5 +- .../autoware.planning_simulator.yaml | 59 --- .../diagnostic_graph_aggregator/autoware.yaml | 68 ---- .../diagnostic_graph_aggregator/control.yaml | 111 ------ .../diagnostic_graph_aggregator/hardware.yaml | 355 ------------------ .../localization.yaml | 91 ----- .../diagnostic_graph_aggregator/map.yaml | 58 --- .../diagnostic_graph_aggregator/others.yaml | 140 ------- .../perception.yaml | 80 ---- .../diagnostic_graph_aggregator/planning.yaml | 184 --------- .../diagnostic_graph_aggregator/sensing.yaml | 296 --------------- .../diagnostic_graph_aggregator/system.yaml | 83 ---- .../diagnostic_graph_aggregator/vehicle.yaml | 82 ---- .../launch/planning_simulator.launch.xml | 2 +- 15 files changed, 2 insertions(+), 1613 deletions(-) delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml delete mode 100644 autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml index 7f002b1380..9950acbc7e 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-awsim.yaml @@ -3,4 +3,3 @@ files: edits: - { type: remove, path: /autoware/system/duplicated_node_checker } - - { type: remove, path: /autoware/control/emergency_braking } diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml index e11f391606..1b6060f84c 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml @@ -1,5 +1,2 @@ files: - - { path: $(find-pkg-share system_diagnostic_monitor)/config/autoware-psim.yaml } - -edits: - - { type: remove, path: /autoware/control/emergency_braking } + - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-psim.yaml } \ No newline at end of file diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml deleted file mode 100644 index c212f19ede..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.planning_simulator.yaml +++ /dev/null @@ -1,59 +0,0 @@ -files: - - { path: $(dirname)/map.yaml } - - { path: $(dirname)/planning.yaml } - - { path: $(dirname)/control.yaml } - - { path: $(dirname)/system.yaml } - -# Exclude paths that does not need on planning simulator -edits: - - path: /map/001-topic_status/pointcloud_map-error - type: remove - - path: /map/001-topic_status/pointcloud_map - type: remove - - path: /system/003-bagpacker_status - type: remove - - path: /system/004-bagpacker_disk_space - type: remove - - path: /system/005-fms_connection - type: remove - - path: /system/005-fms_connection-error - type: remove - -nodes: - - path: /autoware/modes/local - type: stale - - - path: /autoware/modes/remote - type: stale - - # Stop is always available - - path: /autoware/modes/stop - type: ok - - - path: /autoware/modes/autonomous - type: and - list: - - { type: link, link: /map/autonomous_available } - - { type: link, link: /planning/autonomous_available } - - { type: link, link: /control/autonomous_available } - - { type: link, link: /system/autonomous_available } - - - path: /autoware/modes/pull_over - type: and - list: - - { type: link, link: /map/pull_over_available } - - { type: link, link: /planning/pull_over_available } - - { type: link, link: /control/pull_over_available } - - { type: link, link: /system/pull_over_available } - - - path: /autoware/modes/comfortable_stop - type: and - list: - - { type: link, link: /map/comfortable_stop_available } - - { type: link, link: /planning/comfortable_stop_available } - - { type: link, link: /control/comfortable_stop_available } - - { type: link, link: /system/comfortable_stop_available } - - # Emergency stop is always available - - path: /autoware/modes/emergency_stop - type: ok diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml deleted file mode 100644 index ee16bc9819..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware.yaml +++ /dev/null @@ -1,68 +0,0 @@ -files: - - { path: $(dirname)/hardware.yaml } - - { path: $(dirname)/sensing.yaml } - - { path: $(dirname)/map.yaml } - - { path: $(dirname)/localization.yaml } - - { path: $(dirname)/planning.yaml } - - { path: $(dirname)/perception.yaml } - - { path: $(dirname)/control.yaml } - - { path: $(dirname)/vehicle.yaml } - - { path: $(dirname)/system.yaml } - - { path: $(dirname)/others.yaml } - -nodes: - - path: /autoware/modes/local - type: stale - - - path: /autoware/modes/remote - type: stale - - # Stop is always available - - path: /autoware/modes/stop - type: ok - - - path: /autoware/modes/autonomous - type: and - list: - - { type: link, link: /hardware/autonomous_available } - - { type: link, link: /sensing/autonomous_available } - - { type: link, link: /map/autonomous_available } - - { type: link, link: /localization/autonomous_available } - - { type: link, link: /planning/autonomous_available } - - { type: link, link: /perception/autonomous_available } - - { type: link, link: /control/autonomous_available } - - { type: link, link: /vehicle/autonomous_available } - - { type: link, link: /system/autonomous_available } - - { type: link, link: /others/autonomous_available } - - - path: /autoware/modes/pull_over - type: and - list: - - { type: link, link: /hardware/pull_over_available } - - { type: link, link: /sensing/pull_over_available } - - { type: link, link: /map/pull_over_available } - - { type: link, link: /localization/pull_over_available } - - { type: link, link: /planning/pull_over_available } - - { type: link, link: /perception/pull_over_available } - - { type: link, link: /control/pull_over_available } - - { type: link, link: /vehicle/pull_over_available } - - { type: link, link: /system/pull_over_available } - - { type: link, link: /others/pull_over_available } - - - path: /autoware/modes/comfortable_stop - type: and - list: - - { type: link, link: /hardware/comfortable_stop_available } - - { type: link, link: /sensing/comfortable_stop_available } - - { type: link, link: /map/comfortable_stop_available } - - { type: link, link: /localization/comfortable_stop_available } - - { type: link, link: /planning/comfortable_stop_available } - - { type: link, link: /perception/comfortable_stop_available } - - { type: link, link: /control/comfortable_stop_available } - - { type: link, link: /vehicle/comfortable_stop_available } - - { type: link, link: /system/comfortable_stop_available } - - { type: link, link: /others/comfortable_stop_available } - - # Emergency stop is always available - - path: /autoware/modes/emergency_stop - type: ok diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml deleted file mode 100644 index 432781f0ba..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/control.yaml +++ /dev/null @@ -1,111 +0,0 @@ -nodes: - - path: /control/autonomous_available - type: and - list: - - { type: link, link: /control/emergency_stop } - - { type: link, link: /control/pull_over } - - { type: link, link: /control/comfortable_stop } - - - path: /control/pull_over_available - type: and - list: - - { type: link, link: /control/emergency_stop } - - { type: link, link: /control/comfortable_stop } - - - path: /control/comfortable_stop_available - type: and - list: - - { type: link, link: /control/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /control/emergency_stop - type: and - list: - - { type: link, link: /control/001-topic_status/control_command-error } - - { type: link, link: /control/004-lane_departure-error } - - { type: link, link: /control/005-trajectory_deviation-error } - - { type: link, link: /control/009-aeb_emergency_stop } - - { type: link, link: /control/010-max_distance_deviation-error } - - { type: link, link: /control/011-slip_detection } - - - path: /control/comfortable_stop - type: and - - - path: /control/pull_over - type: and - - - path: /control/none - type: and - list: - - { type: link, link: /control/003-gate_heartbeat } - - { type: link, link: /control/007-external_command_converter_heartbeat } - - { type: link, link: /control/008-external_command_selector_heartbeat } - - # Intermediate paths - - path: /control/001-topic_status/control_command-error - type: warn-to-ok - list: - - { type: link, link: /control/001-topic_status/control_command } - - - path: /control/004-lane_departure-error - type: warn-to-ok - list: - - { type: link, link: /control/004-lane_departure } - - - path: /control/005-trajectory_deviation-error - type: warn-to-ok - list: - - { type: link, link: /control/005-trajectory_deviation } - - - path: /control/010-max_distance_deviation-error - type: warn-to-ok - list: - - { type: link, link: /control/010-max_distance_deviation } - - # Diagnostics paths - - path: /control/001-topic_status/control_command - diag: "topic_state_monitor_control_command_control_cmd: control_topic_status" - type: diag - timeout: 1.0 - - - path: /control/003-gate_heartbeat - diag: "vehicle_cmd_gate: heartbeat" - type: diag - timeout: 1.0 - - - path: /control/004-lane_departure - diag: "lane_departure_checker_node: lane_departure" - type: diag - timeout: 1.0 - - - path: /control/005-trajectory_deviation - diag: "lane_departure_checker_node: trajectory_deviation" - type: diag - timeout: 1.0 - - - path: /control/007-external_command_converter_heartbeat - diag: "external_cmd_converter: remote_control_topic_status" - type: diag - timeout: 1.0 - - - path: /control/008-external_command_selector_heartbeat - diag: "external_cmd_selector: heartbeat" - type: diag - timeout: 1.0 - - - path: /control/009-aeb_emergency_stop - diag: "autonomous_emergency_braking: aeb_emergency_stop" - type: diag - timeout: 1.0 - - - path: /control/010-max_distance_deviation - diag: "control_validator: control_validation_max_distance_deviation" - type: diag - timeout: 1.0 - - - path: /control/011-slip_detection - diag: "slip_detector: slip_status" - type: diag - timeout: 3.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml deleted file mode 100644 index 54e909c426..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/hardware.yaml +++ /dev/null @@ -1,355 +0,0 @@ -nodes: - - path: /hardware/autonomous_available - type: and - list: - - { type: link, link: /hardware/emergency_stop } - - { type: link, link: /hardware/pull_over } - - { type: link, link: /hardware/comfortable_stop } - - - path: /hardware/pull_over_available - type: and - list: - - { type: link, link: /hardware/emergency_stop } - - { type: link, link: /hardware/comfortable_stop } - - - path: /hardware/comfortable_stop_available - type: and - list: - - { type: link, link: /hardware/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /hardware/emergency_stop - type: and - list: - - { type: link, link: /hardware/cpu/002-usage-error } - - { type: link, link: /hardware/cpu/004-throttling-error } - - { type: link, link: /hardware/hdd/001-temperature-error } - - { type: link, link: /hardware/hdd/003-connection-error } - - { type: link, link: /hardware/hdd/006-soft_error-error } - - { type: link, link: /hardware/hdd/007-load-error } - - { type: link, link: /hardware/memory/001-usage-error } - - { type: link, link: /hardware/memory/002-ecc-error } - - { type: link, link: /hardware/network/003-crc-error } - - { type: link, link: /hardware/network/004-packet_reassembles-error } - - { type: link, link: /hardware/gpu/001-temperature-error } - - { type: link, link: /hardware/gpu/002-usage-error } - - { type: link, link: /hardware/gpu/003-memory-error } - - { type: link, link: /hardware/gpu/004-throttling-error } - - { type: link, link: /hardware/gpu/005-frequency-error } - - - path: /hardware/pull_over - type: and - list: - - { type: link, link: /hardware/hdd/002-usage-error } - - { type: link, link: /hardware/mot/001-connection } - - { type: link, link: /hardware/signage/001-connection } - - { type: link, link: /hardware/voice/001-connection } - - - path: /hardware/comfortable_stop - type: and - - - path: /hardware/none - type: and - list: - - { type: link, link: /hardware/cpu/001-temperature } - - { type: link, link: /hardware/cpu/003-load_average } - - { type: link, link: /hardware/cpu/005-frequency } - - { type: link, link: /hardware/hdd/004-total_written } - - { type: link, link: /hardware/hdd/005-warranty_period } - - { type: link, link: /hardware/network/001-usage } - - { type: link, link: /hardware/network/002-traffic } - - { type: link, link: /hardware/ntp/001-sync } - - { type: link, link: /hardware/process/001-summary } - - { type: link, link: /hardware/process/002-load } - - { type: link, link: /hardware/process/003-memory } - - { type: link, link: /hardware/bios/001-battery } - - # Intermediate paths - - path: /hardware/cpu/002-usage-error - type: warn-to-ok - list: - - { type: link, link: /hardware/cpu/002-usage } - - - path: /hardware/cpu/004-throttling-error - type: warn-to-ok - list: - - { type: link, link: /hardware/cpu/004-throttling } - - - path: /hardware/hdd/001-temperature-error - type: warn-to-ok - list: - - { type: link, link: /hardware/hdd/001-temperature } - - - path: /hardware/hdd/002-usage-error - type: warn-to-ok - list: - - { type: link, link: /hardware/hdd/002-usage } - - - path: /hardware/hdd/003-connection-error - type: warn-to-ok - list: - - { type: link, link: /hardware/hdd/003-connection } - - - path: /hardware/hdd/006-soft_error-error - type: warn-to-ok - list: - - { type: link, link: /hardware/hdd/006-soft_error } - - - path: /hardware/hdd/007-load-error - type: warn-to-ok - list: - - { type: link, link: /hardware/hdd/007-load } - - - path: /hardware/memory/001-usage-error - type: warn-to-ok - list: - - { type: link, link: /hardware/memory/001-usage } - - - path: /hardware/memory/002-ecc-error - type: warn-to-ok - list: - - { type: link, link: /hardware/memory/002-ecc } - - - path: /hardware/network/003-crc-error - type: warn-to-ok - list: - - { type: link, link: /hardware/network/003-crc } - - - path: /hardware/gpu/001-temperature-error - type: warn-to-ok - list: - - { type: link, link: /hardware/gpu/001-temperature } - - - path: /hardware/gpu/002-usage-error - type: warn-to-ok - list: - - { type: link, link: /hardware/gpu/002-usage } - - - path: /hardware/gpu/003-memory-error - type: warn-to-ok - list: - - { type: link, link: /hardware/gpu/003-memory } - - - path: /hardware/gpu/004-throttling-error - type: warn-to-ok - list: - - { type: link, link: /hardware/gpu/004-throttling } - - - path: /hardware/gpu/005-frequency-error - type: warn-to-ok - list: - - { type: link, link: /hardware/gpu/005-frequency } - - - path: /hardware/network/004-packet_reassembles-error - type: warn-to-ok - list: - - { type: link, link: /hardware/network/004-packet_reassembles } - - - path: /hardware/hdd/007-load - type: and - list: - - { type: link, link: /hardware/hdd/007-load/read_rate } - - { type: link, link: /hardware/hdd/007-load/read_iops } - - { type: link, link: /hardware/hdd/007-load/write_rate } - - { type: link, link: /hardware/hdd/007-load/write_iops } - - - path: /hardware/process/002-load - type: and - list: - - { type: link, link: /hardware/process/002-load/proc-0 } - - { type: link, link: /hardware/process/002-load/proc-1 } - - { type: link, link: /hardware/process/002-load/proc-2 } - - { type: link, link: /hardware/process/002-load/proc-3 } - - { type: link, link: /hardware/process/002-load/proc-4 } - - - path: /hardware/process/003-memory - type: and - list: - - { type: link, link: /hardware/process/003-memory/proc-0 } - - { type: link, link: /hardware/process/003-memory/proc-1 } - - { type: link, link: /hardware/process/003-memory/proc-2 } - - { type: link, link: /hardware/process/003-memory/proc-3 } - - { type: link, link: /hardware/process/003-memory/proc-4 } - - # Diagnostics paths - - path: /hardware/cpu/001-temperature - diag: "cpu_monitor: CPU Temperature" - type: diag - timeout: 3.0 - - path: /hardware/cpu/002-usage - diag: "cpu_monitor: CPU Usage" - type: diag - timeout: 3.0 - - path: /hardware/cpu/003-load_average - diag: "cpu_monitor: CPU Load Average" - type: diag - timeout: 3.0 - - path: /hardware/cpu/004-throttling - diag: "cpu_monitor: CPU Thermal Throttling" - type: diag - timeout: 3.0 - - path: /hardware/cpu/005-frequency - diag: "cpu_monitor: CPU Frequency" - type: diag - timeout: 3.0 - - - path: /hardware/hdd/001-temperature - diag: "hdd_monitor: HDD Temperature" - type: diag - timeout: 3.0 - - path: /hardware/hdd/002-usage - diag: "hdd_monitor: HDD Usage" - type: diag - timeout: 3.0 - - path: /hardware/hdd/003-connection - diag: "hdd_monitor: HDD Connection" - type: diag - timeout: 3.0 - - path: /hardware/hdd/004-total_written - diag: "hdd_monitor: HDD TotalDataWritten" - type: diag - timeout: 3.0 - - path: /hardware/hdd/005-warranty_period - diag: "hdd_monitor: HDD PowerOnHours" - type: diag - timeout: 3.0 - - path: /hardware/hdd/006-soft_error - diag: "hdd_monitor: HDD RecoveredError" - type: diag - timeout: 3.0 - - path: /hardware/hdd/007-load/read_rate - diag: "hdd_monitor: HDD ReadDataRate" - type: diag - timeout: 3.0 - - path: /hardware/hdd/007-load/write_rate - diag: "hdd_monitor: HDD WriteDataRate" - type: diag - timeout: 3.0 - - path: /hardware/hdd/007-load/read_iops - diag: "hdd_monitor: HDD ReadIOPS" - type: diag - timeout: 3.0 - - path: /hardware/hdd/007-load/write_iops - diag: "hdd_monitor: HDD WriteIOPS" - type: diag - timeout: 3.0 - - - path: /hardware/memory/001-usage - diag: "mem_monitor: Memory Usage" - type: diag - timeout: 3.0 - - path: /hardware/memory/002-ecc - diag: "mem_monitor: Memory ECC" - type: diag - timeout: 3.0 - - - path: /hardware/network/001-usage - diag: "net_monitor: Network Usage" - type: diag - timeout: 3.0 - - path: /hardware/network/002-traffic - diag: "net_monitor: Network Traffic" - type: diag - timeout: 3.0 - - path: /hardware/network/003-crc - diag: "net_monitor: Network CRC Error" - type: diag - timeout: 3.0 - - path: /hardware/network/004-packet_reassembles - diag: "net_monitor: IP Packet Reassembles Failed" - type: diag - timeout: 3.0 - - - path: /hardware/ntp/001-sync - diag: "ntp_monitor: NTP Offset" - type: diag - timeout: 10.0 - - - path: /hardware/process/001-summary - diag: "process_monitor: Tasks Summary" - type: diag - timeout: 3.0 - - path: /hardware/process/002-load/proc-0 - diag: "process_monitor: High-load Proc[0]" - type: diag - timeout: 3.0 - - path: /hardware/process/002-load/proc-1 - diag: "process_monitor: High-load Proc[1]" - type: diag - timeout: 3.0 - - path: /hardware/process/002-load/proc-2 - diag: "process_monitor: High-load Proc[2]" - type: diag - timeout: 3.0 - - path: /hardware/process/002-load/proc-3 - diag: "process_monitor: High-load Proc[3]" - type: diag - timeout: 3.0 - - path: /hardware/process/002-load/proc-4 - diag: "process_monitor: High-load Proc[4]" - type: diag - timeout: 3.0 - - path: /hardware/process/003-memory/proc-0 - diag: "process_monitor: High-mem Proc[0]" - type: diag - timeout: 3.0 - - path: /hardware/process/003-memory/proc-1 - diag: "process_monitor: High-mem Proc[1]" - type: diag - timeout: 3.0 - - path: /hardware/process/003-memory/proc-2 - diag: "process_monitor: High-mem Proc[2]" - type: diag - timeout: 3.0 - - path: /hardware/process/003-memory/proc-3 - diag: "process_monitor: High-mem Proc[3]" - type: diag - timeout: 3.0 - - path: /hardware/process/003-memory/proc-4 - diag: "process_monitor: High-mem Proc[4]" - type: diag - timeout: 3.0 - - - path: /hardware/gpu/001-temperature - diag: "gpu_monitor: GPU Temperature" - type: diag - timeout: 3.0 - - path: /hardware/gpu/002-usage - diag: "gpu_monitor: GPU Usage" - type: diag - timeout: 3.0 - - path: /hardware/gpu/003-memory - diag: "gpu_monitor: GPU Memory Usage" - type: diag - timeout: 3.0 - - path: /hardware/gpu/004-throttling - diag: "gpu_monitor: GPU Thermal Throttling" - type: diag - timeout: 3.0 - - path: /hardware/gpu/005-frequency - diag: "gpu_monitor: GPU Frequency" - type: diag - timeout: 3.0 - - - path: /hardware/bios/001-battery - diag: "voltage_monitor: CMOS Battery Status" - type: diag - timeout: 3.0 - - # We assume these diagnostics become only OK or STALE - - path: /hardware/mot/001-connection - diag: "mot: /system/mot_connection : mot heartbeat" - type: diag - timeout: 5.0 - - - path: /hardware/signage/001-connection - diag: "signage: /system/signage_connection : signage heartbeat" - type: diag - timeout: 5.0 - - - path: /hardware/voice/001-connection - diag: "vehicle_voice_alert_system: /system/voice_alert_system_connection : voice alert system heartbeat" - type: diag - timeout: 5.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml deleted file mode 100644 index fd3c75c85f..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/localization.yaml +++ /dev/null @@ -1,91 +0,0 @@ -nodes: - - path: /localization/autonomous_available - type: and - list: - - { type: link, link: /localization/emergency_stop } - - { type: link, link: /localization/pull_over } - - { type: link, link: /localization/comfortable_stop } - - - path: /localization/pull_over_available - type: and - list: - - { type: link, link: /localization/emergency_stop } - - { type: link, link: /localization/comfortable_stop } - - - path: /localization/comfortable_stop_available - type: and - list: - - { type: link, link: /localization/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /localization/emergency_stop - type: and - list: - - { type: link, link: /localization/001-topic_status/initialpose-error } - - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter-error } - - { type: link, link: /localization/002-tf-error } - - { type: link, link: /localization/003-matching_score-error } - - { type: link, link: /localization/004-accuracy-error } - - - path: /localization/comfortable_stop - type: and - - - path: /localization/pull_over - type: and - - - path: /localization/none - type: and - - # Intermediate paths - - path: /localization/001-topic_status/initialpose-error - type: warn-to-ok - list: - - { type: link, link: /localization/001-topic_status/initialpose } - - - path: /localization/001-topic_status/pose_twist_fusion_filter-error - type: warn-to-ok - list: - - { type: link, link: /localization/001-topic_status/pose_twist_fusion_filter } - - - path: /localization/002-tf-error - type: warn-to-ok - list: - - { type: link, link: /localization/002-tf } - - - path: /localization/003-matching_score-error - type: warn-to-ok - list: - - { type: link, link: /localization/003-matching_score } - - - path: /localization/004-accuracy-error - type: warn-to-ok - list: - - { type: link, link: /localization/004-accuracy } - - # Diagnostics paths - - path: /localization/001-topic_status/initialpose - diag: "topic_state_monitor_initialpose3d: localization_topic_status" - type: diag - timeout: 1.0 - - - path: /localization/001-topic_status/pose_twist_fusion_filter - diag: "topic_state_monitor_pose_twist_fusion_filter_pose: localization_topic_status" - type: diag - timeout: 1.0 - - - path: /localization/002-tf - diag: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" - type: diag - timeout: 1.0 - - - path: /localization/003-matching_score - diag: ndt_scan_matcher - type: diag - timeout: 1.0 - - - path: /localization/004-accuracy - diag: "localization: localization_error_monitor" - type: diag - timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml deleted file mode 100644 index 4c2aa69c0e..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/map.yaml +++ /dev/null @@ -1,58 +0,0 @@ -nodes: - - path: /map/autonomous_available - type: and - list: - - { type: link, link: /map/emergency_stop } - - { type: link, link: /map/pull_over } - - { type: link, link: /map/comfortable_stop } - - - path: /map/pull_over_available - type: and - list: - - { type: link, link: /map/emergency_stop } - - { type: link, link: /map/comfortable_stop } - - - path: /map/comfortable_stop_available - type: and - list: - - { type: link, link: /map/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /map/emergency_stop - type: and - list: - - { type: link, link: /map/001-topic_status/vector_map-error } - - { type: link, link: /map/001-topic_status/pointcloud_map-error } - - - path: /map/comfortable_stop - type: and - - - path: /map/pull_over - type: and - - - path: /map/none - type: and - - # Intermediate paths - - path: /map/001-topic_status/vector_map-error - type: warn-to-ok - list: - - { type: link, link: /map/001-topic_status/vector_map } - - - path: /map/001-topic_status/pointcloud_map-error - type: warn-to-ok - list: - - { type: link, link: /map/001-topic_status/pointcloud_map } - - # Diagnostics paths - - path: /map/001-topic_status/vector_map - diag: "topic_state_monitor_vector_map: map_topic_status" - type: diag - timeout: 1.0 - - - path: /map/001-topic_status/pointcloud_map - diag: "topic_state_monitor_pointcloud_map: map_topic_status" - type: diag - timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml deleted file mode 100644 index e47df545e5..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/others.yaml +++ /dev/null @@ -1,140 +0,0 @@ -nodes: - - path: /others/autonomous_available - type: and - list: - - { type: link, link: /others/emergency_stop } - - { type: link, link: /others/pull_over } - - { type: link, link: /others/comfortable_stop } - - - path: /others/pull_over_available - type: and - list: - - { type: link, link: /others/emergency_stop } - - { type: link, link: /others/comfortable_stop } - - - path: /others/comfortable_stop_available - type: and - list: - - { type: link, link: /others/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /others/emergency_stop - type: and - list: - - { type: link, link: /others/002-blockage_validation-error } - - { type: link, link: /others/004-concat_status } - - { type: link, link: /others/005-visibility_validation-error } - - { type: link, link: /others/012-vehicle_stuck_checker } - - - path: /others/comfortable_stop - type: and - list: - - { type: link, link: /others/010-emergency_vehicle-error } - - - path: /others/pull_over - type: and - list: - - { type: link, link: /others/011-daytime_monitor } - - - path: /others/none - type: and - - # Intermediate paths - - path: /others/002-blockage_validation - type: and - list: - - { type: link, link: /others/002-blockage_validation/front_lower } - - { type: link, link: /others/002-blockage_validation/front_upper } - - { type: link, link: /others/002-blockage_validation/left_lower } - - { type: link, link: /others/002-blockage_validation/left_upper } - - { type: link, link: /others/002-blockage_validation/right_lower } - - { type: link, link: /others/002-blockage_validation/right_upper } - - { type: link, link: /others/002-blockage_validation/rear_lower } - - { type: link, link: /others/002-blockage_validation/rear_upper } - - - path: /others/005-visibility_validation - type: and - list: - - { type: link, link: /others/005-visibility_validation/front_lower } - - { type: link, link: /others/005-visibility_validation/left_upper } - - - path: /others/002-blockage_validation-error - type: warn-to-ok - list: - - { type: link, link: /others/002-blockage_validation } - - - path: /others/005-visibility_validation-error - type: warn-to-ok - list: - - { type: link, link: /others/005-visibility_validation } - - - path: /others/010-emergency_vehicle-error - type: warn-to-ok - list: - - { type: link, link: /others/010-emergency_vehicle } - - # Diagnostics paths - - path: /others/002-blockage_validation/front_lower - diag: "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/front_upper - diag: "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/left_lower - diag: "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/left_upper - diag: "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/right_lower - diag: "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/right_upper - diag: "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/rear_lower - diag: "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation" - type: diag - timeout: 1.0 - - path: /others/002-blockage_validation/rear_upper - diag: "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation" - type: diag - timeout: 1.0 - - - path: /others/004-concat_status - diag: "concatenate_data: concat_status" - type: diag - timeout: 1.0 - - - path: /others/005-visibility_validation/front_lower - diag: "dual_return_filter: /sensing/lidar/front_lower: visibility_validation" - type: diag - timeout: 1.0 - - - path: /others/005-visibility_validation/left_upper - diag: "dual_return_filter: /sensing/lidar/left_upper: visibility_validation" - type: diag - timeout: 1.0 - - - path: /others/010-emergency_vehicle - diag: "emergency_vehicle_detector: emergency_vehicle" - type: diag - timeout: 3.0 - - - path: /others/011-daytime_monitor - diag: "daytime_monitor: daytime_status" - type: diag - timeout: 3.0 - - - path: /others/012-vehicle_stuck_checker - diag: "vehicle_stuck_checker: vehicle_stuck_check" - type: diag - timeout: 3.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml deleted file mode 100644 index 425cb97e17..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/perception.yaml +++ /dev/null @@ -1,80 +0,0 @@ -nodes: - - path: /perception/autonomous_available - type: and - list: - - { type: link, link: /perception/emergency_stop } - - { type: link, link: /perception/pull_over } - - { type: link, link: /perception/comfortable_stop } - - - path: /perception/pull_over_available - type: and - list: - - { type: link, link: /perception/emergency_stop } - - { type: link, link: /perception/comfortable_stop } - - - path: /perception/comfortable_stop_available - type: and - list: - - { type: link, link: /perception/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /perception/emergency_stop - type: and - list: - - { type: link, link: /perception/001-topic_status/traffic_signals-error } - - { type: link, link: /perception/001-topic_status/objects-error } - - { type: link, link: /perception/001-topic_status/pointcloud-error } - - { type: link, link: /perception/002-detection_delay-error } - - - path: /perception/comfortable_stop - type: and - - - path: /perception/pull_over - type: and - - - path: /perception/none - type: and - - # Intermediate paths - - path: /perception/001-topic_status/traffic_signals-error - type: warn-to-ok - list: - - { type: link, link: /perception/001-topic_status/traffic_signals } - - - path: /perception/001-topic_status/objects-error - type: warn-to-ok - list: - - { type: link, link: /perception/001-topic_status/objects } - - - path: /perception/001-topic_status/pointcloud-error - type: warn-to-ok - list: - - { type: link, link: /perception/001-topic_status/pointcloud } - - - path: /perception/002-detection_delay-error - type: warn-to-ok - list: - - { type: link, link: /perception/002-detection_delay } - - # Diagnostics paths - - path: /perception/001-topic_status/traffic_signals - diag: "topic_state_monitor_traffic_light_recognition_traffic_signals: perception_topic_status" - type: diag - timeout: 1.0 - - - path: /perception/001-topic_status/objects - diag: "topic_state_monitor_object_recognition_objects: perception_topic_status" - type: diag - timeout: 1.0 - - - path: /perception/001-topic_status/pointcloud - diag: "topic_state_monitor_obstacle_segmentation_pointcloud: perception_topic_status" - type: diag - timeout: 1.0 - - - path: /perception/002-detection_delay - diag: "multi_object_tracker: Perception delay check from original header stamp" - type: diag - timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml deleted file mode 100644 index 16aab65635..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/planning.yaml +++ /dev/null @@ -1,184 +0,0 @@ -nodes: - - path: /planning/autonomous_available - type: and - list: - - { type: link, link: /planning/emergency_stop } - - { type: link, link: /planning/pull_over } - - { type: link, link: /planning/comfortable_stop } - - - path: /planning/pull_over_available - type: and - list: - - { type: link, link: /planning/emergency_stop } - - { type: link, link: /planning/comfortable_stop } - - - path: /planning/comfortable_stop_available - type: and - list: - - { type: link, link: /planning/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /planning/emergency_stop - type: and - list: - - { type: link, link: /planning/001-topic_status/route-error } - - { type: link, link: /planning/001-topic_status/trajectory-error } - - { type: link, link: /planning/003-trajectory_finite_validation-error } - - { type: link, link: /planning/004-trajectory_interval_validation-error } - - { type: link, link: /planning/005-trajectory_curvature_validation-error } - - { type: link, link: /planning/006-trajectory_relative_angle_validation-error } - - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation-error } - - { type: link, link: /planning/008-trajectory_acceleration_validation-error } - - { type: link, link: /planning/009-trajectory_deceleration_validation-error } - - { type: link, link: /planning/010-trajectory_steering_validation-error } - - { type: link, link: /planning/011-trajectory_steering_rate_validation-error } - - { type: link, link: /planning/012-trajectory_velocity_deviation_validation-error } - - { type: link, link: /planning/013-collision_checker-error } - - - path: /planning/comfortable_stop - type: and - - - path: /planning/pull_over - type: and - - - path: /planning/none - type: and - - # Intermediate paths - - path: /planning/001-topic_status/route-error - type: warn-to-ok - list: - - { type: link, link: /planning/001-topic_status/route } - - - path: /planning/001-topic_status/trajectory-error - type: warn-to-ok - list: - - { type: link, link: /planning/001-topic_status/trajectory } - - - path: /planning/003-trajectory_finite_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/003-trajectory_finite_validation } - - - path: /planning/004-trajectory_interval_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/004-trajectory_interval_validation } - - - path: /planning/005-trajectory_curvature_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/005-trajectory_curvature_validation } - - - path: /planning/006-trajectory_relative_angle_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/006-trajectory_relative_angle_validation } - - - path: /planning/007-trajectory_lateral_acceleration_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/007-trajectory_lateral_acceleration_validation } - - - path: /planning/008-trajectory_acceleration_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/008-trajectory_acceleration_validation } - - - path: /planning/009-trajectory_deceleration_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/009-trajectory_deceleration_validation } - - - path: /planning/010-trajectory_steering_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/010-trajectory_steering_validation } - - - path: /planning/011-trajectory_steering_rate_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/011-trajectory_steering_rate_validation } - - - path: /planning/012-trajectory_velocity_deviation_validation-error - type: warn-to-ok - list: - - { type: link, link: /planning/012-trajectory_velocity_deviation_validation } - - - path: /planning/013-collision_checker-error - type: warn-to-ok - list: - - { type: link, link: /planning/013-collision_checker } - - # Diagnostics paths - - path: /planning/001-topic_status/route - diag: "topic_state_monitor_mission_planning_route: planning_topic_status" - type: diag - timeout: 1.0 - - - path: /planning/001-topic_status/trajectory - diag: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" - type: diag - timeout: 1.0 - - # Not implemented - # - path: /planning/002-trajectory_size_validation - # diag: "planning_validator: trajectory_validation_size" - # type: diag - - - path: /planning/003-trajectory_finite_validation - diag: "planning_validator: trajectory_validation_finite" - type: diag - timeout: 1.0 - - - path: /planning/004-trajectory_interval_validation - diag: "planning_validator: trajectory_validation_interval" - type: diag - timeout: 1.0 - - - path: /planning/005-trajectory_curvature_validation - diag: "planning_validator: trajectory_validation_curvature" - type: diag - timeout: 1.0 - - - path: /planning/006-trajectory_relative_angle_validation - diag: "planning_validator: trajectory_validation_relative_angle" - type: diag - timeout: 1.0 - - - path: /planning/007-trajectory_lateral_acceleration_validation - diag: "planning_validator: trajectory_validation_lateral_acceleration" - type: diag - timeout: 1.0 - - - path: /planning/008-trajectory_acceleration_validation - diag: "planning_validator: trajectory_validation_acceleration" - type: diag - timeout: 1.0 - - - path: /planning/009-trajectory_deceleration_validation - diag: "planning_validator: trajectory_validation_deceleration" - type: diag - timeout: 1.0 - - - path: /planning/010-trajectory_steering_validation - diag: "planning_validator: trajectory_validation_steering" - type: diag - timeout: 1.0 - - - path: /planning/011-trajectory_steering_rate_validation - diag: "planning_validator: trajectory_validation_steering_rate" - type: diag - timeout: 1.0 - - - path: /planning/012-trajectory_velocity_deviation_validation - diag: "planning_validator: trajectory_validation_velocity_deviation" - type: diag - timeout: 1.0 - - - path: /planning/013-collision_checker - diag: "collision_checker: collision_check" - type: diag - timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml deleted file mode 100644 index 67ad0d43d8..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/sensing.yaml +++ /dev/null @@ -1,296 +0,0 @@ -nodes: - - path: /sensing/autonomous_available - type: and - list: - - { type: link, link: /sensing/emergency_stop } - - { type: link, link: /sensing/pull_over } - - { type: link, link: /sensing/comfortable_stop } - - - path: /sensing/pull_over_available - type: and - list: - - { type: link, link: /sensing/emergency_stop } - - { type: link, link: /sensing/comfortable_stop } - - - path: /sensing/comfortable_stop_available - type: and - list: - - { type: link, link: /sensing/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /sensing/emergency_stop - type: and - list: - - { type: link, link: /sensing/imu/001-monitor-error } - - { type: link, link: /sensing/imu/002-connection } - - { type: link, link: /sensing/lidar/pndr/001-connection } - - { type: link, link: /sensing/lidar/pndr/002-temperature-error } - - { type: link, link: /sensing/lidar/pndr/003-ptp } - - { type: link, link: /sensing/camera/001-connection } - - { type: link, link: /sensing/radar/001-connection } - - - path: /sensing/pull_over - type: and - list: - - { type: link, link: /sensing/lidar/pndr/002-temperature } - - { type: link, link: /sensing/gnss/001-connection-error } - - - path: /sensing/comfortable_stop - type: and - - - path: /sensing/none - type: and - list: - - { type: link, link: /sensing/imu/003-gyro_bias } - - { type: link, link: /sensing/gnss/002-quality } - - # Intermediate paths - - path: /sensing/lidar/pndr/002-temperature-error - type: warn-to-ok - list: - - { type: link, link: /sensing/lidar/pndr/002-temperature } - - - path: /sensing/imu/001-monitor-error - type: warn-to-ok - list: - - { type: link, link: /sensing/imu/001-monitor } - - - path: /sensing/gnss/001-connection-error - type: warn-to-ok - list: - - { type: link, link: /sensing/gnss/001-connection } - - - path: /sensing/lidar/pndr/001-connection - type: and - list: - - { type: link, link: /sensing/lidar/front_lower/connection } - - { type: link, link: /sensing/lidar/front_upper/connection } - - { type: link, link: /sensing/lidar/left_lower/connection } - - { type: link, link: /sensing/lidar/left_upper/connection } - - { type: link, link: /sensing/lidar/right_lower/connection } - - { type: link, link: /sensing/lidar/right_upper/connection } - - { type: link, link: /sensing/lidar/rear_lower/connection } - - { type: link, link: /sensing/lidar/rear_upper/connection } - - - path: /sensing/lidar/pndr/002-temperature - type: and - list: - - { type: link, link: /sensing/lidar/front_lower/temperature } - - { type: link, link: /sensing/lidar/front_upper/temperature } - - { type: link, link: /sensing/lidar/left_lower/temperature } - - { type: link, link: /sensing/lidar/left_upper/temperature } - - { type: link, link: /sensing/lidar/right_lower/temperature } - - { type: link, link: /sensing/lidar/right_upper/temperature } - - { type: link, link: /sensing/lidar/rear_lower/temperature } - - { type: link, link: /sensing/lidar/rear_upper/temperature } - - - path: /sensing/lidar/pndr/003-ptp - type: and - list: - - { type: link, link: /sensing/lidar/front_lower/ptp } - - { type: link, link: /sensing/lidar/front_upper/ptp } - - { type: link, link: /sensing/lidar/left_lower/ptp } - - { type: link, link: /sensing/lidar/left_upper/ptp } - - { type: link, link: /sensing/lidar/right_lower/ptp } - - { type: link, link: /sensing/lidar/right_upper/ptp } - - { type: link, link: /sensing/lidar/rear_lower/ptp } - - { type: link, link: /sensing/lidar/rear_upper/ptp } - - - path: /sensing/camera/001-connection - type: and - list: - - { type: link, link: /sensing/camera/0/connection } - - { type: link, link: /sensing/camera/1/connection } - - { type: link, link: /sensing/camera/2/connection } - - { type: link, link: /sensing/camera/3/connection } - - { type: link, link: /sensing/camera/4/connection } - - { type: link, link: /sensing/camera/5/connection } - - - path: /sensing/radar/001-connection - type: and - list: - - { type: link, link: /sensing/radar/front_center/connection } - - { type: link, link: /sensing/radar/front_left/connection } - - { type: link, link: /sensing/radar/front_right/connection } - - { type: link, link: /sensing/radar/rear_center/connection } - - { type: link, link: /sensing/radar/rear_left/connection } - - { type: link, link: /sensing/radar/rear_right/connection } - - # Diagnostics paths - - path: /sensing/imu/001-monitor - diag: "imu_monitor: yaw_rate_status" - type: diag - timeout: 5.0 - - path: /sensing/imu/002-connection - diag: "topic_state_monitor_imu_data: imu_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/imu/003-gyro_bias - diag: "gyro_bias_estimator: gyro_bias_validator" - type: diag - timeout: 1.0 - - - path: /sensing/gnss/001-connection - diag: "topic_state_monitor_gnss_pose: gnss_topic_status" - type: diag - timeout: 5.0 - - path: /sensing/gnss/002-quality - diag: "septentrio_driver: Quality indicators" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/front_lower/connection - diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/front_lower/ptp - diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/front_lower/temperature - diag: "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/front_upper/connection - diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/front_upper/ptp - diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/front_upper/temperature - diag: "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/left_lower/connection - diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/left_lower/ptp - diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/left_lower/temperature - diag: "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/left_upper/connection - diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/left_upper/ptp - diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/left_upper/temperature - diag: "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/right_lower/connection - diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/right_lower/ptp - diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/right_lower/temperature - diag: "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/right_upper/connection - diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/right_upper/ptp - diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/right_upper/temperature - diag: "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/rear_lower/connection - diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/rear_lower/ptp - diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/rear_lower/temperature - diag: "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/lidar/rear_upper/connection - diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection" - type: diag - timeout: 5.0 - - path: /sensing/lidar/rear_upper/ptp - diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp" - type: diag - timeout: 5.0 - - path: /sensing/lidar/rear_upper/temperature - diag: "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature" - type: diag - timeout: 5.0 - - - path: /sensing/camera/0/connection - diag: "v4l2_camera_camera0: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/1/connection - diag: "v4l2_camera_camera1: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/2/connection - diag: "v4l2_camera_camera2: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/3/connection - diag: "v4l2_camera_camera3: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/4/connection - diag: "v4l2_camera_camera4: capture_status" - type: diag - timeout: 1.0 - - path: /sensing/camera/5/connection - diag: "v4l2_camera_camera5: capture_status" - type: diag - timeout: 1.0 - - - path: /sensing/radar/front_center/connection - diag: "topic_state_monitor_radar_front_center: radar_front_center_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/radar/front_left/connection - diag: "topic_state_monitor_radar_front_left: radar_front_left_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/radar/front_right/connection - diag: "topic_state_monitor_radar_front_right: radar_front_right_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/radar/rear_center/connection - diag: "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/radar/rear_left/connection - diag: "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status" - type: diag - timeout: 1.0 - - path: /sensing/radar/rear_right/connection - diag: "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status" - type: diag - timeout: 1.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml deleted file mode 100644 index f1405b1f36..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/system.yaml +++ /dev/null @@ -1,83 +0,0 @@ -nodes: - - path: /system/autonomous_available - type: and - list: - - { type: link, link: /system/emergency_stop } - - { type: link, link: /system/pull_over } - - { type: link, link: /system/comfortable_stop } - - - path: /system/pull_over_available - type: and - list: - - { type: link, link: /system/emergency_stop } - - { type: link, link: /system/comfortable_stop } - - - path: /system/comfortable_stop_available - type: and - list: - - { type: link, link: /system/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /system/emergency_stop - type: and - list: - - { type: link, link: /system/001-topic_status-error } - - { type: link, link: /system/002-emergency_stop_operation-error } - - - path: /system/comfortable_stop - type: and - - - path: /system/pull_over - type: and - list: - - { type: link, link: /system/005-fms_connection-error } - - - path: /system/none - type: and - list: - - { type: link, link: /system/003-bagpacker_status } - - { type: link, link: /system/004-bagpacker_disk_space } - - # Intermediate paths - - path: /system/001-topic_status-error - type: warn-to-ok - list: - - { type: link, link: /system/001-topic_status } - - - path: /system/002-emergency_stop_operation-error - type: warn-to-ok - list: - - { type: link, link: /system/002-emergency_stop_operation } - - - path: /system/005-fms_connection-error - type: warn-to-ok - list: - - { type: link, link: /system/005-fms_connection } - - # Diagnostics paths - - path: /system/001-topic_status - diag: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" - type: diag - timeout: 1.0 - - - path: /system/002-emergency_stop_operation - diag: "vehicle_cmd_gate: emergency_stop_operation" - type: diag - timeout: 1.0 - - - path: /system/003-bagpacker_status - diag: rosbag_status - type: diag - timeout: 3.0 - - - path: /system/004-bagpacker_disk_space - diag: disk_status - type: diag - timeout: 3.0 - - - path: /system/005-fms_connection - diag: edge_core_internet_connection - type: diag - timeout: 10.0 diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml deleted file mode 100644 index 19f34ac02e..0000000000 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/vehicle.yaml +++ /dev/null @@ -1,82 +0,0 @@ -nodes: - - path: /vehicle/autonomous_available - type: and - list: - - { type: link, link: /vehicle/emergency_stop } - - { type: link, link: /vehicle/pull_over } - - { type: link, link: /vehicle/comfortable_stop } - - - path: /vehicle/pull_over_available - type: and - list: - - { type: link, link: /vehicle/emergency_stop } - - { type: link, link: /vehicle/comfortable_stop } - - - path: /vehicle/comfortable_stop_available - type: and - list: - - { type: link, link: /vehicle/emergency_stop } - - # ******************************************************************************* - # NOTE: Please modify this section according to your environment and requirements. - # ******************************************************************************* - - path: /vehicle/emergency_stop - type: and - list: - - { type: link, link: /vehicle/001-topic_status/velocity-error } - - { type: link, link: /vehicle/001-topic_status/steering-error } - - { type: link, link: /vehicle/005-vehicle_heartbeat-error } - - { type: link, link: /vehicle/006-vehicle_errors-error } - - - path: /vehicle/comfortable_stop - type: and - list: - - { type: link, link: /vehicle/006-vehicle_errors } - - - path: /vehicle/pull_over - type: and - - - path: /vehicle/none - type: and - - # Intermediate paths - - path: /vehicle/001-topic_status/velocity-error - type: warn-to-ok - list: - - { type: link, link: /vehicle/001-topic_status/velocity } - - - path: /vehicle/001-topic_status/steering-error - type: warn-to-ok - list: - - { type: link, link: /vehicle/001-topic_status/steering } - - - path: /vehicle/005-vehicle_heartbeat-error - type: warn-to-ok - list: - - { type: link, link: /vehicle/005-vehicle_heartbeat } - - - path: /vehicle/006-vehicle_errors-error - type: warn-to-ok - list: - - { type: link, link: /vehicle/006-vehicle_errors } - - # Diagnostics paths - - path: /vehicle/001-topic_status/velocity - diag: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" - type: diag - timeout: 1.0 - - - path: /vehicle/001-topic_status/steering - diag: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" - type: diag - timeout: 1.0 - - - path: /vehicle/005-vehicle_heartbeat - diag: "j6_interface: vehicle_heartbeat_errors" - type: diag - timeout: 1.0 - - - path: /vehicle/006-vehicle_errors - diag: "j6_interface: vehicle_errors" - type: diag - timeout: 1.0 diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 88712916b1..8aad52f2b7 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -61,7 +61,7 @@ - + From 7918ffdfe1a2a69a101c16eb12c1297790060a4f Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 12 Jul 2024 15:14:41 +0900 Subject: [PATCH 207/233] add sensing componet diag to system diagnostic monitor --- .../autoware-main.yaml | 4 + .../system_diagnostic_monitor/sensing.yaml | 209 ++++++++++++++++++ 2 files changed, 213 insertions(+) create mode 100644 autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml index 3f3b855640..773338c8db 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -6,6 +6,7 @@ files: - { path: $(dirname)/perception.yaml } - { path: $(dirname)/planning.yaml } - { path: $(dirname)/others.yaml } + - { path: $(dirname)/sensing.yaml } - { path: $(dirname)/system.yaml } - { path: $(dirname)/vehicle.yaml } @@ -29,6 +30,7 @@ units: - { type: link, link: /perception/autonomous_available } - { type: link, link: /planning/autonomous_available } - { type: link, link: /others/autonomous_available } + - { type: link, link: /sensing/autonomous_available } - { type: link, link: /system/autonomous_available } - { type: link, link: /vehicle/autonomous_available } @@ -42,6 +44,7 @@ units: - { type: link, link: /perception/pull_over_available } - { type: link, link: /planning/pull_over_available } - { type: link, link: /others/pull_over_available } + - { type: link, link: /sensing/pull_over_available } - { type: link, link: /system/pull_over_available } - { type: link, link: /vehicle/pull_over_available } @@ -55,6 +58,7 @@ units: - { type: link, link: /perception/comfortable_stop_available } - { type: link, link: /planning/comfortable_stop_available } - { type: link, link: /others/comfortable_stop_available } + - { type: link, link: /sensing/comfortable_stop_available } - { type: link, link: /system/comfortable_stop_available } - { type: link, link: /vehicle/comfortable_stop_available } diff --git a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml new file mode 100644 index 0000000000..ace203202e --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml @@ -0,0 +1,209 @@ +units: + - path: /sensing/autonomous_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + - { type: link, link: /sensing/pull_over } + - { type: link, link: /sensing/comfortable_stop } + + - path: /sensing/pull_over_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + - { type: link, link: /sensing/comfortable_stop } + + - path: /sensing/comfortable_stop_available + type: and + list: + - { type: link, link: /sensing/emergency_stop } + + # ******************************************************************************* + # NOTE: Please modify this section according to your environment and requirements. + # ******************************************************************************* + - path: /sensing/emergency_stop + type: and + list: + - { type: link, link: /sensing/imu/001-monitor-error } + - { type: link, link: /sensing/imu/002-connection } + # - { type: link, link: /sensing/lidar/pndr/001-connection } + # - { type: link, link: /sensing/lidar/pndr/002-temperature-error } + # - { type: link, link: /sensing/lidar/pndr/003-ptp } + - { type: link, link: /sensing/camera/001-connection } + - { type: link, link: /sensing/radar/001-connection } + + - path: /sensing/pull_over + type: and + list: + # - { type: link, link: /sensing/lidar/pndr/002-temperature } + - { type: link, link: /sensing/gnss/001-connection-error } + + - path: /sensing/comfortable_stop + type: and + + - path: /sensing/none + type: and + list: + - { type: link, link: /sensing/imu/003-gyro_bias } + - { type: link, link: /sensing/gnss/002-quality } + + # Intermediate paths + # - path: /sensing/lidar/pndr/002-temperature-error + # type: warn-to-ok + # list: + # - { type: link, link: /sensing/lidar/pndr/002-temperature } + + - path: /sensing/imu/001-monitor-error + type: warn-to-ok + item: + type: link + link: /sensing/imu/001-monitor + + - path: /sensing/gnss/001-connection-error + type: warn-to-ok + item: + type: link + link: /sensing/gnss/001-connection + + # - path: /sensing/lidar/pndr/001-connection + # type: and + # list: + # - { type: link, link: /sensing/lidar/front_lower/connection } + # - { type: link, link: /sensing/lidar/front_upper/connection } + # - { type: link, link: /sensing/lidar/left_lower/connection } + # - { type: link, link: /sensing/lidar/left_upper/connection } + # - { type: link, link: /sensing/lidar/right_lower/connection } + # - { type: link, link: /sensing/lidar/right_upper/connection } + # - { type: link, link: /sensing/lidar/rear_lower/connection } + # - { type: link, link: /sensing/lidar/rear_upper/connection } + + # - path: /sensing/lidar/pndr/002-temperature + # type: and + # list: + # - { type: link, link: /sensing/lidar/front_lower/temperature } + # - { type: link, link: /sensing/lidar/front_upper/temperature } + # - { type: link, link: /sensing/lidar/left_lower/temperature } + # - { type: link, link: /sensing/lidar/left_upper/temperature } + # - { type: link, link: /sensing/lidar/right_lower/temperature } + # - { type: link, link: /sensing/lidar/right_upper/temperature } + # - { type: link, link: /sensing/lidar/rear_lower/temperature } + # - { type: link, link: /sensing/lidar/rear_upper/temperature } + + # - path: /sensing/lidar/pndr/003-ptp + # type: and + # list: + # - { type: link, link: /sensing/lidar/front_lower/ptp } + # - { type: link, link: /sensing/lidar/front_upper/ptp } + # - { type: link, link: /sensing/lidar/left_lower/ptp } + # - { type: link, link: /sensing/lidar/left_upper/ptp } + # - { type: link, link: /sensing/lidar/right_lower/ptp } + # - { type: link, link: /sensing/lidar/right_upper/ptp } + # - { type: link, link: /sensing/lidar/rear_lower/ptp } + # - { type: link, link: /sensing/lidar/rear_upper/ptp } + + - path: /sensing/camera/001-connection + type: and + list: + - { type: link, link: /sensing/camera/0/connection } + - { type: link, link: /sensing/camera/1/connection } + - { type: link, link: /sensing/camera/2/connection } + - { type: link, link: /sensing/camera/3/connection } + - { type: link, link: /sensing/camera/4/connection } + - { type: link, link: /sensing/camera/5/connection } + + - path: /sensing/radar/001-connection + type: and + list: + - { type: link, link: /sensing/radar/front_center/connection } + - { type: link, link: /sensing/radar/front_left/connection } + - { type: link, link: /sensing/radar/front_right/connection } + - { type: link, link: /sensing/radar/rear_center/connection } + - { type: link, link: /sensing/radar/rear_left/connection } + - { type: link, link: /sensing/radar/rear_right/connection } + + # Diagnostics paths + - path: /sensing/imu/001-monitor + type: diag + node: imu_monitor + name: yaw_rate_status + type: diag + timeout: 5.0 + - path: /sensing/imu/002-connection + type: diag + node: topic_state_monitor_imu_data + name: imu_topic_status + timeout: 1.0 + - path: /sensing/imu/003-gyro_bias + type: diag + node: gyro_bias_estimator + name: gyro_bias_validator + timeout: 1.0 + - path: /sensing/gnss/001-connection + type: diag + node: topic_state_monitor_gnss_pose + name: gnss_topic_status + timeout: 5.0 + - path: /sensing/gnss/002-quality + type: diag + node: septentrio_driver + name: Quality indicators + timeout: 5.0 + - path: /sensing/camera/0/connection + type: diag + node: v4l2_camera_camera0 + name: capture_status + timeout: 1.0 + - path: /sensing/camera/1/connection + type: diag + node: v4l2_camera_camera1 + name: capture_status + timeout: 1.0 + - path: /sensing/camera/2/connection + type: diag + node: v4l2_camera_camera2 + name: capture_status + timeout: 1.0 + - path: /sensing/camera/3/connection + type: diag + node: v4l2_camera_camera3 + name: capture_status + timeout: 1.0 + - path: /sensing/camera/4/connection + type: diag + node: v4l2_camera_camera4 + name: capture_status + timeout: 1.0 + - path: /sensing/camera/5/connection + type: diag + node: v4l2_camera_camera5 + name: capture_status + timeout: 1.0 + - path: /sensing/radar/front_center/connection + type: diag + node: topic_state_monitor_radar_front_center + name: radar_front_center_topic_status + timeout: 1.0 + - path: /sensing/radar/front_left/connection + type: diag + node: topic_state_monitor_radar_front_left + name: radar_front_left_topic_status + timeout: 1.0 + - path: /sensing/radar/front_right/connection + type: diag + node: topic_state_monitor_radar_front_right + name: radar_front_right_topic_status + timeout: 1.0 + - path: /sensing/radar/rear_center/connection + type: diag + node: topic_state_monitor_radar_rear_center + name: radar_rear_center_topic_status + timeout: 1.0 + - path: /sensing/radar/rear_left/connection + type: diag + node: topic_state_monitor_radar_rear_left + name: radar_rear_left_topic_status + timeout: 1.0 + - path: /sensing/radar/rear_right/connection + type: diag + node: topic_state_monitor_radar_rear_right + name: radar_rear_right_topic_status + timeout: 1.0 \ No newline at end of file From aa08228a3712de0604aeee0fd1f2da5a64ddaacc Mon Sep 17 00:00:00 2001 From: saka1-s Date: Fri, 12 Jul 2024 06:17:07 +0000 Subject: [PATCH 208/233] style(pre-commit): autofix --- .../system/diagnostic_graph_aggregator/autoware-main.yaml | 4 +++- .../system/diagnostic_graph_aggregator/autoware-psim.yaml | 4 +++- .../system/system_diagnostic_monitor/autoware-main.yaml | 1 - .../config/system/system_diagnostic_monitor/control.yaml | 2 +- .../config/system/system_diagnostic_monitor/hardware.yaml | 2 +- .../config/system/system_diagnostic_monitor/localization.yaml | 2 +- .../config/system/system_diagnostic_monitor/map.yaml | 2 +- .../config/system/system_diagnostic_monitor/others.yaml | 4 ++-- .../config/system/system_diagnostic_monitor/perception.yaml | 2 +- .../config/system/system_diagnostic_monitor/planning.yaml | 4 ++-- .../config/system/system_diagnostic_monitor/sensing.yaml | 2 +- .../config/system/system_diagnostic_monitor/system.yaml | 2 +- .../config/system/system_diagnostic_monitor/vehicle.yaml | 2 +- 13 files changed, 18 insertions(+), 15 deletions(-) diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml index c5f2a033fe..f79f17398b 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml @@ -1,2 +1,4 @@ files: - - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-main.yaml } \ No newline at end of file + - { + path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-main.yaml, + } diff --git a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml index 1b6060f84c..f7e89443a8 100644 --- a/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml +++ b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-psim.yaml @@ -1,2 +1,4 @@ files: - - { path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-psim.yaml } \ No newline at end of file + - { + path: $(find-pkg-share autoware_launch)/config/system/system_diagnostic_monitor/autoware-psim.yaml, + } diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml index 773338c8db..29ab9d11b0 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -64,4 +64,3 @@ units: - path: /autoware/modes/emergency_stop type: ok - diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml index 06a3f77863..92bde38413 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -120,4 +120,4 @@ units: type: diag node: slip_detector name: slip_status - timeout: 3.0 \ No newline at end of file + timeout: 3.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml index 241d90bd30..20a3b069af 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml @@ -362,4 +362,4 @@ units: type: diag node: voltage_monitor name: CMOS Battery Status - timeout: 3.0 \ No newline at end of file + timeout: 3.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml index 4e47de5621..45adcac68e 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml @@ -96,4 +96,4 @@ units: type: diag node: localization name: localization_error_monitor - timeout: 1.0 \ No newline at end of file + timeout: 1.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml index 69e7638b65..685956f160 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/map.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml @@ -57,4 +57,4 @@ units: type: diag node: topic_state_monitor_pointcloud_map name: map_topic_status - timeout: 1.0 \ No newline at end of file + timeout: 1.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml index a54272fca2..56599c8434 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -31,7 +31,7 @@ units: - path: /others/comfortable_stop type: and # list: - # - { type: link, link: /others/010-emergency_vehicle-error } + # - { type: link, link: /others/010-emergency_vehicle-error } - path: /others/pull_over type: and @@ -160,4 +160,4 @@ units: type: diag node: vehicle_stuck_checker name: vehicle_stuck_check - timeout: 3.0 \ No newline at end of file + timeout: 3.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml index 5614d115c7..273e7ada84 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml @@ -83,4 +83,4 @@ units: type: diag node: multi_object_tracker name: Perception delay check from original header stamp - timeout: 1.0 \ No newline at end of file + timeout: 1.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml index 9487fee709..e983c9e799 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml @@ -45,7 +45,7 @@ units: - path: /planning/none type: and - + - path: /planning/001-topic_status/route-error type: warn-to-ok item: @@ -200,4 +200,4 @@ units: type: diag node: collision_checker name: collision_check - timeout: 1.0 \ No newline at end of file + timeout: 1.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml index ace203202e..1570f86de9 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml @@ -206,4 +206,4 @@ units: type: diag node: topic_state_monitor_radar_rear_right name: radar_rear_right_topic_status - timeout: 1.0 \ No newline at end of file + timeout: 1.0 diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml index 0e15251542..7b41213c44 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -32,7 +32,7 @@ units: - path: /system/pull_over type: and # list: - # - { type: link, link: /system/005-fms_connection-error } + # - { type: link, link: /system/005-fms_connection-error } # - path: /system/none # type: and diff --git a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml index 69bd1e2ba0..48c0ac97b4 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml @@ -85,4 +85,4 @@ units: type: diag node: j6_interface name: vehicle_errors - timeout: 1.0 \ No newline at end of file + timeout: 1.0 From 5e235b68639bfedbfcc1f28fd06fb70c7559f9f9 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 12 Jul 2024 15:59:38 +0900 Subject: [PATCH 209/233] remove imu_monitor from system_diagnostic_monitor --- .../system_diagnostic_monitor/sensing.yaml | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml index 1570f86de9..748d9f4e42 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/sensing.yaml @@ -23,8 +23,8 @@ units: - path: /sensing/emergency_stop type: and list: - - { type: link, link: /sensing/imu/001-monitor-error } - - { type: link, link: /sensing/imu/002-connection } + # - { type: link, link: /sensing/imu/001-monitor-error } + # - { type: link, link: /sensing/imu/002-connection } # - { type: link, link: /sensing/lidar/pndr/001-connection } # - { type: link, link: /sensing/lidar/pndr/002-temperature-error } # - { type: link, link: /sensing/lidar/pndr/003-ptp } @@ -52,11 +52,11 @@ units: # list: # - { type: link, link: /sensing/lidar/pndr/002-temperature } - - path: /sensing/imu/001-monitor-error - type: warn-to-ok - item: - type: link - link: /sensing/imu/001-monitor + # - path: /sensing/imu/001-monitor-error + # type: warn-to-ok + # item: + # type: link + # link: /sensing/imu/001-monitor - path: /sensing/gnss/001-connection-error type: warn-to-ok @@ -121,17 +121,17 @@ units: - { type: link, link: /sensing/radar/rear_right/connection } # Diagnostics paths - - path: /sensing/imu/001-monitor - type: diag - node: imu_monitor - name: yaw_rate_status - type: diag - timeout: 5.0 - - path: /sensing/imu/002-connection - type: diag - node: topic_state_monitor_imu_data - name: imu_topic_status - timeout: 1.0 + # - path: /sensing/imu/001-monitor + # type: diag + # node: imu_monitor + # name: yaw_rate_status + # type: diag + # timeout: 5.0 + # - path: /sensing/imu/002-connection + # type: diag + # node: topic_state_monitor_imu_data + # name: imu_topic_status + # timeout: 1.0 - path: /sensing/imu/003-gyro_bias type: diag node: gyro_bias_estimator From 0a94ddc38fb3180159dd65f5412743d66f11d34b Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 12 Jul 2024 16:08:49 +0900 Subject: [PATCH 210/233] disable sensing diag until aip_launcher fixed --- .../system/system_diagnostic_monitor/autoware-main.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml index 29ab9d11b0..371b866497 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -6,7 +6,7 @@ files: - { path: $(dirname)/perception.yaml } - { path: $(dirname)/planning.yaml } - { path: $(dirname)/others.yaml } - - { path: $(dirname)/sensing.yaml } + # - { path: $(dirname)/sensing.yaml } - { path: $(dirname)/system.yaml } - { path: $(dirname)/vehicle.yaml } @@ -30,7 +30,7 @@ units: - { type: link, link: /perception/autonomous_available } - { type: link, link: /planning/autonomous_available } - { type: link, link: /others/autonomous_available } - - { type: link, link: /sensing/autonomous_available } + # - { type: link, link: /sensing/autonomous_available } - { type: link, link: /system/autonomous_available } - { type: link, link: /vehicle/autonomous_available } @@ -44,7 +44,7 @@ units: - { type: link, link: /perception/pull_over_available } - { type: link, link: /planning/pull_over_available } - { type: link, link: /others/pull_over_available } - - { type: link, link: /sensing/pull_over_available } + # - { type: link, link: /sensing/pull_over_available } - { type: link, link: /system/pull_over_available } - { type: link, link: /vehicle/pull_over_available } @@ -58,7 +58,7 @@ units: - { type: link, link: /perception/comfortable_stop_available } - { type: link, link: /planning/comfortable_stop_available } - { type: link, link: /others/comfortable_stop_available } - - { type: link, link: /sensing/comfortable_stop_available } + # - { type: link, link: /sensing/comfortable_stop_available } - { type: link, link: /system/comfortable_stop_available } - { type: link, link: /vehicle/comfortable_stop_available } From 7ab712e859e08c6eba358b8459408ad8f7db8677 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Fri, 12 Jul 2024 18:23:43 +0900 Subject: [PATCH 211/233] prepare diag configuration for planning simulation --- .../autoware-psim.yaml | 58 ++++++++++++++++--- .../launch/planning_simulator.launch.xml | 2 +- 2 files changed, 51 insertions(+), 9 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml index 3d55079229..673b1911b0 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml @@ -1,11 +1,53 @@ files: - - { path: $(find-pkg-share system_diagnostic_monitor)/config/autoware-main.yaml } + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/system.yaml } +# Exclude paths that does not need on planning simulator edits: - - { type: remove, path: /autoware/map/topic_rate_check/pointcloud_map } - - { type: remove, path: /autoware/localization/scan_matching_status } - - { type: remove, path: /autoware/localization/accuracy } - - { type: remove, path: /autoware/localization/sensor_fusion_status } - - { type: remove, path: /autoware/localization/topic_rate_check/pose_twist_fusion } - - { type: remove, path: /autoware/perception/topic_rate_check/pointcloud } - - { type: remove, path: /autoware/control/emergency_braking } + - { type: remove, path: /map/001-topic_status/pointcloud_map-error } + - { type: remove, path: /map/001-topic_status/pointcloud_map } + # - { type: remove, path: /system/003-bagpacker_status } + # - { type: remove, path: /system/004-bagpacker_disk_space } + # - { type: remove, path: /system/005-fms_connection } + # - { type: remove, path: /system/005-fms_connection-error } + +units: + - path: /autoware/modes/local + type: stale + + - path: /autoware/modes/remote + type: stale + + # Stop is always available + - path: /autoware/modes/stop + type: ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, link: /map/autonomous_available } + - { type: link, link: /planning/autonomous_available } + - { type: link, link: /control/autonomous_available } + - { type: link, link: /system/autonomous_available } + + - path: /autoware/modes/pull_over + type: and + list: + - { type: link, link: /map/pull_over_available } + - { type: link, link: /planning/pull_over_available } + - { type: link, link: /control/pull_over_available } + - { type: link, link: /system/pull_over_available } + + - path: /autoware/modes/comfortable_stop + type: and + list: + - { type: link, link: /map/comfortable_stop_available } + - { type: link, link: /planning/comfortable_stop_available } + - { type: link, link: /control/comfortable_stop_available } + - { type: link, link: /system/comfortable_stop_available } + + # Emergency stop is always available + - path: /autoware/modes/emergency_stop + type: ok \ No newline at end of file diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 8aad52f2b7..88712916b1 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -61,7 +61,7 @@ - + From 04b703502c2d77422bb37de3238c222704078684 Mon Sep 17 00:00:00 2001 From: saka1-s Date: Fri, 12 Jul 2024 09:25:22 +0000 Subject: [PATCH 212/233] style(pre-commit): autofix --- .../config/system/system_diagnostic_monitor/autoware-psim.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml index 673b1911b0..9a86156f65 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml @@ -50,4 +50,4 @@ units: # Emergency stop is always available - path: /autoware/modes/emergency_stop - type: ok \ No newline at end of file + type: ok From 7bb5e380401f1b50fd1f577fe9cd0be050aa1c7a Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 16 Jul 2024 18:51:56 +0900 Subject: [PATCH 213/233] enable misc system diag --- .../system_diagnostic_monitor/system.yaml | 54 +++++++++---------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml index 7b41213c44..02fa87326f 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/system.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -31,14 +31,14 @@ units: - path: /system/pull_over type: and - # list: - # - { type: link, link: /system/005-fms_connection-error } + list: + - { type: link, link: /system/005-fms_connection-error } - # - path: /system/none - # type: and - # list: - # - { type: link, link: /system/003-bagpacker_status } - # - { type: link, link: /system/004-bagpacker_disk_space } + - path: /system/none + type: and + list: + - { type: link, link: /system/003-bagpacker_status } + - { type: link, link: /system/004-bagpacker_disk_space } - path: /system/001-topic_status-error type: warn-to-ok @@ -52,11 +52,11 @@ units: type: link link: /system/002-emergency_stop_operation - # - path: /system/005-fms_connection-error - # type: warn-to-ok - # item: - # type: link - # link: /system/005-fms_connection + - path: /system/005-fms_connection-error + type: warn-to-ok + item: + type: link + link: /system/005-fms_connection - path: /system/001-topic_status type: diag @@ -70,20 +70,20 @@ units: name: emergency_stop_operation timeout: 1.0 - # - path: /system/003-bagpacker_status - # type: diag - # node: system - # name: rosbag_status - # timeout: 3.0 + - path: /system/003-bagpacker_status + type: diag + node: "" + name: rosbag_status + timeout: 3.0 - # - path: /system/004-bagpacker_disk_space - # type: diag - # node: system - # name: disk_status - # timeout: 3.0 + - path: /system/004-bagpacker_disk_space + type: diag + node: "" + name: disk_status + timeout: 3.0 - # - path: /system/005-fms_connection - # type: diag - # node: system - # name: edge_core_internet_connection - # timeout: 10.0 + - path: /system/005-fms_connection + type: diag + node: "" + name: edge_core_internet_connection + timeout: 10.0 From a8d926fc32b4fe1ed83c1dfaf72ffd5b0c8ff4ef Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Tue, 16 Jul 2024 18:52:42 +0900 Subject: [PATCH 214/233] use dummy diag publisher --- .../system/dummy_diag_publisher/dummy_diag_publisher.param.yaml | 2 +- autoware_launch/launch/planning_simulator.launch.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml index 02c6d305eb..bb97229163 100644 --- a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml +++ b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml @@ -131,7 +131,7 @@ "topic_state_monitor_transform_map_to_base_link: localization_topic_status": default ## /localization/003-matching_score - ndt_scan_matcher: default + "ndt_scan_matcher: scan_matching_status": default ## /localization/004-accuracy "localization: localization_error_monitor": default diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 88712916b1..8aad52f2b7 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -61,7 +61,7 @@ - + From ffc4bffed1c5ab926944af58d4cf173728fdc748 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 17 Jul 2024 19:18:33 +0900 Subject: [PATCH 215/233] disable avoidance_for_ambiguous_vehicle --- .../static_obstacle_avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b7b9d333e6..f4d9efdde6 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 @@ -138,7 +138,7 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] + enable: false # [-] closest_distance_to_wait_and_see: 10.0 # [m] condition: th_stopped_time: 3.0 # [s] From 216f79f149128144a58377df3b96cbd38e2ef13d Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Thu, 18 Jul 2024 11:19:01 +0900 Subject: [PATCH 216/233] make same crosswalk parameter as v3.0.0 --- .../crosswalk.param.yaml | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 88d730f654..f0460c4fbf 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -16,7 +16,7 @@ # For the case where the crosswalk width is very wide far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). # For the case where the stop position is determined according to the object position. - stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_distance_from_object: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". slow_down: @@ -30,31 +30,31 @@ enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. - required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle - min_acc: -2.8 # min acceleration [m/ss] - min_jerk: -5.0 # min jerk [m/sss] - max_jerk: 5.0 # max jerk [m/sss] + required_clearance: 10.0 # [m] clearance to be secured between the ego and the ahead vehicle + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles pass_judge: ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) - ego_pass_later_margin_y: [13.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. - min_acc: -2.8 # min acceleration [m/ss] - min_jerk: -5.0 # min jerk [m/sss] - max_jerk: 5.0 # max jerk [m/sss] + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] - stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.25 m/s = 0.9 kmph) + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal + disable_yield_for_new_stopped_object: false # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order timeout_set_for_no_intention_to_walk: [3.0, 0.0] # [s] @@ -62,7 +62,7 @@ # param for target object filtering object_filtering: - crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle target_object: unknown: true # [-] whether to look and stop by UNKNOWN objects @@ -72,7 +72,7 @@ # param for occlusions occlusion: - enable: true # if true, ego will slowdown around crosswalks that are occluded + enable: false # if true, ego will slowdown around crosswalks that are occluded occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space slow_down_velocity: 1.0 # [m/s] time_buffer: 0.5 # [s] consecutive time with/without an occlusion to add/remove the slowdown From 047485cd72b9e6a612e8b8de55d1d3bb4c01930a Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Thu, 18 Jul 2024 11:40:57 +0900 Subject: [PATCH 217/233] make same lane change parameter as v3.0.0 --- .../lane_change/lane_change.param.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 e9bbb65d3f..3dbc3b01dd 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 @@ -49,12 +49,12 @@ longitudinal_velocity_delta_time: 0.3 stuck: expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 # lane expansion for object filtering lane_expansion: From bafc0c3f82104d7001bcb527534fa6bd03c88960 Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Tue, 23 Jul 2024 13:28:11 +0900 Subject: [PATCH 218/233] chore: set intersection occlusion as false (#709) set intersection occlusion as false Signed-off-by: Zhi Wang --- .../behavior_velocity_planner/intersection.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 7820a5db31..768a11de4b 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -89,7 +89,7 @@ object_time_margin_to_collision_point: 4.0 occlusion: - enable: true + enable: false occlusion_attention_area_length: 70.0 free_space_max: 43 occupied_min: 58 From a9c13f974b76483f5185da843cd6d8748095d4ab Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 27 Jun 2024 17:51:21 +0900 Subject: [PATCH 219/233] change param Signed-off-by: Yuki Takagi --- .../vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 6e89a18e06..b3600f1477 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 @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [6.0, 6.0] - lon_jerk_lim: [30.0, 30.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.1, 0.2, 20.0, 30.0] + steer_lim: [1.0, 1.0, 1.0, 0.8] + steer_rate_lim: [1.0, 1.0, 1.0, 0.8] + lon_acc_lim: [6.0, 6.0, 6.0, 6.0] + lon_jerk_lim: [80.0, 30.0, 30.0, 30.0] + lat_acc_lim: [5.0, 5.0, 5.0, 4.0] + lat_jerk_lim: [7.0, 7.0, 7.0, 6.0] + actual_steer_diff_lim: [1.0, 1.0, 1.0, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] From 0ea09ef69d28ae24a618c824c42735c58eff91e3 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 24 Jul 2024 16:52:41 +0900 Subject: [PATCH 220/233] change pid long con params Signed-off-by: Yuki Takagi --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 688e650de1..39dc99e7a6 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -53,12 +53,11 @@ # stopped state stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 + stopped_acc: -3.4 # denotes pedal position # emergency state emergency_vel: 0.0 - emergency_acc: -2.5 + emergency_acc: -2.5 # denotes acceleration emergency_jerk: -1.5 # acceleration limit @@ -68,6 +67,7 @@ # jerk limit max_jerk: 2.0 min_jerk: -30.0 + max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation # pitch From d398b9b946a5c5149f5a974d7be6f01aa213780a Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 24 Jul 2024 17:52:44 +0900 Subject: [PATCH 221/233] change max pitch Signed-off-by: Yuki Takagi --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index 39dc99e7a6..87d9bf0868 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -74,5 +74,5 @@ lpf_pitch_gain: 0.95 slope_source: "trajectory_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive adaptive_trajectory_velocity_th: 1.0 - max_pitch_rad: 0.1 - min_pitch_rad: -0.1 + max_pitch_rad: 0.15 + min_pitch_rad: -0.15 From 73175b5c7767868fedb021d470c756a2fff710f1 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Thu, 25 Jul 2024 18:16:40 +0900 Subject: [PATCH 222/233] chore(other.yaml): Chore/diag config (#713) * enable blockage_validation and visibility_validation Signed-off-by: kotaro-hihara * comment out emergency_vehicle --------- Signed-off-by: kotaro-hihara --- .../system_diagnostic_monitor/others.yaml | 210 +++++++++--------- 1 file changed, 105 insertions(+), 105 deletions(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml index 56599c8434..62ae6d2483 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/others.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -23,15 +23,15 @@ units: - path: /others/emergency_stop type: and list: - # - { type: link, link: /others/002-blockage_validation-error } + #- { type: link, link: /others/002-blockage_validation-error } - { type: link, link: /others/004-concat_status } - # - { type: link, link: /others/005-visibility_validation-error } + - { type: link, link: /others/005-visibility_validation-error } - { type: link, link: /others/012-vehicle_stuck_checker } - path: /others/comfortable_stop type: and - # list: - # - { type: link, link: /others/010-emergency_vehicle-error } + #list: + #- { type: link, link: /others/010-emergency_vehicle-error } - path: /others/pull_over type: and @@ -41,90 +41,90 @@ units: - path: /others/none type: and - # - path: /others/010-emergency_vehicle-error - # type: warn-to-ok - # item: - # type: link - # link: /others/010-emergency_vehicle - - # # Intermediate paths - # - path: /others/002-blockage_validation-error - # type: warn-to-ok - # item: - # type: link - # link: /others/002-blockage_validation - - # - path: /others/002-blockage_validation - # type: and - # list: - # - { type: link, link: /others/002-blockage_validation/front_lower } - # - { type: link, link: /others/002-blockage_validation/front_upper } - # - { type: link, link: /others/002-blockage_validation/left_lower } - # - { type: link, link: /others/002-blockage_validation/left_upper } - # - { type: link, link: /others/002-blockage_validation/right_lower } - # - { type: link, link: /others/002-blockage_validation/right_upper } - # - { type: link, link: /others/002-blockage_validation/rear_lower } - # - { type: link, link: /others/002-blockage_validation/rear_upper } - - # - path: /others/005-visibility_validation - # type: and - # list: - # - { type: link, link: /others/005-visibility_validation/front_lower } - # - { type: link, link: /others/005-visibility_validation/left_upper } - - # - path: /others/005-visibility_validation-error - # type: warn-to-ok - # item: - # type: link - # link: /others/005-visibility_validation - - # - path: /others/002-blockage_validation/front_lower - # type: diag - # node: "blockage_return_diag: /sensing/lidar/front_lower" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/front_upper - # type: diag - # node: "blockage_return_diag: /sensing/lidar/front_upper" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/left_lower - # type: diag - # node: "blockage_return_diag: /sensing/lidar/left_lower" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/left_upper - # type: diag - # node: "blockage_return_diag: /sensing/lidar/left_upper" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/right_lower - # type: diag - # node: "blockage_return_diag: /sensing/lidar/right_lower" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/right_upper - # type: diag - # node: "blockage_return_diag: /sensing/lidar/right_upper" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/rear_lower - # type: diag - # node: "blockage_return_diag: /sensing/lidar/rear_lower" - # name: blockage_validation - # timeout: 1.0 - - # - path: /others/002-blockage_validation/rear_upper - # type: diag - # node: "blockage_return_diag: /sensing/lidar/rear_upper" - # name: blockage_validation - # timeout: 1.0 + - path: /others/010-emergency_vehicle-error + type: warn-to-ok + item: + type: link + link: /others/010-emergency_vehicle + + # Intermediate paths + - path: /others/002-blockage_validation-error + type: warn-to-ok + item: + type: link + link: /others/002-blockage_validation + + - path: /others/002-blockage_validation + type: and + list: + - { type: link, link: /others/002-blockage_validation/front_lower } + - { type: link, link: /others/002-blockage_validation/front_upper } + - { type: link, link: /others/002-blockage_validation/left_lower } + - { type: link, link: /others/002-blockage_validation/left_upper } + - { type: link, link: /others/002-blockage_validation/right_lower } + - { type: link, link: /others/002-blockage_validation/right_upper } + - { type: link, link: /others/002-blockage_validation/rear_lower } + - { type: link, link: /others/002-blockage_validation/rear_upper } + + - path: /others/005-visibility_validation + type: and + list: + - { type: link, link: /others/005-visibility_validation/front_lower } + - { type: link, link: /others/005-visibility_validation/left_upper } + + - path: /others/005-visibility_validation-error + type: warn-to-ok + item: + type: link + link: /others/005-visibility_validation + + - path: /others/002-blockage_validation/front_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/front_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/front_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/front_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/left_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/left_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/left_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/left_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/right_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/right_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/right_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/right_upper" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/rear_lower + type: diag + node: "blockage_return_diag: /sensing/lidar/rear_lower" + name: blockage_validation + timeout: 1.0 + + - path: /others/002-blockage_validation/rear_upper + type: diag + node: "blockage_return_diag: /sensing/lidar/rear_upper" + name: blockage_validation + timeout: 1.0 - path: /others/004-concat_status type: diag @@ -132,23 +132,23 @@ units: name: concat_status timeout: 1.0 - # - path: /others/005-visibility_validation/front_lower - # type: diag - # node: "dual_return_filter: /sensing/lidar/front_lower" - # name: visibility_validation - # timeout: 1.0 - - # - path: /others/005-visibility_validation/left_upper - # type: diag - # node: "dual_return_filter: /sensing/lidar/left_upper" - # name: visibility_validation - # timeout: 1.0 - - # - path: /others/010-emergency_vehicle - # type: diag - # node: emergency_vehicle_detector - # name: emergency_vehicle - # timeout: 3.0 + - path: /others/005-visibility_validation/front_lower + type: diag + node: "dual_return_filter: /sensing/lidar/front_lower" + name: visibility_validation + timeout: 1.0 + + - path: /others/005-visibility_validation/left_upper + type: diag + node: "dual_return_filter: /sensing/lidar/left_upper" + name: visibility_validation + timeout: 1.0 + + - path: /others/010-emergency_vehicle + type: diag + node: emergency_vehicle_detector + name: emergency_vehicle + timeout: 3.0 - path: /others/011-daytime_monitor type: diag From 00b80fdff164002c7b8c18ca0e848cbfd9d848c3 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 1 Aug 2024 14:53:23 +0900 Subject: [PATCH 223/233] Update 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 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../static_obstacle_avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7b23c8498e..f079547e76 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 @@ -142,7 +142,7 @@ # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. # "ignore" : never avoid it. - policy: "auto" # [-] + policy: "ignore" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] From 05c78e13a258e0602b7ebc2f9acd441abf28f6d7 Mon Sep 17 00:00:00 2001 From: kotaro-hihara Date: Wed, 7 Aug 2024 10:39:51 +0900 Subject: [PATCH 224/233] chore(detection): fix max_cluster_size (#723) fix max_cluster_size --- .../clustering/voxel_grid_based_euclidean_cluster.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml index 26b027f007..7251fa9fa8 100644 --- a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -4,7 +4,7 @@ voxel_leaf_size: 0.3 min_points_number_per_voxel: 1 min_cluster_size: 10 - max_cluster_size: 3000 + max_cluster_size: 4893 use_height: false input_frame: "base_link" From 3017a25f0a93e9a98d4de80c7e97b09d3e6da716 Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Wed, 7 Aug 2024 14:48:06 +0900 Subject: [PATCH 225/233] add main and sub launch file Signed-off-by: Zhi Wang --- .../launch/autoware.main.launch.xml | 153 ++++++++++++++++++ .../launch/autoware.sub.launch.xml | 153 ++++++++++++++++++ 2 files changed, 306 insertions(+) create mode 100644 autoware_launch/launch/autoware.main.launch.xml create mode 100644 autoware_launch/launch/autoware.sub.launch.xml diff --git a/autoware_launch/launch/autoware.main.launch.xml b/autoware_launch/launch/autoware.main.launch.xml new file mode 100644 index 0000000000..9c517db7a4 --- /dev/null +++ b/autoware_launch/launch/autoware.main.launch.xml @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml new file mode 100644 index 0000000000..b5b7b741a8 --- /dev/null +++ b/autoware_launch/launch/autoware.sub.launch.xml @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8a71f6cd20f3459ce932d6ccbc37055acc02fc9b Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Thu, 8 Aug 2024 11:26:22 +0900 Subject: [PATCH 226/233] remove system component launch on sub ecu Signed-off-by: Zhi Wang --- autoware_launch/launch/autoware.sub.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml index b5b7b741a8..873db6a22d 100644 --- a/autoware_launch/launch/autoware.sub.launch.xml +++ b/autoware_launch/launch/autoware.sub.launch.xml @@ -79,9 +79,9 @@ --> - + - + diff --git a/autoware_launch/launch/components/tier4_system_component.main.launch.xml b/autoware_launch/launch/components/tier4_system_component.main.launch.xml new file mode 100644 index 0000000000..3918596c46 --- /dev/null +++ b/autoware_launch/launch/components/tier4_system_component.main.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 3128ce2abc276f0adb733bed408a3687e577bdd7 Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Wed, 14 Aug 2024 12:07:26 +0900 Subject: [PATCH 229/233] fix main system launch file Signed-off-by: Zhi Wang --- .../tier4_system_component.main.launch.xml | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/autoware_launch/launch/components/tier4_system_component.main.launch.xml b/autoware_launch/launch/components/tier4_system_component.main.launch.xml index 3918596c46..c8c9d23521 100644 --- a/autoware_launch/launch/components/tier4_system_component.main.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.main.launch.xml @@ -1,16 +1,15 @@ - + + - - - - - - - - - - + + + + + + + + From 7ef5528138bc7e84dfa2a9911d13268f64aa4040 Mon Sep 17 00:00:00 2001 From: Naophis Date: Wed, 14 Aug 2024 12:20:42 +0900 Subject: [PATCH 230/233] chore: comment out max_distance_deviation-error link in control.yaml Signed-off-by: Naophis --- .../config/system/system_diagnostic_monitor/control.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml index 92bde38413..bf7e053a35 100644 --- a/autoware_launch/config/system/system_diagnostic_monitor/control.yaml +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -27,7 +27,7 @@ units: - { type: link, link: /control/004-lane_departure-error } - { type: link, link: /control/005-trajectory_deviation-error } # - { type: link, link: /control/009-aeb_emergency_stop } - - { type: link, link: /control/010-max_distance_deviation-error } + # - { type: link, link: /control/010-max_distance_deviation-error } - { type: link, link: /control/011-slip_detection } - path: /control/comfortable_stop From c445edcb453406a450bcd78dfc7d195958dac5ef Mon Sep 17 00:00:00 2001 From: Zhi Wang Date: Wed, 14 Aug 2024 12:36:49 +0900 Subject: [PATCH 231/233] launch system for sub ecu Signed-off-by: Zhi Wang --- autoware_launch/launch/autoware.sub.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml index 873db6a22d..b5b7b741a8 100644 --- a/autoware_launch/launch/autoware.sub.launch.xml +++ b/autoware_launch/launch/autoware.sub.launch.xml @@ -79,9 +79,9 @@ --> - + - + diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml index b5b7b741a8..873db6a22d 100644 --- a/autoware_launch/launch/autoware.sub.launch.xml +++ b/autoware_launch/launch/autoware.sub.launch.xml @@ -79,9 +79,9 @@ --> - + - - - - - - - - - - - - - - -