From d2188112962f2e8f91a34d84fa2dc98ea4490d83 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Sat, 31 Aug 2024 17:45:04 +0900 Subject: [PATCH] chore: sync config files in gen1 to gen2 Signed-off-by: Tomohito ANDO --- .../sensor_kit.param.yaml | 13 ++ .../config/dual_return_filter.param.yaml | 2 +- .../sensor_kit.param.yaml | 2 +- .../config/gyro_bias_estimator.param.yaml | 6 + .../launch/topic_state_monitor.launch.py | 161 ------------------ 5 files changed, 21 insertions(+), 163 deletions(-) create mode 100644 aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml diff --git a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml index 6d83d1fc..f2ffdcbd 100644 --- a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -85,3 +85,16 @@ startswith: ["gnss"] contains: [": gnss"] timeout: 5.0 + imu: + type: diagnostic_aggregator/AnalyzerGroup + path: imu + analyzers: + bias_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: bias_monitoring + analyzers: + gyro_bias_validator: + type: diagnostic_aggregator/GenericAnalyzer + path: gyro_bias_validator + contains: [": gyro_bias_validator"] + timeout: 1.0 diff --git a/aip_x2_gen2_launch/config/dual_return_filter.param.yaml b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml index dd68e5ec..75168e7b 100644 --- a/aip_x2_gen2_launch/config/dual_return_filter.param.yaml +++ b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml @@ -4,7 +4,7 @@ weak_first_local_noise_threshold: 2 # description="for No_ROI roi_mode, recommended value is 10" /> visibility_error_threshold: 0.95 visibility_warn_threshold: 0.97 - max_distance: 5.0 + max_distance: 10.0 x_max: 18.0 x_min: -12.0 y_max: 2.0 diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml index b5b53c45..dd09b44e 100644 --- a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -38,7 +38,7 @@ "pandar_monitor: /sensing/lidar/right_lower: pandar_ptp": default "pandar_monitor: /sensing/lidar/right_upper: pandar_ptp": default "pandar_monitor: /sensing/lidar/rear_lower: pandar_ptp": default - "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": defaultlt + "pandar_monitor: /sensing/lidar/rear_upper: pandar_ptp": default "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default diff --git a/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py index ff3c46d1..0eafa9e0 100644 --- a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py +++ b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py @@ -173,159 +173,6 @@ def generate_launch_description(): extra_arguments=[{"use_intra_process_comms": True}], ) - # Camera topic monitors - camera0_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera0", - parameters=[ - { - "topic": "/sensing/camera/camera0/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera0_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera1_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera1", - parameters=[ - { - "topic": "/sensing/camera/camera1/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera1_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera2_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera2", - parameters=[ - { - "topic": "/sensing/camera/camera2/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera2_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera3_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera3", - parameters=[ - { - "topic": "/sensing/camera/camera3/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera3_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera4_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera4", - parameters=[ - { - "topic": "/sensing/camera/camera4/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera4_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera5_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera5", - parameters=[ - { - "topic": "/sensing/camera/camera5/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera5_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera6_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera6", - parameters=[ - { - "topic": "/sensing/camera/camera6/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera6_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - camera7_topic_monitor = ComposableNode( - package="topic_state_monitor", - plugin="topic_state_monitor::TopicStateMonitorNode", - name="topic_state_monitor_camera7", - parameters=[ - { - "topic": "/sensing/camera/camera7/camera_info", - "topic_type": "sensor_msgs/msg/CameraInfo", - "best_effort": True, - "diag_name": "camera7_topic_status", - "warn_rate": 5.0, - "error_rate": 1.0, - "timeout": 5.0, - "window_size": 1, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - # ComposableNodeContainer to run all ComposableNodes container = ComposableNodeContainer( name="topic_state_monitor_container", @@ -341,14 +188,6 @@ def generate_launch_description(): radar_rear_center_monitor, radar_rear_left_monitor, radar_rear_right_monitor, - camera0_topic_monitor, - camera1_topic_monitor, - camera2_topic_monitor, - camera3_topic_monitor, - camera4_topic_monitor, - camera5_topic_monitor, - camera6_topic_monitor, - camera7_topic_monitor, ], output="screen", )