From cf30b926e09423dd6bd0b6d7650b86ef3d5298ea Mon Sep 17 00:00:00 2001
From: h-ohta <hiroki.ota@tier4.jp>
Date: Tue, 25 Jun 2024 11:05:14 +0900
Subject: [PATCH] add rest piece

---
 .../config/sensor_kit_calibration.yaml        |  76 +++++++---
 .../config/sensors_calibration.yaml           |  40 ++++++
 aip_x2_description/urdf/gnss.xacro            |  24 ++++
 aip_x2_description/urdf/sensor_kit.xacro      | 135 +++++++++++++-----
 aip_x2_description/urdf/sensors.xacro         |  57 +++++++-
 5 files changed, 274 insertions(+), 58 deletions(-)
 create mode 100644 aip_x2_description/urdf/gnss.xacro

diff --git a/aip_x2_description/config/sensor_kit_calibration.yaml b/aip_x2_description/config/sensor_kit_calibration.yaml
index 63879cf1..6139eee3 100644
--- 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
index b2a2eb8c..fbb534a4 100644
--- 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
new file mode 100644
index 00000000..55002be0
--- /dev/null
+++ 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
index 886731ea..06b3a289 100644
--- 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>
diff --git a/aip_x2_description/urdf/sensors.xacro b/aip_x2_description/urdf/sensors.xacro
index 805b0caa..5c7c30b5 100644
--- a/aip_x2_description/urdf/sensors.xacro
+++ b/aip_x2_description/urdf/sensors.xacro
@@ -19,7 +19,7 @@
 
   <!-- radar -->
   <xacro:radar_macro
-    name="ars548/front_center/radar"
+    name="ars548/front_center/radar_link"
     parent="base_link"
     x="${calibration['base_link']['ars548/front_center/radar_link']['x']}"
     y="${calibration['base_link']['ars548/front_center/radar_link']['y']}"
@@ -29,4 +29,59 @@
     yaw="${calibration['base_link']['ars548/front_center/radar_link']['yaw']}"
   />
 
+  <xacro:radar_macro
+    name="ars548/front_left/radar_link"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/front_left/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/front_left/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/front_left/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/front_left/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/front_left/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/front_left/radar_link']['yaw']}"
+  />
+
+  <xacro:radar_macro
+    name="ars548/front_right/radar_link"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/front_right/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/front_right/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/front_right/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/front_right/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/front_right/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/front_right/radar_link']['yaw']}"
+  />
+
+  <xacro:radar_macro
+    name="ars548/rear_center/radar_link"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/rear_center/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/rear_center/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/rear_center/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/rear_center/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/rear_center/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/rear_center/radar_link']['yaw']}"
+  />
+
+  <xacro:radar_macro
+    name="ars548/rear_left/radar_link"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/rear_left/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/rear_left/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/rear_left/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/rear_left/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/rear_left/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/rear_left/radar_link']['yaw']}"
+  />
+
+  <xacro:radar_macro
+    name="ars548/rear_right/radar_link"
+    parent="base_link"
+    x="${calibration['base_link']['ars548/rear_right/radar_link']['x']}"
+    y="${calibration['base_link']['ars548/rear_right/radar_link']['y']}"
+    z="${calibration['base_link']['ars548/rear_right/radar_link']['z']}"
+    roll="${calibration['base_link']['ars548/rear_right/radar_link']['roll']}"
+    pitch="${calibration['base_link']['ars548/rear_right/radar_link']['pitch']}"
+    yaw="${calibration['base_link']['ars548/rear_right/radar_link']['yaw']}"
+  />
+
 </robot>