diff --git a/aip_xx1_description/config/sensor_kit_calibration.yaml b/aip_xx1_description/config/sensor_kit_calibration.yaml
index 88288533..b3c920c6 100644
--- a/aip_xx1_description/config/sensor_kit_calibration.yaml
+++ b/aip_xx1_description/config/sensor_kit_calibration.yaml
@@ -1,113 +1,92 @@
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
+ x: 0.10731
+ y: 0.56343
+ z: -0.27697
+ roll: -0.025
+ pitch: 0.315
+ yaw: 1.035
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
+ x: -0.10731
+ y: -0.56343
+ z: -0.27697
+ roll: -0.025
+ pitch: 0.32
+ yaw: -2.12
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
+ x: 0.10731
+ y: -0.56343
+ z: -0.27697
+ roll: -0.00
+ pitch: 0.335
+ yaw: -1.04
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
+ x: -0.10731
+ y: 0.56343
+ z: -0.27697
+ roll: 0.0
+ pitch: 0.325
+ yaw: 2.0943951
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
+ x: 0.07356
+ y: 0.0
+ z: -0.0525
+ roll: 0.0
+ pitch: -0.03
+ yaw: -0.005
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
+ x: -0.07356
+ y: 0.0
+ z: -0.0525
+ roll: 0.0
+ pitch: -0.01
+ yaw: 3.125
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
+ x: 0.05
+ y: 0.0175
+ z: -0.1
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
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
+ x: 0.05
+ y: -0.0175
+ z: -0.1
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
+ velodyne_top_base_link:
+ x: 0.0
+ y: 0.0
+ z: 0.0
+ roll: 0.0
+ pitch: 0.0
+ yaw: 1.575
+ velodyne_left_base_link:
+ x: 0.0
+ y: 0.56362
+ z: -0.30555
+ roll: -0.02
+ pitch: 0.71
+ yaw: 1.575
+ velodyne_right_base_link:
+ x: 0.0
+ y: -0.56362
+ z: -0.30555
+ roll: -0.01
+ pitch: 0.71
+ yaw: -1.580
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
+ x: -0.1
+ y: 0.0
+ z: -0.2
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
tamagawa/imu_link:
- x: -0.129 # Design Value
- y: 0.0 # Design Value
- z: -0.160 # Design Value
+ x: 0.0
+ y: 0.0
+ z: 0.0
roll: 3.14159265359
- pitch: 0.0 # Design Value
- yaw: 3.14159265359 # Design Value
+ pitch: 0.0
+ yaw: 3.14159265359
\ No newline at end of file
diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml
index a57d3ea9..c7389f7e 100644
--- a/aip_xx1_description/config/sensors_calibration.yaml
+++ b/aip_xx1_description/config/sensors_calibration.yaml
@@ -1,71 +1,36 @@
base_link:
+ ars408_front_center:
+ x: 3.8
+ y: 0.0
+ z: 0.5
+ roll: 0.0
+ pitch: 0.0
+ yaw: 0.0
sensor_kit_base_link:
x: 0.9
y: 0.0
z: 2.0
+ roll: -0.001
+ pitch: 0.015
+ yaw: -0.0364
+ livox_front_right_base_link:
+ x: 3.290
+ y: -0.65485
+ z: 0.3216
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
+ yaw: -0.872664444
+ livox_front_left_base_link:
+ x: 3.290
+ y: 0.65485
+ z: 0.3016
+ roll: -0.021
+ pitch: 0.05
+ yaw: 0.872664444
+ velodyne_rear_base_link:
+ x: -0.358
+ y: 0.0
+ z: 1.631
+ roll: -0.02
+ pitch: 0.7281317
+ yaw: 3.141592
\ No newline at end of file
diff --git a/aip_xx1_description/urdf/sensor_kit.xacro b/aip_xx1_description/urdf/sensor_kit.xacro
index 8830a4d3..e4a6c40d 100644
--- a/aip_xx1_description/urdf/sensor_kit.xacro
+++ b/aip_xx1_description/urdf/sensor_kit.xacro
@@ -1,9 +1,9 @@
-
-
+
+
@@ -25,37 +25,57 @@
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
+
\ No newline at end of file
diff --git a/aip_xx1_description/urdf/sensors.xacro b/aip_xx1_description/urdf/sensors.xacro
index 64dbf6bb..52299371 100644
--- a/aip_xx1_description/urdf/sensors.xacro
+++ b/aip_xx1_description/urdf/sensors.xacro
@@ -15,8 +15,8 @@
pitch="${calibration['base_link']['sensor_kit_base_link']['pitch']}"
yaw="${calibration['base_link']['sensor_kit_base_link']['yaw']}"
/>
-
-
+
- -->
-
-
+
+
-
-
-
+
+
-
-
-
-
-
-
+
+
\ No newline at end of file
diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt
new file mode 100644
index 00000000..50723262
--- /dev/null
+++ b/aip_xx1_gen2_description/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.5)
+project(aip_xx1_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..b35b48e0
--- /dev/null
+++ b/aip_xx1_gen2_description/package.xml
@@ -0,0 +1,17 @@
+
+
+ aip_xx1_description
+ 0.1.0
+ The aip_xx1_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..8830a4d3
--- /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..64dbf6bb
--- /dev/null
+++ b/aip_xx1_gen2_description/urdf/sensors.xacro
@@ -0,0 +1,121 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/aip_xx1_launch/config/radar_simple_object_merger.param.yaml b/aip_xx1_launch/config/radar_simple_object_merger.param.yaml
deleted file mode 100644
index a817e0b7..00000000
--- a/aip_xx1_launch/config/radar_simple_object_merger.param.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-/**:
- 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"]