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")