From 53e0b0f70f6b0b00b8b1cb382f94f328031c7c82 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu Date: Mon, 17 Jun 2024 16:40:43 +0900 Subject: [PATCH] add new folders Signed-off-by: Yuxuan Liu --- aip_xx1_gen2_launch/CMakeLists.txt | 16 ++ .../sensor_kit.param.yaml | 109 +++++++++ .../sensor_kit.param.yaml | 30 +++ .../radar_simple_object_merger.param.yaml | 6 + .../data/traffic_light_camera.yaml | 20 ++ aip_xx1_gen2_launch/launch/camera.launch.xml | 19 ++ aip_xx1_gen2_launch/launch/gnss.launch.xml | 41 ++++ aip_xx1_gen2_launch/launch/imu.launch.xml | 39 ++++ aip_xx1_gen2_launch/launch/lidar.launch.xml | 106 +++++++++ .../launch/pointcloud_preprocessor.launch.py | 118 ++++++++++ aip_xx1_gen2_launch/launch/radar.launch.xml | 214 ++++++++++++++++++ aip_xx1_gen2_launch/launch/sensing.launch.xml | 49 ++++ aip_xx1_gen2_launch/package.xml | 32 +++ aip_xx1_launch/launch/imu.launch.xml | 24 +- aip_xx1_launch/launch/lidar.launch.xml | 112 +++++---- .../launch/pointcloud_preprocessor.launch.py | 33 +-- aip_xx1_launch/launch/radar.launch.xml | 211 +---------------- 17 files changed, 892 insertions(+), 287 deletions(-) create mode 100644 aip_xx1_gen2_launch/CMakeLists.txt create mode 100644 aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml create mode 100644 aip_xx1_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml create mode 100644 aip_xx1_gen2_launch/config/radar_simple_object_merger.param.yaml create mode 100644 aip_xx1_gen2_launch/data/traffic_light_camera.yaml create mode 100644 aip_xx1_gen2_launch/launch/camera.launch.xml create mode 100644 aip_xx1_gen2_launch/launch/gnss.launch.xml create mode 100644 aip_xx1_gen2_launch/launch/imu.launch.xml create mode 100644 aip_xx1_gen2_launch/launch/lidar.launch.xml create mode 100644 aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py create mode 100644 aip_xx1_gen2_launch/launch/radar.launch.xml create mode 100644 aip_xx1_gen2_launch/launch/sensing.launch.xml create mode 100644 aip_xx1_gen2_launch/package.xml 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..ab32e0fe --- /dev/null +++ b/aip_xx1_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,109 @@ +/**: + 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: + 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/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..7f1f0337 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..7f77ad2d --- /dev/null +++ b/aip_xx1_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,106 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..4aa9736c --- /dev/null +++ b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,118 @@ +# 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 ComposableNodeContainer +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/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", + ], + "output_frame": LaunchConfiguration("base_frame"), + "input_offset": [ + 0.035, + 0.025, + 0.025, + 0.025, + 0.025, + ], # each sensor will wait 60, 70, 70, 70ms + "timeout_sec": 0.095, # set shorter than 100ms + "input_twist_topic_type": "twist", + "publish_synchronized_pointcloud": True, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name=LaunchConfiguration("individual_container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + target_container = ( + container + if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) + else LaunchConfiguration("pointcloud_container_name") + ) + + # load concat or passthrough filter + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=target_container, + condition=IfCondition(LaunchConfiguration("use_concat_filter")), + ) + + return [container, 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("use_pointcloud_container", "False") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_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_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..aba667d9 --- /dev/null +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,214 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..ba6b935c --- /dev/null +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/aip_xx1_launch/launch/imu.launch.xml b/aip_xx1_launch/launch/imu.launch.xml index 7f1f0337..f0509930 100644 --- a/aip_xx1_launch/launch/imu.launch.xml +++ b/aip_xx1_launch/launch/imu.launch.xml @@ -1,23 +1,17 @@ + + + - - - - - - - - - - - - - + + + + @@ -29,11 +23,13 @@ + + - + \ No newline at end of file diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 7f77ad2d..1b5bef3c 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -5,102 +5,124 @@ - + - - - + + + - + + + + + + - - - - - + + + + + - - - + + + + + + + + - - - - - + + + + + - - - + + + + + + + + - - - - - + + + + + - - - + + + + + + + + - - - - - - - - - - - - - - + + - - + \ No newline at end of file diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 4aa9736c..a5d67206 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -20,7 +20,6 @@ from launch.conditions import IfCondition 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 @@ -39,10 +38,9 @@ def launch_setup(context, *args, **kwargs): { "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", + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/rear/pointcloud_before_sync", ], "output_frame": LaunchConfiguration("base_frame"), "input_offset": [ @@ -50,7 +48,6 @@ def launch_setup(context, *args, **kwargs): 0.025, 0.025, 0.025, - 0.025, ], # each sensor will wait 60, 70, 70, 70ms "timeout_sec": 0.095, # set shorter than 100ms "input_twist_topic_type": "twist", @@ -60,30 +57,14 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # set container to run all required components in the same process - container = ComposableNodeContainer( - name=LaunchConfiguration("individual_container_name"), - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - target_container = ( - container - if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("pointcloud_container_name") - ) - # load concat or passthrough filter concat_loader = LoadComposableNodes( composable_node_descriptions=[concat_component], - target_container=target_container, + target_container=LaunchConfiguration("pointcloud_container_name"), condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) - return [container, concat_loader] + return [concat_loader] def generate_launch_description(): @@ -95,9 +76,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("base_frame", "base_link") add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") - add_launch_arg("use_pointcloud_container", "False") add_launch_arg("pointcloud_container_name", "pointcloud_container") - add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", @@ -115,4 +94,4 @@ def add_launch_arg(name: str, default_value=None): launch_arguments + [set_container_executable, set_container_mt_executable] + [OpaqueFunction(function=launch_setup)] - ) + ) \ No newline at end of file diff --git a/aip_xx1_launch/launch/radar.launch.xml b/aip_xx1_launch/launch/radar.launch.xml index aba667d9..f8ede59f 100644 --- a/aip_xx1_launch/launch/radar.launch.xml +++ b/aip_xx1_launch/launch/radar.launch.xml @@ -1,214 +1,23 @@ - - - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - + \ No newline at end of file