From 7c118dcb8693a5ab11f8b02c9066f2c48844bcdb Mon Sep 17 00:00:00 2001
From: Shunsuke Miura <shunsuke.miura@tier4.jp>
Date: Wed, 26 Jul 2023 06:57:33 +0900
Subject: [PATCH] feat(aip_xx1_description): add radar description to xx1

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
---
 .../config/sensors_calibration.yaml           |  7 +++++
 aip_xx1_description/urdf/radar.xacro          | 31 +++++++++++++++++++
 aip_xx1_description/urdf/sensors.xacro        | 13 ++++++++
 aip_xx1_launch/launch/radar.launch.xml        |  2 --
 4 files changed, 51 insertions(+), 2 deletions(-)
 create mode 100644 aip_xx1_description/urdf/radar.xacro

diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml
index 473f302d..e8c4b75e 100644
--- a/aip_xx1_description/config/sensors_calibration.yaml
+++ b/aip_xx1_description/config/sensors_calibration.yaml
@@ -1,4 +1,11 @@
 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
diff --git a/aip_xx1_description/urdf/radar.xacro b/aip_xx1_description/urdf/radar.xacro
new file mode 100644
index 00000000..8b0f8d4b
--- /dev/null
+++ b/aip_xx1_description/urdf/radar.xacro
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="radar_macro" params="name parent x y z roll pitch yaw">
+    <xacro:property name="mass" value="0.01" />
+
+    <joint name="${name}_base_mount_joint" type="fixed">
+      <origin rpy="${roll} ${pitch} ${yaw}" xyz="${x} ${y} ${z}"/>
+      <parent link="${parent}"/>
+      <child  link="${name}"/>
+    </joint>
+
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <box size="0.03 0.1 0.1"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <material name="black">
+          <color rgba="0.1 0.1 0.1 1.0"/>
+        </material>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <mass value="${mass}"/>
+        <inertia ixx="${(0.03*0.03+0.03*0.03)*mass/12.0}" ixy="0.0" ixz="0.0"
+                 iyy="${(0.1*0.1+0.1*0.1)*mass/12.0}" iyz="0.0"
+                 izz="${(0.1*0.1+0.1*0.1)*mass/12.0}"/>
+      </inertial>
+    </link>
+  </xacro:macro>
+</robot>
diff --git a/aip_xx1_description/urdf/sensors.xacro b/aip_xx1_description/urdf/sensors.xacro
index df14d53e..0484bdc3 100644
--- a/aip_xx1_description/urdf/sensors.xacro
+++ b/aip_xx1_description/urdf/sensors.xacro
@@ -56,4 +56,17 @@
     yaw="${calibration['base_link']['livox_front_right_base_link']['yaw']}"
   />
 
+  <!-- radar -->
+  <xacro:include filename="$(find aip_xx1_description)/urdf/radar.xacro"/>
+  <xacro:radar_macro
+    name="ars408_front_center"
+    parent="base_link"
+    x="${calibration['base_link']['ars408_front_center']['x']}"
+    y="${calibration['base_link']['ars408_front_center']['y']}"
+    z="${calibration['base_link']['ars408_front_center']['z']}"
+    roll="${calibration['base_link']['ars408_front_center']['roll']}"
+    pitch="${calibration['base_link']['ars408_front_center']['pitch']}"
+    yaw="${calibration['base_link']['ars408_front_center']['yaw']}"
+  />
+
 </robot>
diff --git a/aip_xx1_launch/launch/radar.launch.xml b/aip_xx1_launch/launch/radar.launch.xml
index 6a04996e..89c3b673 100644
--- a/aip_xx1_launch/launch/radar.launch.xml
+++ b/aip_xx1_launch/launch/radar.launch.xml
@@ -10,8 +10,6 @@
       <push-ros-namespace namespace="front_center"/>
 
       <!-- tf launch (temporary) -->
-      <node pkg="tf2_ros" exec="static_transform_publisher" name="base_link_to_ars408" output="screen" args="3.8 0.0 0.0 0.0 0.0 0.0 base_link ars408_front_center" />
-
       <include file="$(find-pkg-share common_sensor_launch)/launch/ars408.launch.xml">
         <arg name="launch_driver" value="$(var launch_driver)" />
         <arg name="interface" value="$(var radar_can_device)" />