diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt new file mode 100644 index 00000000..549de0f8 --- /dev/null +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_xx1_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_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..88288533 --- /dev/null +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,113 @@ +sensor_kit_base_link: + camera0/camera_link: + x: 0.372 # Design Value + y: 0.0 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera1/camera_link: + x: 0.372 # Design Value + y: 0.045 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera2/camera_link: + x: 0.372 # Design Value + y: -0.045 # Design Value + z: -0.207 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + camera3/camera_link: + x: 0.133 # Design Value + y: 0.498 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.872665 # Design Value + camera4/camera_link: + x: 0.133 # Design Value + y: -0.498 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -0.872665 # Design Value + camera5/camera_link: + x: 0.095 # Design Value + y: 0.524 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 1.0472 # Design Value + camera6/camera_link: + x: 0.095 # Design Value + y: -0.524 # Design Value + z: -0.246 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -1.0472 # Design Value + camera7/camera_link: + x: -0.345 # Design Value + y: 0.244 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.70526 # Design Value + camera8/camera_link: + x: -0.345 # Design Value + y: -0.244 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.70526 # Design Value + camera9/camera_link: + x: -0.362 # Design Value + y: 0.202 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.79253 # Design Value + camera10/camera_link: + x: -0.362 # Design Value + y: -0.202 # Design Value + z: -0.174 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.79253 # Design Value + hesai_top_base_link: + x: 0.0 # Design Value + y: 0.0 # Design Value + z: 0.0 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 4.36332298038 # Design Value + hesai_left_base_link: + x: 0.0 # Design Value + y: 0.564 # Design Value + z: -0.300 # Design Value + roll: 0.872665 # Design Value + pitch: 0.0 # Design Value + yaw: 3.14159265359 # Design Value + hesai_right_base_link: + x: 0.0 # Design Value + y: -0.564 # Design Value + z: -0.300 # Design Value + roll: 0.69813132679 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + gnss_link: + x: -0.279 # Design Value + y: 0.0 # Design Value + z: -0.160 # Design Value + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + tamagawa/imu_link: + x: -0.129 # Design Value + y: 0.0 # Design Value + z: -0.160 # Design Value + roll: 3.14159265359 + pitch: 0.0 # Design Value + yaw: 3.14159265359 # Design Value diff --git a/aip_xx1_gen2_description/config/sensors_calibration.yaml b/aip_xx1_gen2_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..a57d3ea9 --- /dev/null +++ b/aip_xx1_gen2_description/config/sensors_calibration.yaml @@ -0,0 +1,71 @@ +base_link: + sensor_kit_base_link: + x: 0.9 + y: 0.0 + z: 2.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + hesai_front_left_base_link: + x: 3.373 # Design Value + y: 0.740 # Design Value + z: 0.5482 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.44346132679 # Design Value + hesai_front_right_base_link: + x: 3.373 # Design Value + y: -0.740 # Design Value + z: 0.5482 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.69813132679 # Design Value + # velodyne_rear_base_link: #unused + # x: -0.358 + # y: 0.0 + # z: 1.631 + # roll: -0.02 + # pitch: 0.7281317 + # yaw: 3.141592 + front_center/radar_link: + x: 3.520 # Design Value + y: 0.0 # Design Value + z: 0.6352 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 0.0 # Design Value + front_right/radar_link: + x: 3.384 # Design Value + y: -0.7775 # Design Value + z: 0.410 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -1.22173 # Design Value + front_left/radar_link: + x: 3.384 # Design Value + y: 0.7775 # Design Value + z: 0.410 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 1.22173 # Design Value + rear_center/radar_link: + x: -0.858 # Design Value + y: 0.0 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 3.141592 # Design Value + rear_right/radar_link: + x: -0.782 # Design Value + y: -0.761 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: -2.0944 # Design Value + rear_left/radar_link: + x: -0.782 # Design Value + y: 0.761 # Design Value + z: 0.520 + roll: 0.0 # Design Value + pitch: 0.0 # Design Value + yaw: 2.0944 # Design Value diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml new file mode 100644 index 00000000..9b010d72 --- /dev/null +++ b/aip_xx1_gen2_description/package.xml @@ -0,0 +1,17 @@ + + + aip_xx1_gen2_description + 0.1.0 + The aip_xx1_gen2_description package + + Yukihiro Saito + Apache 2 + + ament_cmake_auto + + velodyne_description + + + ament_cmake + + diff --git a/aip_xx1_gen2_description/urdf/sensor_kit.xacro b/aip_xx1_gen2_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..137b3589 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/sensor_kit.xacro @@ -0,0 +1,253 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_description/urdf/sensors.xacro b/aip_xx1_gen2_description/urdf/sensors.xacro new file mode 100644 index 00000000..81faabe8 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/sensors.xacro @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt new file mode 100644 index 00000000..7f8d9f60 --- /dev/null +++ b/aip_xx1_gen2_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_xx1_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_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 00000000..56fc9dbd --- /dev/null +++ b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,118 @@ +/**: + 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 + velodyne: + type: diagnostic_aggregator/AnalyzerGroup + path: velodyne + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": velodyne_connection"] + timeout: 3.0 + + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": velodyne_temperature"] + timeout: 3.0 + + rpm: + type: diagnostic_aggregator/GenericAnalyzer + path: rpm + contains: [": velodyne_rpm"] + timeout: 3.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": gnss_connection"] + timeout: 3.0 + + data: + type: diagnostic_aggregator/GenericAnalyzer + path: data + contains: [": gnss_data"] + timeout: 3.0 + + antenna: + type: diagnostic_aggregator/GenericAnalyzer + path: antenna + contains: [": gnss_antenna"] + timeout: 3.0 + + tx_usage: + type: diagnostic_aggregator/GenericAnalyzer + path: tx_usage + contains: [": gnss_tx_usage"] + timeout: 3.0 + + spoofing: + type: diagnostic_aggregator/GenericAnalyzer + path: spoofing + contains: [": gnss_spoofing"] + timeout: 3.0 + + jamming: + type: diagnostic_aggregator/GenericAnalyzer + path: jamming + contains: [": gnss_jamming"] + timeout: 3.0 + + fix_topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: fix_topic_status + contains: [": fix topic status"] + timeout: 3.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_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml new file mode 100644 index 00000000..80cc7944 --- /dev/null +++ b/aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -0,0 +1,30 @@ +# 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 + gnss_connection: default + gnss_data: default + gnss_antenna: default + gnss_tx_usage: default + gnss_spoofing: default + gnss_jamming: default + fix topic status: default + + # velodyne + velodyne_connection: default + velodyne_temperature: default + velodyne_rpm: default + + # imu + gyro_bias_estimator: default diff --git a/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_xx1_gen2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_xx1_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_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml new file mode 100644 index 00000000..fcfefdd4 --- /dev/null +++ b/aip_xx1_gen2_launch/config/lidar_gen2.yaml @@ -0,0 +1,69 @@ +launches: + - sensor_type: hesai_OT128 + namespace: top + parameters: + max_range: 300.0 + sensor_frame: hesai_top + sensor_ip: 192.168.1.201 + data_port: 2368 + scan_phase: 160 + vertical_bins: 128 + - sensor_type: hesai_XT32 + namespace: front_left + parameters: + max_range: 300.0 + sensor_frame: hesai_front_left + sensor_ip: 192.168.1.21 + data_port: 2369 + scan_phase: 50.0 + cloud_min_angle: 50 + cloud_max_angle: 320 + vertical_bins: 16 + horizontal_ring_id: 0 + - sensor_type: hesai_XT32 + namespace: front_right + parameters: + max_range: 300.0 + sensor_frame: hesai_front_right + sensor_ip: 192.168.1.22 + data_port: 2370 + scan_phase: 310.0 + cloud_min_angle: 40 + cloud_max_angle: 310 + - sensor_type: hesai_XT32 + namespace: side_left + parameters: + max_range: 10.0 + sensor_frame: hesai_side_left + sensor_ip: 192.168.1.23 + data_port: 2371 + scan_phase: 90.0 + cloud_min_angle: 90 + cloud_max_angle: 270 + - sensor_type: hesai_XT32 + namespace: side_right + parameters: + max_range: 10.0 + sensor_frame: hesai_side_right + sensor_ip: 192.168.1.24 + data_port: 2372 + scan_phase: 270.0 + cloud_min_angle: 90 + cloud_max_angle: 270 + +preprocessor: + input_topics: + - /sensing/lidar/top/pointcloud_before_sync + - /sensing/lidar/side_left/pointcloud_before_sync + - /sensing/lidar/side_right/pointcloud_before_sync + - /sensing/lidar/front_left/pointcloud_before_sync + - /sensing/lidar/front_right/pointcloud_before_sync + input_offset: + - 0.035 + - 0.025 + - 0.025 + - 0.025 + - 0.025 + timeout_sec: 0.095 + input_twist_topic_type: twist + publish_synchronized_pointcloud: true diff --git a/aip_xx1_gen2_launch/config/lidar_launch.yaml b/aip_xx1_gen2_launch/config/lidar_launch.yaml new file mode 100644 index 00000000..c702f766 --- /dev/null +++ b/aip_xx1_gen2_launch/config/lidar_launch.yaml @@ -0,0 +1,70 @@ +launches: + - sensor_type: velodyne_VLS128 + namespace: top + parameters: + max_range: 250.0 + sensor_frame: velodyne_top + sensor_ip: 192.168.1.201 + data_port: 2368 + scan_phase: 300.0 + vertical_bins: 128 + horizontal_ring_id: 64 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: left + parameters: + max_range: 5.0 + sensor_frame: velodyne_left + sensor_ip: 192.168.1.202 + data_port: 2369 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: right + parameters: + max_range: 5.0 + sensor_frame: velodyne_right + sensor_ip: 192.168.1.203 + data_port: 2370 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + - sensor_type: velodyne_VLP16 + namespace: rear + parameters: + max_range: 1.5 + sensor_frame: velodyne_rear + sensor_ip: 192.168.1.204 + data_port: 2371 + scan_phase: 180.0 + cloud_min_angle: 300 + cloud_max_angle: 60 + vertical_bins: 16 + horizontal_ring_id: 0 + horizontal_resolution: 0.4 + is_channel_order_top2down: false + +preprocessor: + input_topics: + - /sensing/lidar/top/pointcloud + - /sensing/lidar/left/pointcloud + - /sensing/lidar/right/pointcloud + - /sensing/lidar/rear/pointcloud + input_offset: + - 0.035 + - 0.025 + - 0.025 + - 0.025 + timeout_sec: 0.095 + input_twist_topic_type: twist + publish_synchronized_pointcloud: false diff --git a/aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml b/aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml new file mode 100644 index 00000000..a817e0b7 --- /dev/null +++ b/aip_xx1_gen2_launch/config/radar_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_xx1_gen2_launch/data/traffic_light_camera.yaml b/aip_xx1_gen2_launch/data/traffic_light_camera.yaml new file mode 100644 index 00000000..458ad17c --- /dev/null +++ b/aip_xx1_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_xx1_gen2_launch/launch/camera.launch.xml b/aip_xx1_gen2_launch/launch/camera.launch.xml new file mode 100644 index 00000000..041c70c8 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/camera.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..37ad6bde --- /dev/null +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/imu.launch.xml b/aip_xx1_gen2_launch/launch/imu.launch.xml new file mode 100644 index 00000000..04011706 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py new file mode 100644 index 00000000..7174fc9f --- /dev/null +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -0,0 +1,188 @@ +# 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 copy import deepcopy +import os +from typing import Any +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import PushRosNamespace +import yaml + + +def join_list_of_arguments(arguments: List[Any]) -> str: + """Join a list of arguments into a string, used by Include Launch Description. + + Example: + join_list_of_arguments([1,2,3]) -> "[1, 2, 3]" + """ + return f"[{', '.join([str(arg) for arg in arguments])}]" + + +def generate_launch_dictionary(): + path_dictionary = { + "hesai_OT128": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "hesai_OT128.launch.xml", + ) + ), + "hesai_XT32": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "hesai_XT32.launch.xml", + ) + ), + "velodyne_VLS128": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "velodyne_VLS128.launch.xml", + ) + ), + "velodyne_VLP16": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "velodyne_VLP16.launch.xml", + ) + ), + "livox_horizon": AnyLaunchDescriptionSource( + os.path.join( + get_package_share_directory("common_sensor_launch"), + "launch", + "livox_horizon.launch.py", + ) + ), + } + return path_dictionary + + +def load_sub_launches_from_yaml(context, *args, **kwargs): + def load_yaml(yaml_file_path): + with open(LaunchConfiguration(yaml_file_path).perform(context), "r") as f: + return yaml.safe_load(f) + + config = load_yaml("config_file") + + path_dictionary = generate_launch_dictionary() + + base_parameters = {} + base_parameters["host_ip"] = LaunchConfiguration("host_ip") + base_parameters["vehicle_mirror_param_file"] = LaunchConfiguration( + "vehicle_mirror_param_file" + ).perform(context) + base_parameters["launch_driver"] = LaunchConfiguration("launch_driver").perform(context) + base_parameters["vehicle_id"] = LaunchConfiguration("vehicle_id").perform(context) + base_parameters["pointcloud_container_name"] = LaunchConfiguration( + "pointcloud_container_name" + ).perform(context) + base_parameters["enable_blockage_diag"] = LaunchConfiguration("enable_blockage_diag").perform( + context + ) + + sub_launch_actions = [] + for launch in config["launches"]: + launch_parameters = deepcopy(base_parameters) + launch_parameters.update(launch["parameters"]) # dict + launch_parameter_list_tuple = [(str(k), str(v)) for k, v in launch_parameters.items()] + sub_launch_action = GroupAction( + [ + PushRosNamespace(launch["namespace"]), + IncludeLaunchDescription( + deepcopy(path_dictionary[launch["sensor_type"]]), + launch_arguments=launch_parameter_list_tuple, + ), + ] + ) + sub_launch_actions.append(sub_launch_action) + + processor_dict = config["preprocessor"] + sub_launch_actions.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("aip_xx1_gen2_launch"), + "launch", + "pointcloud_preprocessor.launch.py", + ) + ), + launch_arguments=[ + ("base_frame", "base_link"), + ("use_multithread", "true"), + ("use_intra_process", "true"), + ("use_pointcloud_container", LaunchConfiguration("use_pointcloud_container")), + ("pointcloud_container_name", LaunchConfiguration("pointcloud_container_name")), + ("input_topics", join_list_of_arguments(processor_dict["input_topics"])), + ("input_offset", join_list_of_arguments(processor_dict["input_offset"])), + ("timeout_sec", str(processor_dict["timeout_sec"])), + ("input_twist_topic_type", str(processor_dict["input_twist_topic_type"])), + ( + "publish_synchronized_pointcloud", + str(processor_dict["publish_synchronized_pointcloud"]), + ), + ], + ) + ) + return [ + GroupAction([PushRosNamespace("lidar"), *sub_launch_actions]), + ] + + +def generate_launch_description(): + # Define launch arguments + launch_arguments = [] + config_file_arg = DeclareLaunchArgument( + "config_file", + default_value=os.path.join( + get_package_share_directory("aip_xx1_gen2_launch"), "config", "lidar_launch.yaml" + ), + description="Path to the configuration file", + ) + launch_arguments.append(config_file_arg) + + def add_launch_arg(name: str, default_value=None, **kwargs): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value, **kwargs)) + + add_launch_arg("launch_driver", "true") + add_launch_arg("host_ip", "192.168.1.10") + add_launch_arg("use_concat_filter", "true") + add_launch_arg( + "vehicle_id", + default_value=EnvironmentVariable("VEHICLE_ID", default_value="default"), + ) + add_launch_arg("vehicle_mirror_param_file") + add_launch_arg("use_pointcloud_container", "false", description="launch pointcloud container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("enable_blockage_diag", "false") + + # Create launch description with the config_file argument + ld = LaunchDescription(launch_arguments) + # Add sub-launch files dynamically based on the YAML configuration + ld.add_action(OpaqueFunction(function=load_sub_launches_from_yaml)) + + return ld diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.xml b/aip_xx1_gen2_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..4f4b37b7 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..433b7f58 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,100 @@ +# 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": LaunchConfiguration("input_topics"), + "output_frame": LaunchConfiguration("base_frame"), + "input_offset": LaunchConfiguration( + "input_offset" + ), # each sensor will wait 60, 70, 70, 70ms + "timeout_sec": LaunchConfiguration("timeout_sec"), # set shorter than 100ms + "input_twist_topic_type": LaunchConfiguration("input_twist_topic_type"), + "publish_synchronized_pointcloud": LaunchConfiguration( + "publish_synchronized_pointcloud" + ), + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # load concat or passthrough filter + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=LaunchConfiguration("pointcloud_container_name"), + condition=IfCondition(LaunchConfiguration("use_concat_filter")), + ) + + 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", "False") + add_launch_arg("use_intra_process", "False") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_container") + add_launch_arg( + "input_topics", + "[/sensing/lidar/top/pointcloud, /sensing/lidar/left/pointcloud, /sensing/lidar/right/pointcloud, /sensing/lidar/rear/pointcloud]", + ) + add_launch_arg("input_offset", "[0.035, 0.025, 0.025, 0.025]") + add_launch_arg("timeout_sec", "0.095") + add_launch_arg("input_twist_topic_type", "twist") + add_launch_arg("publish_synchronized_pointcloud", "False") + + 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_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..7eb4dbcb --- /dev/null +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,236 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/aip_xx1_gen2_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..74f24c15 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/package.xml b/aip_xx1_gen2_launch/package.xml new file mode 100644 index 00000000..8f2f9df9 --- /dev/null +++ b/aip_xx1_gen2_launch/package.xml @@ -0,0 +1,32 @@ + + + + aip_xx1_gen2_launch + 0.1.0 + The aip_xx1_gen2_launch package + + Hiroki OTA + Apache License 2.0 + + ament_cmake_auto + + common_sensor_launch + glog_component + gnss_poser + imu_corrector + pacmod3 + pointcloud_preprocessor + ros2_socketcan + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + vehicle_velocity_converter + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common_sensor_launch/launch/hesai_OT128.launch.xml b/common_sensor_launch/launch/hesai_OT128.launch.xml new file mode 100644 index 00000000..7d4713fd --- /dev/null +++ b/common_sensor_launch/launch/hesai_OT128.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index 56587f72..c6ea9b64 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -25,12 +25,18 @@ - - + + + + + + + + diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index c3339e0c..1990f695 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -113,6 +113,8 @@ def create_parameter_dict(*args): "cloud_min_angle", "cloud_max_angle", "dual_return_distance_threshold", + "setup_sensor", + "retry_hw", ), }, ], @@ -124,6 +126,36 @@ def create_parameter_dict(*args): ) ) + nodes.append( + ComposableNode( + package="nebula_ros", + plugin=sensor_make + "HwMonitorRosWrapper", + name=sensor_make.lower() + "_hw_monitor_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + **create_parameter_dict( + "return_mode", + "frame_id", + "scan_phase", + "sensor_ip", + "host_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "rotation_speed", + "cloud_min_angle", + "cloud_max_angle", + "diag_span", + "dual_return_distance_threshold", + "delay_monitor_ms", + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + ) + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") cropbox_parameters["negative"] = True @@ -240,6 +272,11 @@ def create_parameter_dict(*args): "packet_mtu_size", "dual_return_distance_threshold", "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + "retry_hw", ), } ], @@ -297,6 +334,7 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("retry_hw", "false", "retry hw") 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") @@ -319,6 +357,13 @@ def add_launch_arg(name: str, default_value=None, description=None): 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("ptp_profile", "1588v2") + add_launch_arg("ptp_transport_type", "L2") + add_launch_arg("ptp_switch_type", "TSN") + add_launch_arg("ptp_domain", "0") + add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") + add_launch_arg("diag_span", "1000", "") + add_launch_arg("delay_monitor_ms", "2000", "") add_launch_arg("output_as_sensor_frame", "True", "output final pointcloud in sensor frame") add_launch_arg("enable_blockage_diag", "true")