From c864d7c3b3081b3a87d6c450c45a7b10081509e3 Mon Sep 17 00:00:00 2001
From: Autumn60 <harada.akiro@gmail.com>
Date: Sat, 31 Aug 2024 02:17:47 +0900
Subject: [PATCH] create xacro macro for wheel

Signed-off-by: Autumn60 <harada.akiro@gmail.com>
---
 .../urdf/robot.urdf.xacro                     | 34 +++++--------------
 .../urdf/wheel/wheel.urdf.xacro               | 21 ++++++++++++
 2 files changed, 29 insertions(+), 26 deletions(-)
 create mode 100644 src/description/fourth_robot/fourth_robot_description/urdf/wheel/wheel.urdf.xacro

diff --git a/src/description/fourth_robot/fourth_robot_description/urdf/robot.urdf.xacro b/src/description/fourth_robot/fourth_robot_description/urdf/robot.urdf.xacro
index 8edbd07..755dbf7 100644
--- a/src/description/fourth_robot/fourth_robot_description/urdf/robot.urdf.xacro
+++ b/src/description/fourth_robot/fourth_robot_description/urdf/robot.urdf.xacro
@@ -2,6 +2,8 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:property name="robot_info" value="${xacro.load_yaml('$(find fourth_robot_description)/config/robot_info.param.yaml')}"/>
 
+  <xacro:include filename="$(find fourth_robot_description)/urdf/wheel/wheel.urdf.xacro"/>
+
   <link name="base_link">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -18,31 +20,11 @@
     <origin xyz="0 0 -${robot_info['/**']['ros__parameters']['wheel_radius']}" rpy="0 0 0"/>
   </joint>
 
-  <link name="wheel_left_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://fourth_robot_description/mesh/wheel/wheel.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-  </link>
-  <joint name="wheel_left_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="wheel_left_link"/>
-    <origin xyz="0 -${robot_info['/**']['ros__parameters']['wheel_tread'] / 2} 0" rpy="0 0 0"/>
-  </joint>
+  <xacro:wheel name="wheel_left" parent="base_link" wheel_radius="${robot_info['/**']['ros__parameters']['wheel_radius']}">
+    <origin xyz="0 ${robot_info['/**']['ros__parameters']['wheel_tread'] / 2} 0" rpy="0 0 0"/>
+  </xacro:wheel>
 
-  <link name="wheel_right_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://fourth_robot_description/mesh/wheel/wheel.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-  </link>
-  <joint name="wheel_right_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="wheel_right_link"/>
-    <origin xyz="0 ${robot_info['/**']['ros__parameters']['wheel_tread'] / 2} 0" rpy="0 0 ${pi}"/>
-  </joint>
+  <xacro:wheel name="wheel_right" parent="base_link" wheel_radius="${robot_info['/**']['ros__parameters']['wheel_radius']}">
+    <origin xyz="0 -${robot_info['/**']['ros__parameters']['wheel_tread'] / 2} 0" rpy="0 0 ${pi}"/>
+  </xacro:wheel>
 </robot>
diff --git a/src/description/fourth_robot/fourth_robot_description/urdf/wheel/wheel.urdf.xacro b/src/description/fourth_robot/fourth_robot_description/urdf/wheel/wheel.urdf.xacro
new file mode 100644
index 0000000..049eb82
--- /dev/null
+++ b/src/description/fourth_robot/fourth_robot_description/urdf/wheel/wheel.urdf.xacro
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:property name="model_wheel_radius" value="0.193125"/>
+
+  <xacro:macro name="wheel" params="name parent wheel_radius *joint_origin">
+    <link name="${name}_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://fourth_robot_description/mesh/wheel/wheel.dae" scale="${wheel_radius / model_wheel_radius} 1 ${wheel_radius / model_wheel_radius}"/>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="${name}_joint" type="fixed">
+      <xacro:insert_block name="joint_origin"/>
+      <parent link="${parent}"/>
+      <child link="${name}_link"/>
+    </joint>
+  </xacro:macro>
+</robot>