diff --git a/.github/workflows/sync-beta-upstream.yaml b/.github/workflows/sync-beta-upstream.yaml index 25b83306ca..677b98804e 100644 --- a/.github/workflows/sync-beta-upstream.yaml +++ b/.github/workflows/sync-beta-upstream.yaml @@ -21,10 +21,10 @@ jobs: uses: autowarefoundation/autoware-github-actions/sync-branches@v1 with: token: ${{ steps.generate-token.outputs.token }} - base-branch: beta/ + 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/ + sync-target-branch: beta/v0.29.0 pr-title: "chore: sync beta upstream" pr-labels: | bot 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 135aecc56b..7f318a5309 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) @@ -45,11 +45,11 @@ # -- 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] + 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] @@ -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 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 97a1d64ed8..87d9bf0868 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - delay_compensation_time: 0.1 + delay_compensation_time: 0.15 enable_smooth_stop: true 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 @@ -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 @@ -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 @@ -57,21 +57,22 @@ # emergency state emergency_vel: 0.0 - emergency_acc: -5.0 # denotes acceleration - emergency_jerk: -3.0 + emergency_acc: -2.5 # denotes acceleration + emergency_jerk: -1.5 # acceleration limit - max_acc: 3.0 - min_acc: -5.0 + max_acc: 1.86 + min_acc: -6.00 # jerk limit max_jerk: 2.0 - min_jerk: -5.0 + min_jerk: -30.0 max_acc_cmd_diff: 50.0 # [m/s^2 * s^-1] # slope compensation + # pitch 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 + max_pitch_rad: 0.15 + min_pitch_rad: -0.15 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..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 @@ -2,28 +2,28 @@ 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 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 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: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.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] diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 19bfd2d498..6df24435cc 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -2,7 +2,7 @@ ros__parameters: node: show_debug_info: false - enable_yaw_bias_estimation: true + enable_yaw_bias_estimation: false predict_frequency: 50.0 tf_rate: 50.0 publish_tf: true @@ -42,5 +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" 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 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" 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/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/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 3f33218eb3..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 @@ -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 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 @@ -19,18 +19,85 @@ 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: 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 + + # 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 + 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: + 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 + 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 + + front_lower_crop_box_filter: + parameters: + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.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 + 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: 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 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 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..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 @@ -2,5 +2,5 @@ ros__parameters: external_time_tolerance: 5.0 perception_time_tolerance: 1.0 - external_priority: false + external_priority: true enable_signal_matching: false 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..12c1bcadf5 --- /dev/null +++ b/autoware_launch/config/planning/preset/x2_preset.yaml @@ -0,0 +1,128 @@ +launch: + # behavior path modules + - arg: + name: launch_static_obstacle_avoidance + default: "true" + - arg: + name: launch_avoidance_by_lane_change_module + default: "false" + - arg: + 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" + - 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_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: 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 + # option: obstacle_stop_planner + # obstacle_cruise_planner + # none + + - arg: + name: 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" 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 770c4bb86e..4ad6cc9684 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] @@ -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] 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..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,13 +1,13 @@ /**: 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: 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: 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 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml index 4776884734..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 @@ -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 @@ -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] @@ -225,7 +225,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 @@ -264,7 +264,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. @@ -272,7 +272,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 @@ -281,10 +281,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: @@ -292,7 +292,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] # path generation method. select "shift_line_base" or "optimization_base" or "both". 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 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..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 @@ -33,4 +33,5 @@ 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 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 67708f3386..5e381a2bd3 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,11 +14,11 @@ 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: 3.0 - max_lateral_offset: 1.0 + longitudinal_margin: 6.0 + max_lateral_offset: 0.0 lateral_offset_interval: 0.5 ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.75 @@ -60,8 +60,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 @@ -77,7 +77,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 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 f40992f354..f119532ba6 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,14 +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: 0.0 # [m] - lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_jerk: 0.3 # [m/s3] - minimum_lane_changing_velocity: 2.78 # [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 @@ -23,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 skip_process: longitudinal_distance_diff_threshold: @@ -52,6 +53,14 @@ longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 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 @@ -59,14 +68,6 @@ 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 - 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 # lane expansion for object filtering lane_expansion: @@ -76,8 +77,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.5, 0.5, 0.5] # target object target_object: @@ -96,9 +97,9 @@ 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: true + use_all_predicted_path: false # lane change regulations regulation: @@ -114,8 +115,8 @@ # lane change cancel cancel: enable_on_prepare_phase: true - enable_on_lane_changing_phase: true - delta_time: 1.0 # [s] + enable_on_lane_changing_phase: false + delta_time: 2.0 # [s] duration: 5.0 # [s] max_lateral_jerk: 100.0 # [m/s3] overhang_tolerance: 0.0 # [m] 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..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 @@ -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,7 +22,7 @@ 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 max_module_size: 1 @@ -30,14 +30,14 @@ 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 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 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 a901a9f29a..0d0528947d 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 @@ -44,7 +44,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 @@ -141,7 +141,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 extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" 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..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,7 +30,7 @@ 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 + 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] @@ -38,10 +38,10 @@ # 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 @@ -51,18 +51,18 @@ 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: [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: - 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 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..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 @@ -70,15 +70,15 @@ collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 partially_prioritized: - collision_start_margin_time: 3.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: 3.0 - collision_end_margin_time: 2.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 yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 - duration: 3.0 - object_dist_to_stopline: 10.0 + duration: 8.0 + object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: car: 2.0 @@ -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 @@ -112,11 +112,12 @@ debug: ttc: [0] + enable_rtc: intersection: false intersection_to_occlusion: false merge_from_private: - stopline_margin: 3.0 - stop_duration_sec: 1.0 + stopline_margin: 1.7 + stop_duration_sec: 1.5 stop_distance_threshold: 1.0 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..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 @@ -1,8 +1,8 @@ /**: ros__parameters: traffic_light: - stop_margin: 0.0 - tl_state_timeout: 1.0 + stop_margin: 0.5 + tl_state_timeout: 0.36 stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true 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 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..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" @@ -26,10 +26,10 @@ - "traffic_light" - "lane_change_left" - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "avoidance_by_lane_change_left" - - "avoidance_by_lane_change_right" + # - "avoidance_left" + # - "avoidance_right" + # - "avoidance_by_lane_change_left" + # - "avoidance_by_lane_change_right" - "goal_planner" - "start_planner" - "intersection_occlusion" 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/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..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 @@ -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] @@ -21,11 +21,11 @@ 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: 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: @@ -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: @@ -105,9 +105,8 @@ 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 successive_num_to_exit_slow_down_condition: 5 @@ -189,14 +188,14 @@ 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 + 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/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index d608a7ab11..2f4dfab0a4 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -1,37 +1,193 @@ /**: 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" + # 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" + + # 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" + 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" + + # CAMERA + 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" + 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" + 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-002: "topic_state_monitor_transform_map_to_base_link: localization_topic_status" + LOCALIZATION-003: "ndt_scan_matcher" + LOCALIZATION-004: "localization: localization_error_monitor" + + # 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" + 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-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" + 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: "rosbag_status" + SYSTEM-004: "disk_status" + 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/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" 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 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-main.yaml b/autoware_launch/config/system/diagnostic_graph_aggregator/autoware-main.yaml index f35f219df6..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,5 +1,4 @@ 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, + } 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..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,5 +1,4 @@ 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, + } 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..6d0359c6b3 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,198 @@ /**: 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: scan_matching_status": 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/002-blockage_validation/front_lower + "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation": default + + ## /others/002-blockage_validation/front_upper + "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation": default + + ## /others/002-blockage_validation/left_lower + "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation": default + + ## /others/002-blockage_validation/left_upper + "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation": default + + ## /others/002-blockage_validation/right_upper + "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation": default + + ## /others/002-blockage_validation/right_lower + "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation": default + + ## /others/002-blockage_validation/rear_lower + "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation": default + + ## /others/002-blockage_validation/rear_upper + "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation": default + + ## /others/005-visibility_validation/front_lower + "dual_return_filter: /sensing/lidar/front_lower: visibility_validation": default + + ## /others/005-visibility_validation/left_upper + "dual_return_filter: /sensing/lidar/left_upper: visibility_validation": default + + ## /others/010-emergency_vehicle + "emergency_vehicle_detector: emergency_vehicle": default 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: 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_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: 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..371b866497 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-main.yaml @@ -0,0 +1,66 @@ +files: + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/hardware.yaml } + - { path: $(dirname)/localization.yaml } + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/perception.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/others.yaml } + # - { path: $(dirname)/sensing.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: /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: /sensing/autonomous_available } + - { type: link, link: /system/autonomous_available } + - { type: link, link: /vehicle/autonomous_available } + + - path: /autoware/modes/pull_over + type: and + list: + - { 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: /sensing/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: /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: /sensing/comfortable_stop_available } + - { type: link, link: /system/comfortable_stop_available } + - { type: link, link: /vehicle/comfortable_stop_available } + + - path: /autoware/modes/emergency_stop + type: ok 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..9a86156f65 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/autoware-psim.yaml @@ -0,0 +1,53 @@ +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: + - { 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 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..bf7e053a35 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/control.yaml @@ -0,0 +1,123 @@ +units: + - 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 + 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: /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: /control/003-gate_heartbeat + type: diag + node: vehicle_cmd_gate + name: heartbeat + timeout: 1.0 + + - path: /control/004-lane_departure + type: diag + node: lane_departure_checker_node + name: lane_departure + timeout: 1.0 + + - path: /control/005-trajectory_deviation + type: diag + node: lane_departure_checker_node + name: trajectory_deviation + timeout: 1.0 + + - path: /control/007-external_command_converter_heartbeat + type: diag + node: external_cmd_converter + name: remote_control_topic_status + timeout: 1.0 + + - path: /control/008-external_command_selector_heartbeat + type: diag + node: external_cmd_selector + 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/010-max_distance_deviation + type: diag + 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 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..20a3b069af --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/hardware.yaml @@ -0,0 +1,365 @@ +units: + - 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 } + - 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 } + + - 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: /hardware/cpu/002-usage + type: diag + node: cpu_monitor + name: CPU Usage + timeout: 3.0 + - path: /hardware/cpu/003-load_average + type: diag + node: cpu_monitor + name: CPU Load Average + timeout: 3.0 + - path: /hardware/cpu/004-throttling + type: diag + node: cpu_monitor + name: CPU Thermal Throttling + timeout: 3.0 + - path: /hardware/cpu/005-frequency + type: diag + node: cpu_monitor + name: CPU Frequency + timeout: 3.0 + - path: /hardware/hdd/001-temperature + type: diag + node: hdd_monitor + name: HDD Temperature + timeout: 3.0 + - path: /hardware/hdd/002-usage + type: diag + node: hdd_monitor + name: HDD Usage + timeout: 3.0 + - path: /hardware/hdd/003-connection + type: diag + node: hdd_monitor + name: HDD Connection + timeout: 3.0 + - path: /hardware/hdd/004-total_written + type: diag + node: hdd_monitor + name: HDD TotalDataWritten + timeout: 3.0 + - path: /hardware/hdd/005-warranty_period + type: diag + node: hdd_monitor + name: HDD PowerOnHours + timeout: 3.0 + - path: /hardware/hdd/006-soft_error + type: diag + node: hdd_monitor + name: HDD RecoveredError + timeout: 3.0 + - path: /hardware/hdd/007-load/read_rate + type: diag + node: hdd_monitor + name: HDD ReadDataRate + timeout: 3.0 + - path: /hardware/hdd/007-load/write_rate + type: diag + node: hdd_monitor + name: HDD WriteDataRate + timeout: 3.0 + - path: /hardware/hdd/007-load/read_iops + type: diag + node: hdd_monitor + name: HDD ReadIOPS + timeout: 3.0 + - path: /hardware/hdd/007-load/write_iops + type: diag + node: hdd_monitor + name: HDD WriteIOPS + timeout: 3.0 + - path: /hardware/memory/001-usage + type: diag + node: mem_monitor + name: Memory Usage + timeout: 3.0 + - path: /hardware/memory/002-ecc + type: diag + node: mem_monitor + name: Memory ECC + timeout: 3.0 + - path: /hardware/network/001-usage + type: diag + node: net_monitor + name: Network Usage + timeout: 3.0 + - path: /hardware/network/002-traffic + type: diag + node: net_monitor + name: Network Traffic + timeout: 3.0 + - path: /hardware/network/003-crc + type: diag + node: net_monitor + name: Network CRC Error + timeout: 3.0 + - 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 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..45adcac68e --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/localization.yaml @@ -0,0 +1,99 @@ +units: + - 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 + + - 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_initialpose3d + name: localization_topic_status + timeout: 1.0 + + - 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: /localization/002-tf + type: diag + node: topic_state_monitor_transform_map_to_base_link + name: localization_topic_status + timeout: 1.0 + + - path: /localization/003-matching_score + type: diag + node: ndt_scan_matcher + name: scan_matching_status + timeout: 1.0 + + - path: /localization/004-accuracy + type: diag + node: localization + name: localization_error_monitor + 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 new file mode 100644 index 0000000000..685956f160 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/map.yaml @@ -0,0 +1,60 @@ +units: + - 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 + + - 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: /map/001-topic_status/pointcloud_map + type: diag + node: topic_state_monitor_pointcloud_map + name: map_topic_status + 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 new file mode 100644 index 0000000000..0c6e32a2ce --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/others.yaml @@ -0,0 +1,163 @@ +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 + + - 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 + 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 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..273e7ada84 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/perception.yaml @@ -0,0 +1,86 @@ +units: + - 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 + + - 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: /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 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..e983c9e799 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/planning.yaml @@ -0,0 +1,203 @@ +units: + - 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 + + - path: /planning/001-topic_status/route-error + type: warn-to-ok + item: + type: link + link: /planning/001-topic_status/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: /planning/001-topic_status/trajectory + type: diag + node: topic_state_monitor_scenario_planning_trajectory + name: planning_topic_status + timeout: 1.0 + + - path: /planning/003-trajectory_finite_validation + type: diag + node: planning_validator + name: trajectory_validation_finite + timeout: 1.0 + + - path: /planning/004-trajectory_interval_validation + type: diag + node: planning_validator + name: trajectory_validation_interval + timeout: 1.0 + + - path: /planning/005-trajectory_curvature_validation + type: diag + node: planning_validator + name: trajectory_validation_curvature + timeout: 1.0 + + - path: /planning/006-trajectory_relative_angle_validation + type: diag + node: planning_validator + name: trajectory_validation_relative_angle + timeout: 1.0 + + - path: /planning/007-trajectory_lateral_acceleration_validation + type: diag + node: planning_validator + name: trajectory_validation_lateral_acceleration + timeout: 1.0 + + - path: /planning/008-trajectory_acceleration_validation + type: diag + node: planning_validator + name: trajectory_validation_acceleration + timeout: 1.0 + + - path: /planning/009-trajectory_deceleration_validation + type: diag + node: planning_validator + name: trajectory_validation_deceleration + timeout: 1.0 + + - path: /planning/010-trajectory_steering_validation + type: diag + node: planning_validator + name: trajectory_validation_steering + timeout: 1.0 + + - path: /planning/011-trajectory_steering_rate_validation + type: diag + node: planning_validator + name: trajectory_validation_steering_rate + timeout: 1.0 + + - 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 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..748d9f4e42 --- /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 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..02fa87326f --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/system.yaml @@ -0,0 +1,89 @@ +units: + - 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 } + + - 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: /system/001-topic_status + type: diag + node: topic_state_monitor_system_emergency_control_cmd + name: system_topic_status + timeout: 1.0 + + - 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: "" + name: rosbag_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: "" + 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 new file mode 100644 index 0000000000..48c0ac97b4 --- /dev/null +++ b/autoware_launch/config/system/system_diagnostic_monitor/vehicle.yaml @@ -0,0 +1,88 @@ +units: + - 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 + + - 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: /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 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 c1821a05ab..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 + # /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: 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 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 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 diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 38f8550f72..3376d6f700 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -8,7 +8,7 @@ - + @@ -22,6 +22,8 @@ + + @@ -39,12 +41,13 @@ - + - + + @@ -121,6 +124,20 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/launch/autoware.sub.launch.xml b/autoware_launch/launch/autoware.sub.launch.xml new file mode 100644 index 0000000000..873db6a22d --- /dev/null +++ b/autoware_launch/launch/autoware.sub.launch.xml @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index e13b987607..c98357d429 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -24,6 +24,15 @@ + + + + + + + + + + + - + diff --git a/autoware_launch/launch/components/tier4_simulator_component.launch.xml b/autoware_launch/launch/components/tier4_simulator_component.launch.xml index a0eb99c7a7..998cfc7cd4 100644 --- a/autoware_launch/launch/components/tier4_simulator_component.launch.xml +++ b/autoware_launch/launch/components/tier4_simulator_component.launch.xml @@ -27,6 +27,8 @@ + + - + + diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml index d05d0acb62..d1e7d7a6e8 100644 --- a/autoware_launch/launch/e2e_simulator.launch.xml +++ b/autoware_launch/launch/e2e_simulator.launch.xml @@ -9,7 +9,7 @@ - + @@ -81,6 +81,8 @@ + + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 8780578a6e..fa33147824 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -8,7 +8,7 @@ - + @@ -33,7 +33,9 @@ - + + + @@ -69,6 +71,10 @@ + + + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index d038e7096b..8aad52f2b7 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -7,7 +7,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -61,7 +61,7 @@ - + @@ -72,6 +72,11 @@ + + + + + diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index ae20a8f22d..d5d13aa3d2 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