add rest piece

tier4 · Jun 25, 2024 · cf30b92 · cf30b92
1 parent d295887
commit cf30b92
Show file tree

Hide file tree

Showing 5 changed files with 274 additions and 58 deletions.
diff --git a/aip_x2_description/config/sensor_kit_calibration.yaml b/aip_x2_description/config/sensor_kit_calibration.yaml
@@ -1,19 +1,19 @@
 sensor_kit_base_link:
   # left upper
-  pandar/top_left/lidar_link:
+  pandar/top_left/lidar_base_link:
     x: 0.0
     y: 0.0
     z: 0.0
     roll: 0.0
     pitch: 0.0
-    yaw: 1.570796 # 90 [deg]
-  pandar/top_left_bottom/lidar_link:
+    yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle
+  pandar/top_left_bottom/lidar_base_link:
     x: 0.0
     y: -0.025
     z: -0.115
-    roll: 0.0
-    pitch: 0.6981 # 40[deg]
-    yaw: 1.570796 # 90 [deg]
+    roll: 0.6981 # 40[deg]
+    pitch: 0.0
+    yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle
   camera/top_left_front/camera_link:
     x: 0.12758
     y: -0.04589
@@ -37,20 +37,20 @@ sensor_kit_base_link:
     yaw: 1.570796 # 90 [deg]
 
   # right upper
-  pandar/top_right/lidar_link:
+  pandar/top_right/lidar_base_link:
     x: 0.0
     y: -2.15178
     z: 0.0
     roll: 0.0
     pitch: 0.0
-    yaw: -1.570796 # -90 [deg]
-  pandar/top_right_bottom/lidar_link:
+    yaw: 0.0 # to turn the connector toward the vehicle
+  pandar/top_right_bottom/lidar_base_link:
     x: 0.0
     y: -2.12678
     z: -0.115
-    roll: 0.0
-    pitch: 0.6981 # 40 [deg]
-    yaw: -1.570796 # -90 [deg]
+    roll: 0.6981 # 40 [deg]
+    pitch: 0.0
+    yaw: 0.0 # to turn the connector toward the vehicle
   camera/top_right_front/camera_link:
     x: 0.12758
     y: -2.10589
@@ -116,24 +116,58 @@ sensor_kit_base_link:
     roll: 0.0
     pitch: 0.0
     yaw: 0.0
+  gnss/front/gnss_link:
+    x: 0.30133
+    y: -1.07589
+    z: 0.02861
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
+
+  # rear upper
+  camera/rear_center/camera_link:
+    x: -6.06359
+    y: -1.20389
+    z: -0.14078
+    roll: 0.0
+    pitch: 0.0
+    yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle
+  gnss/rear/gnss_link:
+    x: -5.77517
+    y: -1.07589
+    z: 0.02861
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0
 
   # front lower
-  pandar/front/lidar_link:
+  pandar/front/lidar_base_link:
     x: 0.94058
     y: -1.07589
     z: -1.91826
     roll: 0.0
     pitch: 0.0
-    yaw: 0.0
-  pandar/front_bottom/lidar_link:
+    yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle
+  pandar/front_bottom/lidar_base_link:
     x: 0.96758
     y: -1.07589
     z: -2.15234
-    roll: 0.0
-    pitch: 0.6981 # 40[deg]
-    yaw: 0.0
-
+    roll: 0.6981 # 40[deg]
+    pitch: 0.0
+    yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle
 
   # rear lower
-
-  # GNSS
+  pandar/rear/lidar_base_link:
+    x: -6.08042
+    y: -1.07589
+    z: -1.91826
+    roll: 0.0
+    pitch: 0.0
+    yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle
+  pandar/rear_bottom/lidar_base_link:
+    x: -6.10742
+    y: -1.07589
+    z: -2.15234
+    roll: 0.6981 # 40[deg]
+    pitch: 0.0
+    yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle
diff --git a/aip_x2_description/config/sensors_calibration.yaml b/aip_x2_description/config/sensors_calibration.yaml
@@ -15,3 +15,43 @@ base_link:
     roll: 0.0
     pitch: 0.0
     yaw: 0.0
+
+  ars548/front_left/radar_link:
+    x: 5.37204
+    y: 1.08537
+    z: 0.73431
+    roll: 0.0
+    pitch: 0.0
+    yaw: 1.2211 # 70 [deg]
+
+  ars548/front_right/radar_link:
+    x: 5.37204
+    y: -1.06642
+    z: 0.73431
+    roll: 0.0
+    pitch: 0.0
+    yaw: -1.2211 # 70 [deg]
+
+  ars548/rear_center/radar_link:
+    x: -1.50704
+    y: 0.0
+    z: 0.78894
+    roll: 0.0
+    pitch: 0.0
+    yaw: 3.141592 # 180 [deg]
+
+  ars548/rear_left/radar_link:
+    x: -1.27564
+    y: 1.07767
+    z: 0.5487
+    roll: 0.0
+    pitch: 0.0
+    yaw: 1.9189 # 110 [deg]
+
+  ars548/rear_right/radar_link:
+    x: -1.27564
+    y: -1.07768
+    z: 0.5487
+    roll: 0.0
+    pitch: 0.0
+    yaw: -1.9189 # 110 [deg]
diff --git a/aip_x2_description/urdf/gnss.xacro b/aip_x2_description/urdf/gnss.xacro
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="gnss_macro" params="name parent x y z roll pitch yaw">
+    <xacro:property name="sensor_box" value="0.05"/>
+
+    <joint name="${name}_joint" type="fixed">
+      <origin rpy="${roll} ${pitch} ${yaw}" xyz="${x} ${y} ${z}"/>
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+    </joint>
+
+    <link name="${name}_link">
+      <visual>
+        <geometry>
+          <box size="${sensor_box} ${sensor_box} ${sensor_box}"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <material name="green">
+          <color rgba="0.0 1.0 0.0 1.0"/>
+        </material>
+      </visual>
+    </link>
+  </xacro:macro>
+</robot>
diff --git a/aip_x2_description/urdf/sensor_kit.xacro b/aip_x2_description/urdf/sensor_kit.xacro
@@ -4,6 +4,7 @@
     <xacro:include filename="$(find pandar_description)/urdf/pandar_qt.xacro" />
     <xacro:include filename="$(find camera_description)/urdf/monocular_camera.xacro" />
     <xacro:include filename="$(find imu_description)/urdf/imu.xacro" />
+    <xacro:include filename="$(find aip_x2_description)/urdf/gnss.xacro" />
 
     <xacro:arg name="gpu" default="false" />
     <xacro:arg name="config_dir" default="$(find aip_x2_description)/config" />
@@ -25,23 +26,23 @@
     <xacro:PandarQT
       name="pandar/top_left/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/top_left/lidar_base_link']['yaw']}"
     />
 
     <xacro:PandarQT
       name="pandar/top_left_bottom/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/top_left_bottom/lidar_base_link']['yaw']}"
     />
 
     <xacro:monocular_camera_macro
@@ -96,23 +97,23 @@
     <xacro:PandarQT
       name="pandar/top_right/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/top_right/lidar_base_link']['yaw']}"
     />
 
     <xacro:PandarQT
       name="pandar/top_right_bottom/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/top_right_bottom/lidar_base_link']['yaw']}"
     />
 
     <xacro:monocular_camera_macro
@@ -254,27 +255,89 @@
       fov="1.3"
     />
 
+    <xacro:gnss_macro
+      name="gnss/front/gnss"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['gnss/front/gnss_link']['yaw']}"
+    />
+
+    <!-- rear upper -->
+    <xacro:monocular_camera_macro
+      name="camera/rear_center/camera"
+      parent="sensor_kit_base_link"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['camera/rear_center/camera_link']['yaw']}"
+      fps="30"
+      width="800"
+      height="400"
+      fov="1.3"
+    />
+
+    <xacro:gnss_macro
+      name="gnss/rear/gnss"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['gnss/rear/gnss_link']['yaw']}"
+    />
+
     <!-- front lower -->
     <xacro:PandarQT
       name="pandar/front/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/front/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/front/lidar_base_link']['yaw']}"
     />
 
     <xacro:PandarQT
       name="pandar/front_bottom/lidar"
       parent="sensor_kit_base_link"
-      x="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['x']}"
-      y="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['y']}"
-      z="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['z']}"
-      roll="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['roll']}"
-      pitch="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['pitch']}"
-      yaw="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_link']['yaw']}"
+      x="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/front_bottom/lidar_base_link']['yaw']}"
+    />
+
+    <!-- rear lower -->
+    <xacro:PandarQT
+      name="pandar/rear/lidar"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/rear/lidar_base_link']['yaw']}"
+    />
+
+    <xacro:PandarQT
+      name="pandar/rear_bottom/lidar"
+      parent="sensor_kit_base_link"
+      x="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['pandar/rear_bottom/lidar_base_link']['yaw']}"
     />
 
   </xacro:macro>