From d0c36fda19288ce27950c0a830f20c320e833abb Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 25 Jul 2024 11:34:22 +0900 Subject: [PATCH] feat: add x2 gen2 files (#269) * feat: add aip_x2_gen2 files Signed-off-by: Tomohito ANDO * rename for gen2 Signed-off-by: Tomohito ANDO --------- Signed-off-by: Tomohito ANDO --- aip_x2_gen2_description/CMakeLists.txt | 11 + .../config/sensor_kit_calibration.yaml | 173 +++++++ .../config/sensors_calibration.yaml | 57 +++ aip_x2_gen2_description/package.xml | 17 + aip_x2_gen2_description/urdf/gnss.xacro | 24 + aip_x2_gen2_description/urdf/radar.xacro | 31 ++ aip_x2_gen2_description/urdf/sensor_kit.xacro | 345 ++++++++++++++ aip_x2_gen2_description/urdf/sensors.xacro | 87 ++++ aip_x2_gen2_launch/CMakeLists.txt | 16 + .../blockage_diagnostics_param_file.yaml | 14 + .../sensor_kit.param.yaml | 87 ++++ .../config/dual_return_filter.param.yaml | 13 + .../sensor_kit.param.yaml | 95 ++++ .../config/mosaic_x5_rover.param.yaml | 82 ++++ .../config/simple_object_merger.param.yaml | 6 + .../data/traffic_light_camera.yaml | 20 + aip_x2_gen2_launch/launch/gnss.launch.xml | 29 ++ .../launch/hesai_OT128.launch.xml | 69 +++ .../launch/hesai_QT128.launch.xml | 68 +++ aip_x2_gen2_launch/launch/imu.launch.xml | 43 ++ aip_x2_gen2_launch/launch/lidar.launch.xml | 286 ++++++++++++ .../launch/nebula_node_container.launch.py | 442 ++++++++++++++++++ .../launch/pandar_node_container.launch.py | 364 +++++++++++++++ .../launch/pointcloud_preprocessor.launch.py | 93 ++++ aip_x2_gen2_launch/launch/radar.launch.xml | 214 +++++++++ aip_x2_gen2_launch/launch/sensing.launch.xml | 40 ++ .../launch/topic_state_monitor.launch.py | 356 ++++++++++++++ aip_x2_gen2_launch/package.xml | 31 ++ 28 files changed, 3113 insertions(+) create mode 100644 aip_x2_gen2_description/CMakeLists.txt create mode 100644 aip_x2_gen2_description/config/sensor_kit_calibration.yaml create mode 100644 aip_x2_gen2_description/config/sensors_calibration.yaml create mode 100644 aip_x2_gen2_description/package.xml create mode 100644 aip_x2_gen2_description/urdf/gnss.xacro create mode 100644 aip_x2_gen2_description/urdf/radar.xacro create mode 100644 aip_x2_gen2_description/urdf/sensor_kit.xacro create mode 100644 aip_x2_gen2_description/urdf/sensors.xacro create mode 100644 aip_x2_gen2_launch/CMakeLists.txt create mode 100644 aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml create mode 100644 aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml create mode 100644 aip_x2_gen2_launch/config/dual_return_filter.param.yaml create mode 100644 aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml create mode 100644 aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml create mode 100644 aip_x2_gen2_launch/config/simple_object_merger.param.yaml create mode 100644 aip_x2_gen2_launch/data/traffic_light_camera.yaml create mode 100644 aip_x2_gen2_launch/launch/gnss.launch.xml create mode 100644 aip_x2_gen2_launch/launch/hesai_OT128.launch.xml create mode 100644 aip_x2_gen2_launch/launch/hesai_QT128.launch.xml create mode 100644 aip_x2_gen2_launch/launch/imu.launch.xml create mode 100644 aip_x2_gen2_launch/launch/lidar.launch.xml create mode 100644 aip_x2_gen2_launch/launch/nebula_node_container.launch.py create mode 100644 aip_x2_gen2_launch/launch/pandar_node_container.launch.py create mode 100644 aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py create mode 100644 aip_x2_gen2_launch/launch/radar.launch.xml create mode 100644 aip_x2_gen2_launch/launch/sensing.launch.xml create mode 100644 aip_x2_gen2_launch/launch/topic_state_monitor.launch.py create mode 100644 aip_x2_gen2_launch/package.xml diff --git a/aip_x2_gen2_description/CMakeLists.txt b/aip_x2_gen2_description/CMakeLists.txt new file mode 100644 index 00000000..68dbdf53 --- /dev/null +++ b/aip_x2_gen2_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_gen2_description) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + urdf + config +) diff --git a/aip_x2_gen2_description/config/sensor_kit_calibration.yaml b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..4a37420d --- /dev/null +++ b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,173 @@ +sensor_kit_base_link: + # left upper + left_upper/lidar_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_lower/lidar_base_link: + x: 0.0 + y: -0.025 + z: -0.115 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_front/camera_link: + x: 0.12758 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 0.85478 # 49 [deg] + left_rear/camera_link: + x: -0.12842 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 2.2678 # 130 [deg] + left_center/camera_link: + x: 0.0 + y: -0.04089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: 1.570796 # 90 [deg] + + # right upper + right_upper/lidar_base_link: + x: 0.0 + y: -2.15178 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_lower/lidar_base_link: + x: 0.0 + y: -2.12678 + z: -0.115 + roll: 0.6981 # 40 [deg] + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_front/camera_link: + x: 0.12758 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -0.85478 # -49 [deg] + right_rear/camera_link: + x: -0.12842 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -2.2678 # -130 [deg] + right_center/camera_link: + x: 0.0 + y: -2.11089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: -1.570796 # -90 [deg] + + # front upper + top_front_left/imu_link: + x: 0.562 + y: -0.99974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/imu_link: + x: 0.562 + y: -1.13974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/camera_link: + x: 0.73758 + y: -1.26439 + z: -0.06666 + roll: 0.0 + pitch: 0.8373 # 48 [deg] + yaw: 0.0 + top_front_center_right/camera_link: + x: 0.73058 + y: -1.18899 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_center_left/camera_link: + x: 0.73058 + y: -1.03819 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_left/camera_link: + x: 0.73058 + y: -0.96279 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front/gnss_link: + x: 0.30133 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # rear upper + top_rear_center/camera_link: + x: -6.06359 + y: -1.20389 + z: -0.14078 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + top_rear/gnss_link: + x: -5.77517 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # front lower + front_upper/lidar_base_link: + x: 0.94058 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + front_lower/lidar_base_link: + x: 0.96758 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + + # rear lower + rear_upper/lidar_base_link: + x: -6.08042 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle + rear_lower/lidar_base_link: + x: -6.10742 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle diff --git a/aip_x2_gen2_description/config/sensors_calibration.yaml b/aip_x2_gen2_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..1b5105f0 --- /dev/null +++ b/aip_x2_gen2_description/config/sensors_calibration.yaml @@ -0,0 +1,57 @@ +base_link: + sensor_kit_base_link: + x: 4.66244 + y: 1.07589 + z: 2.78926 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # radar + front_center/radar_link: + x: 5.69207 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + front_left/radar_link: + x: 5.37204 + y: 1.08537 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: 1.2211 # 70 [deg] + + front_right/radar_link: + x: 5.37204 + y: -1.06642 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: -1.2211 # 70 [deg] + + rear_center/radar_link: + x: -1.50704 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] + + rear_left/radar_link: + x: -1.27564 + y: 1.07767 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: 1.9189 # 110 [deg] + + rear_right/radar_link: + x: -1.27564 + y: -1.07768 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: -1.9189 # 110 [deg] diff --git a/aip_x2_gen2_description/package.xml b/aip_x2_gen2_description/package.xml new file mode 100644 index 00000000..b35ad7a3 --- /dev/null +++ b/aip_x2_gen2_description/package.xml @@ -0,0 +1,17 @@ + + + aip_x2_gen2_description + 0.1.0 + The aip_x2_gen2_description package + + Tomohito Ando + Apache 2 + + ament_cmake_auto + + pandar_description + + + ament_cmake + + diff --git a/aip_x2_gen2_description/urdf/gnss.xacro b/aip_x2_gen2_description/urdf/gnss.xacro new file mode 100644 index 00000000..55002be0 --- /dev/null +++ b/aip_x2_gen2_description/urdf/gnss.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/radar.xacro b/aip_x2_gen2_description/urdf/radar.xacro new file mode 100644 index 00000000..8b0f8d4b --- /dev/null +++ b/aip_x2_gen2_description/urdf/radar.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/sensor_kit.xacro b/aip_x2_gen2_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..2db48d09 --- /dev/null +++ b/aip_x2_gen2_description/urdf/sensor_kit.xacro @@ -0,0 +1,345 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/sensors.xacro b/aip_x2_gen2_description/urdf/sensors.xacro new file mode 100644 index 00000000..34b10729 --- /dev/null +++ b/aip_x2_gen2_description/urdf/sensors.xacro @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/CMakeLists.txt b/aip_x2_gen2_launch/CMakeLists.txt new file mode 100644 index 00000000..f3997ac7 --- /dev/null +++ b/aip_x2_gen2_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_gen2_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + data + config +) diff --git a/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..e43b2039 --- /dev/null +++ b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + blockage_kernel: 10 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 new file mode 100644 index 00000000..6d83d1fc --- /dev/null +++ b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,87 @@ +/**: + ros__parameters: + sensing: + type: diagnostic_aggregator/AnalyzerGroup + path: sensing + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 + + lidar: + type: diagnostic_aggregator/AnalyzerGroup + path: lidar + analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + blockage: + type: diagnostic_aggregator/GenericAnalyzer + path: blockage + contains: [": blockage_validation"] + timeout: 1.0 + visibility: + type: diagnostic_aggregator/GenericAnalyzer + path: visibility + contains: ["left_upper: visibility_validation"] + timeout: 1.0 + concat_status: + type: diagnostic_aggregator/GenericAnalyzer + path: concat_status + contains: [": concat_status"] + timeout: 1.0 + pandar: + type: diagnostic_aggregator/AnalyzerGroup + path: pandar + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": pandar_connection"] + timeout: 5.0 + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": pandar_temperature"] + timeout: 5.0 + ptp: + type: diagnostic_aggregator/GenericAnalyzer + path: ptp + contains: [": pandar_ptp"] + timeout: 5.0 + + dust: + type: diagnostic_aggregator/GenericAnalyzer + path: dust + contains: [": dust_validation"] + timeout: 1.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + septentrio: + type: diagnostic_aggregator/AnalyzerGroup + path: septentrio + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + gnss: + type: diagnostic_aggregator/GenericAnalyzer + path: gnss + startswith: ["gnss"] + contains: [": gnss"] + timeout: 5.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 new file mode 100644 index 00000000..dd68e5ec --- /dev/null +++ b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + roi_mode: "Fixed_azimuth_ROI" # description="options: `No_ROI`, `Fixed_xyz_ROI` or `Fixed_azimuth_ROI`"/> + 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 + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.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 new file mode 100644 index 00000000..58fdc890 --- /dev/null +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -0,0 +1,95 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + # gnss + ## /sensing/gnss/001-connection + "topic_state_monitor_gnss_pose: gnss_topic_status": default + + ## /sensing/gnss/002-quality + "septentrio_driver: Quality indicators": default + + # imu + ## /sensing/imu/001-monitor + "imu_monitor: yaw_rate_status": default + + ## /sensing/imu/002-connection + "topic_state_monitor_imu_data: imu_topic_status": default + + ## /sensing/imu/003-gyro_bias + "gyro_bias_estimator: gyro_bias_validator": default + + # lidar + ## /sensing/lidar/pndr/001-connection + "pandar_monitor: /sensing/lidar/front_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_connection": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_connection": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_connection": default + + ## /sensing/lidar/pndr/002-temperature + "pandar_monitor: /sensing/lidar/front_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/right_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/right_upper: pandar_temperature": default + "pandar_monitor: /sensing/lidar/rear_lower: pandar_temperature": default + "pandar_monitor: /sensing/lidar/rear_upper: pandar_temperature": default + + ## /sensing/lidar/pndr/003-ptp + "pandar_monitor: /sensing/lidar/front_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/front_upper: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_lower: pandar_ptp": default + "pandar_monitor: /sensing/lidar/left_upper: pandar_ptp": default + "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": default + + ## /sensing/camera/001-connection + "topic_state_monitor_camera0: camera0_topic_status": default + "topic_state_monitor_camera1: camera1_topic_status": default + "topic_state_monitor_camera2: camera2_topic_status": default + "topic_state_monitor_camera3: camera3_topic_status": default + "topic_state_monitor_camera4: camera4_topic_status": default + "topic_state_monitor_camera5: camera5_topic_status": default + "topic_state_monitor_camera6: camera6_topic_status": default + "topic_state_monitor_camera7: camera7_topic_status": default + + ## /sensing/radar/001-connection + "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default + "topic_state_monitor_radar_front_left: radar_front_left_topic_status": default + "topic_state_monitor_radar_front_right: radar_front_right_topic_status": default + "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status": default + "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status": default + "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status": default + + ## /others/002-blockage_validation + "blockage_return_diag: /sensing/lidar/front_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/front_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/left_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/left_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/right_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/right_upper: blockage_validation": default + "blockage_return_diag: /sensing/lidar/rear_lower: blockage_validation": default + "blockage_return_diag: /sensing/lidar/rear_upper: blockage_validation": default + + ## /others/004-concat_status + "concatenate_data: concat_status": default + + ## /others/005-visibility_validation/front_lower + "dual_return_filter: /sensing/lidar/front_lower: visibility_validation": default diff --git a/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml new file mode 100644 index 00000000..566ee6ba --- /dev/null +++ b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + device: tcp://192.168.100.101:28784 + + frame_id: gnss_link + aux1_frame_id: aux1 + get_spatial_config_from_tf: false + use_ros_axis_orientation: false + receiver_type: gnss + multi_antenna: false + + datum: Default + + att_offset: + heading: 0.0 + pitch: 0.0 + + ant_type: Unknown + ant_serial_nr: Unknown + ant_aux1_type: Unknown + ant_aux1_serial_nr: Unknown + + polling_period: + pvt: 200 + rest: 200 + + use_gnss_time: false + + rtk_settings: + ntrip_1: + id: "" + caster: "" + caster_port: 2101 + username: "" + password: "" + mountpoint: "" + version: "v2" + tls: false + fingerprint: "" + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + ip_server_1: + id: "IPS1" + port: 28785 + rtk_standard: "RTCMv3" + send_gga: "auto" + keep_open: true + serial_1: + port: "" + baud_rate: 115200 + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + + publish: + # For both GNSS and INS Rxs + navsatfix: true + gpsfix: false + gpgga: false + gprmc: false + gpst: false + measepoch: false + pvtcartesian: false + pvtgeodetic: true + basevectorcart: false + basevectorgeod: false + poscovcartesian: false + poscovgeodetic: true + velcovgeodetic: false + atteuler: false + attcoveuler: false + pose: false + twist: false + diagnostics: true + # For GNSS Rx only + gpgsa: false + gpgsv: false + + # logger + + activate_debug_log: false diff --git a/aip_x2_gen2_launch/config/simple_object_merger.param.yaml b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml new file mode 100644 index 00000000..a817e0b7 --- /dev/null +++ b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: ["/sensing/radar/front_center/detected_objects", "/sensing/radar/front_left/detected_objects", "/sensing/radar/rear_left/detected_objects", "/sensing/radar/rear_center/detected_objects", "/sensing/radar/rear_right/detected_objects", "/sensing/radar/front_right/detected_objects"] diff --git a/aip_x2_gen2_launch/data/traffic_light_camera.yaml b/aip_x2_gen2_launch/data/traffic_light_camera.yaml new file mode 100644 index 00000000..458ad17c --- /dev/null +++ b/aip_x2_gen2_launch/data/traffic_light_camera.yaml @@ -0,0 +1,20 @@ +image_width: 1920 +image_height: 1080 +camera_name: traffic_light/camera +camera_matrix: + rows: 3 + cols: 3 + data: [2410.755261, 0.000000, 922.621401, 0.000000, 2403.573140, 534.752500, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [2370.254883, 0.000000, 920.136018, 0.000000, 0.000000, 2388.885254, 535.599668, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/aip_x2_gen2_launch/launch/gnss.launch.xml b/aip_x2_gen2_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..edcbda19 --- /dev/null +++ b/aip_x2_gen2_launch/launch/gnss.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml new file mode 100644 index 00000000..d4a0a909 --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml new file mode 100644 index 00000000..ccd227fb --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/imu.launch.xml b/aip_x2_gen2_launch/launch/imu.launch.xml new file mode 100644 index 00000000..b8a3ee81 --- /dev/null +++ b/aip_x2_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..29ecbc05 --- /dev/null +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,286 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py new file mode 100644 index 00000000..710f4339 --- /dev/null +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,442 @@ +# Copyright 2023 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + + +def get_lidar_make(sensor_name): + if sensor_name[:6].lower() == "pandar": + return "Hesai", ".csv" + elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: + return "Velodyne", ".yaml" + return "unrecognized_sensor_model" + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def get_vehicle_mirror_info(context): + path = LaunchConfiguration("vehicle_mirror_param_file").perform(context) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + # Model and make + sensor_model = LaunchConfiguration("sensor_model").perform(context) + sensor_make, sensor_extension = get_lidar_make(sensor_model) + nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") + + # Calibration file + sensor_calib_fp = os.path.join( + nebula_decoders_share_dir, + "calibration", + sensor_make.lower(), + sensor_model + sensor_extension, + ) + assert os.path.exists( + sensor_calib_fp + ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp) + + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + nebula_ros_hw_monitor_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hardware_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": LaunchConfiguration("sensor_model"), + "return_mode": LaunchConfiguration("return_mode"), + "frame_id": LaunchConfiguration("frame_id"), + "scan_phase": LaunchConfiguration("scan_phase"), + "sensor_ip": LaunchConfiguration("sensor_ip"), + "host_ip": LaunchConfiguration("host_ip"), + "data_port": LaunchConfiguration("data_port"), + "gnss_port": LaunchConfiguration("gnss_port"), + "packet_mtu_size": LaunchConfiguration("packet_mtu_size"), + "rotation_speed": LaunchConfiguration("rotation_speed"), + "cloud_min_angle": LaunchConfiguration("cloud_min_angle"), + "cloud_max_angle": LaunchConfiguration("cloud_max_angle"), + "diag_span": LaunchConfiguration("diag_span"), + "dual_return_distance_threshold": LaunchConfiguration( + "dual_return_distance_threshold" + ), + "delay_monitor_ms": LaunchConfiguration("delay_monitor_ms"), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + nebula_ros_hw_interface_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwInterfaceRosWrapper", + name=sensor_make.lower() + "_hw_interface_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + "calibration_file": sensor_calib_fp, + **create_parameter_dict( + "sensor_ip", + "host_ip", + "scan_phase", + "return_mode", + "frame_id", + "rotation_speed", + "data_port", + "cloud_min_angle", + "cloud_max_angle", + "dual_return_distance_threshold", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + nebula_ros_driver_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "DriverRosWrapper", + name=sensor_make.lower() + "_driver_ros_wrapper_node", + parameters=[ + { + "calibration_file": sensor_calib_fp, + "sensor_model": sensor_model, + **create_parameter_dict( + "host_ip", + "sensor_ip", + "data_port", + "return_mode", + "min_range", + "max_range", + "frame_id", + "scan_phase", + "dual_return_distance_threshold", + ), + "launch_hw": True, + }, + ], + remappings=[ + ("aw_points", "pointcloud_raw"), + ("aw_points_ex", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + self_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + mirror_info = load_composable_node_param("vehicle_mirror_param_file") + right = mirror_info["right"] + cropbox_parameters.update( + min_x=right["min_longitudinal_offset"], + max_x=right["max_longitudinal_offset"], + min_y=right["min_lateral_offset"], + max_y=right["max_lateral_offset"], + min_z=right["min_height_offset"], + max_z=right["max_height_offset"], + ) + + right_mirror_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_right", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "right_mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + left = mirror_info["left"] + cropbox_parameters.update( + min_x=left["min_longitudinal_offset"], + max_x=left["max_longitudinal_offset"], + min_y=left["min_lateral_offset"], + max_y=left["max_lateral_offset"], + min_z=left["min_height_offset"], + max_z=left["max_height_offset"], + ) + + left_mirror_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_left", + remappings=[ + ("input", "right_mirror_cropped/pointcloud_ex"), + ("output", "mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + undistort_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/velocity_report", "/vehicle/status/velocity_status"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": LaunchConfiguration("blockage_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="pandar_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + glog_component, + nebula_ros_driver_component, + self_crop_component, + right_mirror_crop_component, + left_mirror_crop_component, + undistort_component, + ], + ) + + driver_loader = LoadComposableNodes( + composable_node_descriptions=[ + nebula_ros_hw_interface_component, + nebula_ros_hw_monitor_component, + ], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + driver_loader, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_loader, + ] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("sensor_model", description="sensor model name") + add_launch_arg("config_file", "", description="sensor configuration file") + add_launch_arg("launch_driver", "True", "do launch driver") + add_launch_arg("setup_sensor", "True", "configure sensor") + add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") + add_launch_arg("host_ip", "255.255.255.255", "host ip address") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("base_frame", "base_link", "base frame id") + add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") + add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") + add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") + add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") + add_launch_arg("data_port", "2368", "device data port number") + add_launch_arg("gnss_port", "2380", "device gnss port number") + add_launch_arg("packet_mtu_size", "1500", "packet mtu size") + add_launch_arg("rotation_speed", "600", "rotational frequency") + add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") + add_launch_arg("frame_id", "lidar", "frame id") + add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg( + "vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml" + ) + add_launch_arg("diag_span", "1000") + add_launch_arg("delay_monitor_ms", "2000") + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + + add_launch_arg("dual_return_filter_param_file") + add_launch_arg("blockage_diagnostics_param_file") + + add_launch_arg("vertical_bins", "40") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("blockage_range", "[270.0, 90.0]") + + add_launch_arg("min_azimuth_deg", "135.0") + add_launch_arg("max_azimuth_deg", "225.0") + add_launch_arg("enable_blockage_diag", "true") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x2_gen2_launch/launch/pandar_node_container.launch.py b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py new file mode 100644 index 00000000..8d8b13cf --- /dev/null +++ b/aip_x2_gen2_launch/launch/pandar_node_container.launch.py @@ -0,0 +1,364 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def get_pandar_monitor_info(): + path = os.path.join( + get_package_share_directory("pandar_monitor"), + "config", + "pandar_monitor.param.yaml", + ) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + +def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = -0.3 # margin to crop pointcloud under vehicle + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + monitor_node = Node( + executable="pandar_monitor_node", + package="pandar_monitor", + name="pandar_monitor", + parameters=[ + { + "ip_address": LaunchConfiguration("device_ip"), + } + ] + + [get_pandar_monitor_info()], + condition=IfCondition(LaunchConfiguration("launch_driver")), + output="screen", + ) + + driver_component = ComposableNode( + package="pandar_driver", + plugin="pandar_driver::PandarDriver", + name="pandar_driver", + parameters=[ + { + **create_parameter_dict( + "pcap", "device_ip", "lidar_port", "gps_port", "scan_phase", "model", "frame_id" + ) + } + ], + ) + + pointcloud_component = ComposableNode( + package="pandar_pointcloud", + plugin="pandar_pointcloud::PandarCloud", + name="pandar_cloud", + parameters=[ + { + **create_parameter_dict( + "model", + "scan_phase", + "angle_range", + "distance_range", + "device_ip", + "calibration", + "return_mode", + ) + } + ], + remappings=[("pandar_points", "pointcloud_raw"), ("pandar_points_ex", "pointcloud_raw_ex")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + self_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + mirror_info = load_composable_node_param("vehicle_mirror_param_file") + right = mirror_info["right"] + cropbox_parameters.update( + min_x=right["min_longitudinal_offset"], + max_x=right["max_longitudinal_offset"], + min_y=right["min_lateral_offset"], + max_y=right["max_lateral_offset"], + min_z=right["min_height_offset"], + max_z=right["max_height_offset"], + ) + + right_mirror_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_right", + remappings=[ + ("input", "self_cropped/pointcloud_ex"), + ("output", "right_mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + left = mirror_info["left"] + cropbox_parameters.update( + min_x=left["min_longitudinal_offset"], + max_x=left["max_longitudinal_offset"], + min_y=left["min_lateral_offset"], + max_y=left["max_lateral_offset"], + min_z=left["min_height_offset"], + max_z=left["max_height_offset"], + ) + + left_mirror_crop_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_mirror_left", + remappings=[ + ("input", "right_mirror_cropped/pointcloud_ex"), + ("output", "mirror_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + undistort_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/velocity_report", "/vehicle/status/velocity_status"), + ("~/input/pointcloud", "mirror_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) + blockage_diag_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": LaunchConfiguration("blockage_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="pandar_node_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + pointcloud_component, + self_crop_component, + right_mirror_crop_component, + left_mirror_crop_component, + undistort_component, + ], + ) + + driver_loader = LoadComposableNodes( + composable_node_descriptions=[driver_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("launch_driver")), + ) + + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + driver_loader, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_loader, + monitor_node, + ] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("model") + add_launch_arg("launch_driver", "true") + add_launch_arg("calibration", "") + add_launch_arg("device_ip", "192.168.1.201") + add_launch_arg("scan_phase", "0.0") + add_launch_arg("angle_range", "[270.0, 90.0]") + add_launch_arg("distance_range", "[0.1, 200.0]") + add_launch_arg("return_mode", "Dual") + add_launch_arg("base_frame", "base_link") + add_launch_arg("container_name", "pandar_composable_node_container") + add_launch_arg("pcap", "") + add_launch_arg("lidar_port", "2321") + add_launch_arg("gps_port", "10121") + add_launch_arg("frame_id", "pandar") + add_launch_arg("input_frame", LaunchConfiguration("base_frame")) + add_launch_arg("output_frame", LaunchConfiguration("base_frame")) + add_launch_arg("dual_return_filter_param_file") + add_launch_arg("horizontal_resolution", "0.4") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"], + ) + add_launch_arg("vehicle_mirror_param_file") + add_launch_arg("use_multithread", "true") + add_launch_arg("use_intra_process", "true") + add_launch_arg("vertical_bins", "40") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("blockage_range", "[270.0, 90.0]") + + add_launch_arg("min_azimuth_deg", "135.0") + add_launch_arg("max_azimuth_deg", "225.0") + add_launch_arg("enable_blockage_diag", "true") + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..ef03f232 --- /dev/null +++ b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,93 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + # set concat filter as a component + concat_component = ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud"), + ], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/front_upper/pointcloud", + "/sensing/lidar/front_lower/pointcloud", + "/sensing/lidar/left_upper/pointcloud", + "/sensing/lidar/left_lower/pointcloud", + "/sensing/lidar/right_upper/pointcloud", + "/sensing/lidar/right_lower/pointcloud", + "/sensing/lidar/rear_upper/pointcloud", + "/sensing/lidar/rear_lower/pointcloud", + ], + "input_offset": [0.005, 0.025, 0.050, 0.005, 0.050, 0.005, 0.005, 0.025], + "timeout_sec": 0.075, + "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "twist", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=LaunchConfiguration("pointcloud_container_name"), + ) + + return [concat_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("use_multithread", "True") + add_launch_arg("use_intra_process", "True") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..c5214773 --- /dev/null +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..c50b68be --- /dev/null +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py new file mode 100644 index 00000000..ff3c46d1 --- /dev/null +++ b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py @@ -0,0 +1,356 @@ +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # GNSS topic monitor + gnss_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_gnss_pose", + parameters=[ + { + "topic": "/sensing/gnss/pose", + "topic_type": "geometry_msgs/msg/PoseStamped", + "best_effort": True, + "diag_name": "gnss_topic_status", + "warn_rate": 2.5, + "error_rate": 0.5, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # IMU topic monitor + imu_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_imu_data", + parameters=[ + { + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": True, + "diag_name": "imu_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Radar topic monitors + radar_front_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_center", + parameters=[ + { + "topic": "/sensing/radar/front_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_left", + parameters=[ + { + "topic": "/sensing/radar/front_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_right", + parameters=[ + { + "topic": "/sensing/radar/front_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_center", + parameters=[ + { + "topic": "/sensing/radar/rear_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_left", + parameters=[ + { + "topic": "/sensing/radar/rear_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_right", + parameters=[ + { + "topic": "/sensing/radar/rear_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + 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", + namespace="topic_state_monitor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + gnss_topic_monitor, + imu_topic_monitor, + radar_front_center_monitor, + radar_front_left_monitor, + radar_front_right_monitor, + 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", + ) + + return LaunchDescription([container]) diff --git a/aip_x2_gen2_launch/package.xml b/aip_x2_gen2_launch/package.xml new file mode 100644 index 00000000..2743c6a8 --- /dev/null +++ b/aip_x2_gen2_launch/package.xml @@ -0,0 +1,31 @@ + + + + aip_x2_gen2_launch + 0.1.0 + The aip_x2_gen2_launch package + + Tomohito Ando + Apache License 2.0 + + ament_cmake_auto + + common_sensor_launch + dummy_diag_publisher + gnss_poser + imu_corrector + pointcloud_preprocessor + septentrio_gnss_driver + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + vehicle_velocity_converter + + ament_lint_auto + autoware_lint_common + + + ament_cmake + +