From 47274dcb36e9177d6ce4f0596276790422b3b639 Mon Sep 17 00:00:00 2001
From: Matt Spencer <Matthew.Spencer@amd.com>
Date: Fri, 17 Jul 2026 08:28:02 +0000
Subject: [PATCH] Refactor the robot description, and include mecanum control.

---
 .../config/ros2_controllers.yaml              |  23 +-
 .../meshes/{ => wheels}/wheel_lb_link.stl     | Bin
 .../meshes/{ => wheels}/wheel_lf_link.stl     | Bin
 .../meshes/{ => wheels}/wheel_rb_link.stl     | Bin
 .../meshes/{ => wheels}/wheel_rf_link.stl     | Bin
 .../urdf/agv_pro.gazebo.xacro                 |  38 +-
 .../urdf/agv_pro.ros2_control.xacro           |  59 ---
 .../urdf/agv_pro.urdf.xacro                   | 353 +++++++-----------
 .../urdf/control/agv_pro.ros2_control.xacro   |  55 +++
 .../urdf/parts/inertia.urdf.xacro             |  59 +++
 .../urdf/parts/wheel_macro.xacro              | 113 ++++++
 .../agv_pro_gazebo/config/agv_control.yaml    |  18 +-
 12 files changed, 373 insertions(+), 345 deletions(-)
 rename src/elephant_robotics/agv_pro_description/meshes/{ => wheels}/wheel_lb_link.stl (100%)
 rename src/elephant_robotics/agv_pro_description/meshes/{ => wheels}/wheel_lf_link.stl (100%)
 rename src/elephant_robotics/agv_pro_description/meshes/{ => wheels}/wheel_rb_link.stl (100%)
 rename src/elephant_robotics/agv_pro_description/meshes/{ => wheels}/wheel_rf_link.stl (100%)
 delete mode 100644 src/elephant_robotics/agv_pro_description/urdf/agv_pro.ros2_control.xacro
 create mode 100644 src/elephant_robotics/agv_pro_description/urdf/control/agv_pro.ros2_control.xacro
 create mode 100644 src/elephant_robotics/agv_pro_description/urdf/parts/inertia.urdf.xacro
 create mode 100644 src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro

diff --git a/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml b/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml
index 721baa0..0054f27 100644
--- a/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml
+++ b/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml
@@ -11,26 +11,21 @@ controller_manager:
 # ─────────────────────────────────────────────────────────────────────────────
 # Mecanum drive controller
 # ─────────────────────────────────────────────────────────────────────────────
-# NOTE: The URDF joint names do not match the physical wheel positions due to a
-# naming inconsistency in agv_pro.urdf.  The mapping between the controller's
-# logical positions and the URDF joint names is as follows:
-#
-#   Physical position │ URDF joint name
-#   ──────────────────┼────────────────────────────
-#   front-left  (FL)  │ left_front_wheel_joint   ✓
-#   front-right (FR)  │ left_rear_wheel_joint     ← physically front-right
-#   rear-left   (RL)  │ right_front_wheel_joint   ← physically rear-left
-#   rear-right  (RR)  │ right_rear_wheel_joint   ✓
+# Wheel link names match their physical corners:
+#   front-left  (FL) -> left_front_wheel_joint
+#   front-right (FR) -> right_front_wheel_joint
+#   rear-left   (RL) -> left_rear_wheel_joint
+#   rear-right  (RR) -> right_rear_wheel_joint
 #
 # The same mapping is used by the hardware interface (front_left_joint /
 # front_right_joint / rear_left_joint / rear_right_joint params in the URDF
 # <ros2_control> block).
 mecanum_drive_controller:
   ros__parameters:
-    front_left_wheel_command_joint_name:  left_front_wheel_joint
-    front_right_wheel_command_joint_name: left_rear_wheel_joint
-    rear_left_wheel_command_joint_name:   right_front_wheel_joint
-    rear_right_wheel_command_joint_name:  right_rear_wheel_joint
+    front_left_wheel_command_joint_name:  front_left_wheel_joint
+    front_right_wheel_command_joint_name: front_right_wheel_joint
+    rear_left_wheel_command_joint_name:   rear_left_wheel_joint
+    rear_right_wheel_command_joint_name:  rear_right_wheel_joint
 
     odom_frame_id:  odom
     base_frame_id:  base_footprint
diff --git a/src/elephant_robotics/agv_pro_description/meshes/wheel_lb_link.stl b/src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_lb_link.stl
similarity index 100%
rename from src/elephant_robotics/agv_pro_description/meshes/wheel_lb_link.stl
rename to src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_lb_link.stl
diff --git a/src/elephant_robotics/agv_pro_description/meshes/wheel_lf_link.stl b/src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_lf_link.stl
similarity index 100%
rename from src/elephant_robotics/agv_pro_description/meshes/wheel_lf_link.stl
rename to src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_lf_link.stl
diff --git a/src/elephant_robotics/agv_pro_description/meshes/wheel_rb_link.stl b/src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_rb_link.stl
similarity index 100%
rename from src/elephant_robotics/agv_pro_description/meshes/wheel_rb_link.stl
rename to src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_rb_link.stl
diff --git a/src/elephant_robotics/agv_pro_description/meshes/wheel_rf_link.stl b/src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_rf_link.stl
similarity index 100%
rename from src/elephant_robotics/agv_pro_description/meshes/wheel_rf_link.stl
rename to src/elephant_robotics/agv_pro_description/meshes/wheels/wheel_rf_link.stl
diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro
index 25aa39b..0fad2ca 100644
--- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro
+++ b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro
@@ -2,40 +2,18 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <!--
-    Gazebo (gz-sim Harmonic) specific tags. Only included when sim=true.
-    Provides wheel surface friction/material and loads the gz_ros2_control
-    system plugin that hosts the controller_manager inside gz-sim.
+    Gazebo (gz-sim Harmonic) integration. Only two responsibilities:
+      1. Load the gz_ros2_control system plugin, which hosts the
+         controller_manager inside gz-sim and reads the controller yaml.
+    Per-wheel mecanum friction is emitted by the wheel macro when
+    gazebo_ignition (sim) is true, so it is NOT repeated here.
   -->
-  <xacro:macro name="agv_pro_gazebo" params="namespace controllers_file">
-
-    <!-- Per-wheel surface friction + dark material -->
-    <xacro:macro name="wheel_gz" params="link">
-      <gazebo reference="${link}">
-        <mu1>0.8</mu1>
-        <mu2>0.8</mu2>
-        <kp>100000.0</kp>
-        <kd>1.0</kd>
-        <visual>
-          <material>
-            <ambient>0.1 0.1 0.1 1</ambient>
-            <diffuse>0.1 0.1 0.1 1</diffuse>
-            <specular>0.1 0.1 0.1 1</specular>
-          </material>
-        </visual>
-      </gazebo>
-    </xacro:macro>
-
-    <xacro:wheel_gz link="${namespace}left_front_wheel_link"/>
-    <xacro:wheel_gz link="${namespace}right_front_wheel_link"/>
-    <xacro:wheel_gz link="${namespace}left_rear_wheel_link"/>
-    <xacro:wheel_gz link="${namespace}right_rear_wheel_link"/>
-
-    <!-- gz_ros2_control system plugin: hosts the controller_manager in gz-sim -->
+  <xacro:macro name="agv_pro_gazebo" params="prefix controllers_file">
     <gazebo>
       <plugin filename="gz_ros2_control-system"
-              name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+        name="gz_ros2_control::GazeboSimROS2ControlPlugin">
         <parameters>${controllers_file}</parameters>
       </plugin>
     </gazebo>
   </xacro:macro>
-</robot>
+</robot>
\ No newline at end of file
diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.ros2_control.xacro b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.ros2_control.xacro
deleted file mode 100644
index 6ffdfbd..0000000
--- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.ros2_control.xacro
+++ /dev/null
@@ -1,59 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-  <!--
-    ros2_control block shared by the physical robot and the Gazebo simulation.
-
-    The only difference between the two is the <hardware> plugin:
-      sim=false  ->  agv_pro_hardware/AgvProHardwareInterface  (real serial base)
-      sim=true   ->  gz_ros2_control/GazeboSimSystem           (Gazebo Harmonic)
-
-    Joint-name note: the URDF joint origins show two wheels are physically at the
-    'wrong' position relative to their names (left_rear is physically front-right,
-    right_front is physically rear-left). The hardware plugin (real) and the
-    mecanum_drive_controller (both) resolve this via the front/rear/left/right
-    parameter mapping.
-  -->
-  <xacro:macro name="agv_pro_ros2_control" params="sim namespace port_name">
-    <ros2_control name="AgvProSystem" type="system">
-      <hardware>
-        <xacro:if value="${sim}">
-          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
-        </xacro:if>
-        <xacro:unless value="${sim}">
-          <plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
-          <param name="port_name">${port_name}</param>
-          <param name="wheel_radius">0.072</param>
-          <param name="lx">0.172</param>
-          <param name="ly">0.180</param>
-          <!-- Physical wheel position -> URDF joint name mapping -->
-          <param name="front_left_joint">${namespace}left_front_wheel_joint</param>
-          <param name="front_right_joint">${namespace}left_rear_wheel_joint</param>
-          <param name="rear_left_joint">${namespace}right_front_wheel_joint</param>
-          <param name="rear_right_joint">${namespace}right_rear_wheel_joint</param>
-        </xacro:unless>
-      </hardware>
-
-      <joint name="${namespace}left_front_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="${namespace}left_rear_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="${namespace}right_front_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      <joint name="${namespace}right_rear_wheel_joint">
-        <command_interface name="velocity"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-    </ros2_control>
-  </xacro:macro>
-</robot>
diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro
index f481bfc..696f688 100644
--- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro
+++ b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro
@@ -1,255 +1,152 @@
 <?xml version="1.0" encoding="utf-8"?>
-<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro"
+    xmlns:gz="http://gazebosim.org/schema">
 
-  <!-- Optional namespace prefix for all link/joint names -->
-  <xacro:arg name="namespace" default=""/>
-  <xacro:property name="namespace" value="$(arg namespace)"/>
+    <!-- Optional namespace prefix for all link/joint names -->
+    <xacro:arg name="prefix" default="" />
+    <xacro:property name="prefix" value="$(arg prefix)" />
 
-  <!-- Hardware selection: false -> real serial base, true -> Gazebo (gz-sim) -->
-  <xacro:arg name="sim" default="false"/>
-  <xacro:property name="sim" value="$(arg sim)"/>
+    <!-- Hardware selection: false -> real serial base, true -> Gazebo (gz-sim) -->
+    <xacro:arg name="sim" default="false" />
+    <xacro:property name="sim" value="$(arg sim)" />
 
-  <!-- Real-hardware serial port (ignored when sim=true) -->
-  <xacro:arg name="port_name" default="/dev/agvpro_controller"/>
-  <xacro:property name="port_name" value="$(arg port_name)"/>
+    <!-- Real-hardware serial port (ignored when sim=true) -->
+    <xacro:arg name="port_name" default="/dev/agvpro_controller" />
+    <xacro:property name="port_name" value="$(arg port_name)" />
 
-  <!-- Controller params file consumed by gz_ros2_control (sim only) -->
-  <xacro:arg name="controllers_file" default=""/>
-  <xacro:property name="controllers_file" value="$(arg controllers_file)"/>
+    <!-- Controller params file consumed by gz_ros2_control (sim only) -->
+    <xacro:arg name="controllers_file" default="" />
+    <xacro:property name="controllers_file" value="$(arg controllers_file)" />
 
-  <link name="${namespace}base_footprint"/>
+    <link name="${prefix}base_footprint" />
 
-  <joint name="${namespace}base_joint" type="fixed">
-    <parent link="${namespace}base_footprint"/>
-    <child link="${namespace}base_link" />
-    <origin xyz="0 0 0.020" rpy="0 0 0"/>
-  </joint>
+    <joint name="${prefix}base_joint" type="fixed">
+        <parent link="${prefix}base_footprint" />
+        <child link="${prefix}base_link" />
+        <origin xyz="0 0 0.020" rpy="0 0 0" />
+    </joint>
 
-  <link name="${namespace}base_link">
-    <inertial>
-      <origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
-      <mass value="19.236" />
-      <inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
-               iyy="0.24191" iyz="0.0044629"
-               izz="0.33755" />
-    </inertial>
+    <link name="${prefix}base_link">
+        <inertial>
+            <origin xyz="-0.0076254 0.0 0.06693" rpy="0 0 0" />
+            <mass value="19.236" />
+            <inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
+                iyy="0.24191" iyz="0.0044629"
+                izz="0.33755" />
+        </inertial>
 
-    <visual>
-      <origin xyz="0 0 0"
-              rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/base_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
+        <visual>
+            <origin xyz="0 0 0"
+                rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://agv_pro_description/meshes/base_link.stl" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
 
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/base_link.stl" />
-      </geometry>
-    </collision>
-  </link>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://agv_pro_description/meshes/base_link.stl" />
+            </geometry>
+        </collision>
+    </link>
 
-  <link name="${namespace}right_rear_wheel_link">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="0.21659" />
-      <inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
-               iyy="0.00097519" iyz="-2.3635E-07"
-               izz="0.00051178" />
-    </inertial>
+    <!-- Wheels (built from the shared wheel macro) -->
+    <xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro" />
 
-    <visual>
-      <origin xyz="0 0 0"
-              rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
+    <xacro:agv_pro_wheel
+        prefix="${prefix}front_right_"
+        parent="${prefix}base_link"
+        reflect="false"
+        mesh="wheel_lb_link.stl"
+        origin_xyz="0.17128 -0.1799 0.052"
+        gazebo_ignition="${sim}" />
 
-    <collision>
-      <origin xyz="0 0 0"
-              rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
-      </geometry>
-    </collision>
-  </link>
+    <xacro:agv_pro_wheel
+        prefix="${prefix}front_left_"
+        parent="${prefix}base_link"
+        reflect="true"
+        mesh="wheel_lf_link.stl"
+        origin_xyz="0.17128 0.1799 0.052"
+        gazebo_ignition="${sim}" />
 
-  <joint name="${namespace}right_rear_wheel_joint" type="continuous">
-    <origin xyz="-0.171806101587598 -0.179900399999999 0.0518836514526621" rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}right_rear_wheel_link" />
-    <axis xyz="0 1 0" />
-  </joint>
+    <xacro:agv_pro_wheel
+        prefix="${prefix}rear_left_"
+        parent="${prefix}base_link"
+        reflect="false"
+        mesh="wheel_rf_link.stl"
+        origin_xyz="-0.17181 0.1799 0.052"
+        gazebo_ignition="${sim}" />
 
-  <link name="${namespace}right_front_wheel_link">
-    <inertial>
-      <origin xyz="6.6563E-05 -0.019725 8.3836E-05" rpy="0 0 0" />
-      <mass value="0.21659122149244" />
-      <inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
-               iyy="0.00097519" iyz="-2.3635E-07"
-               izz="0.00051178" />
-    </inertial>
+    <xacro:agv_pro_wheel
+        prefix="${prefix}rear_right_"
+        parent="${prefix}base_link"
+        reflect="true"
+        mesh="wheel_rb_link.stl"
+        origin_xyz="-0.171806101587598 -0.179900399999999 0.052"
+        gazebo_ignition="${sim}" />
 
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
 
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
-      </geometry>
-    </collision>
-  </link>
+    <link name="${prefix}laser_link">
+        <inertial>
+            <origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
+            <mass value="0.049095" />
+            <inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05"
+                iyz="-4.2154E-09"
+                izz="3.4612E-05" />
+        </inertial>
 
-  <joint name="${namespace}right_front_wheel_joint" type="continuous">
-    <origin xyz="-0.17181 0.1799 0.051884"
-      rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}right_front_wheel_link" />
-    <axis xyz="0 1 0" />
-  </joint>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
 
-  <link name="${namespace}left_front_wheel_link">
-    <inertial>
-      <origin xyz="1.4671E-06 -0.019803 4.3218E-06" rpy="0 0 0" />
-      <mass value="0.3015" />
-      <inertia ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
-               iyy="0.00099948" iyz="7.5332E-08"
-               izz="0.00052362" />
-    </inertial>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
+            </geometry>
+        </collision>
+    </link>
 
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
+    <joint name="${prefix}lidar_joint" type="fixed">
+        <origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
+        <parent link="${prefix}base_link" />
+        <child link="${prefix}laser_link" />
+    </joint>
 
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
-      </geometry>
-    </collision>
-  </link>
+    <link name="${prefix}camera_link" />
 
-  <joint name="${namespace}left_front_wheel_joint" type="continuous">
-    <origin xyz="0.17128 0.1799 0.052"
-      rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}left_front_wheel_link" />
-    <axis xyz="0 1 0" />
-  </joint>
+    <joint name="${prefix}camera_joint" type="fixed">
+        <origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
+        <parent link="${prefix}base_link" />
+        <child link="${prefix}camera_link" />
+    </joint>
 
-  <link name="${namespace}left_rear_wheel_link">
-    <inertial>
-      <origin xyz="-2.4454E-06 0.019725 -4.3121E-06" rpy="0 0 0" />
-      <mass value="0.29613" />
-      <inertia ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
-               iyy="0.0009752" iyz="1.173E-07"
-               izz="0.00051131" />
-    </inertial>
+    <link name="${prefix}imu_link" />
 
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
+    <joint name="${prefix}imu_joint" type="fixed">
+        <origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
+        <parent link="${prefix}base_link" />
+        <child link="${prefix}imu_link" />
+    </joint>
 
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
-      </geometry>
-    </collision>
-  </link>
+    <!-- ros2_control: real hardware or gz_ros2_control depending on 'sim' -->
+    <xacro:include filename="$(find agv_pro_description)/urdf/control/agv_pro.ros2_control.xacro" />
+    <xacro:agv_pro_ros2_control sim="${sim}" prefix="${prefix}" port_name="${port_name}" />
 
-  <joint name="${namespace}left_rear_wheel_joint" type="continuous">
-    <origin xyz="0.17128 -0.1799 0.052" rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}left_rear_wheel_link" />
-    <axis xyz="0 1 0" />
-  </joint>
+    <!-- Gazebo system plugin (hosts controller_manager); wheel friction is in the wheel macro -->
+    <xacro:if value="${sim}">
+        <xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro" />
+        <xacro:agv_pro_gazebo prefix="${prefix}" controllers_file="${controllers_file}" />
+    </xacro:if>
 
-  <link name="${namespace}laser_link">
-    <inertial>
-      <origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
-      <mass value="0.049095" />
-      <inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05"
-               iyz="-4.2154E-09"
-               izz="3.4612E-05" />
-    </inertial>
-
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
-      </geometry>
-      <material name="">
-        <color rgba="1 1 1 1" />
-      </material>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="${namespace}lidar_joint" type="fixed">
-    <origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}laser_link" />
-  </joint>
-
-  <link name="${namespace}camera_link"/>
-
-  <joint name="${namespace}camera_joint" type="fixed">
-    <origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}camera_link" />
-  </joint>
-
-  <link name="${namespace}imu_link"/>
-
-  <joint name="${namespace}imu_joint" type="fixed">
-    <origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
-    <parent link="${namespace}base_link" />
-    <child link="${namespace}imu_link" />
-  </joint>
-
-  <!-- ros2_control: real hardware or gz_ros2_control depending on 'sim' -->
-  <xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.ros2_control.xacro"/>
-  <xacro:agv_pro_ros2_control sim="${sim}" namespace="${namespace}" port_name="${port_name}"/>
-
-  <!-- Gazebo-only tags (friction, material, gz_ros2_control system plugin) -->
-  <xacro:if value="${sim}">
-    <xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro"/>
-    <xacro:agv_pro_gazebo namespace="${namespace}" controllers_file="${controllers_file}"/>
-  </xacro:if>
-
-</robot>
+</robot>
\ No newline at end of file
diff --git a/src/elephant_robotics/agv_pro_description/urdf/control/agv_pro.ros2_control.xacro b/src/elephant_robotics/agv_pro_description/urdf/control/agv_pro.ros2_control.xacro
new file mode 100644
index 0000000..60198f9
--- /dev/null
+++ b/src/elephant_robotics/agv_pro_description/urdf/control/agv_pro.ros2_control.xacro
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--
+    Single ros2_control block, shared by the real robot and the Gazebo sim.
+    Only the <hardware> plugin differs:
+      sim=false -> agv_pro_hardware/AgvProHardwareInterface  (real serial base)
+      sim=true  -> gz_ros2_control/GazeboSimSystem           (Gazebo Harmonic)
+
+    Joint names match the wheel macro:
+      <prefix>{front_left,front_right,rear_left,rear_right}_wheel_joint
+    Each wheel sits at its true corner, so the mapping is 1:1 (no swaps).
+  -->
+  <xacro:macro name="agv_pro_ros2_control" params="sim prefix port_name">
+    <ros2_control name="AgvProSystem" type="system">
+      <hardware>
+        <xacro:if value="${sim}">
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </xacro:if>
+        <xacro:unless value="${sim}">
+          <plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
+          <param name="port_name">${port_name}</param>
+          <param name="wheel_radius">0.072</param>
+          <param name="lx">0.172</param>
+          <param name="ly">0.180</param>
+          <param name="front_left_joint">${prefix}front_left_wheel_joint</param>
+          <param name="front_right_joint">${prefix}front_right_wheel_joint</param>
+          <param name="rear_left_joint">${prefix}rear_left_wheel_joint</param>
+          <param name="rear_right_joint">${prefix}rear_right_wheel_joint</param>
+        </xacro:unless>
+      </hardware>
+
+      <joint name="${prefix}front_left_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+      <joint name="${prefix}front_right_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+      <joint name="${prefix}rear_left_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+      <joint name="${prefix}rear_right_wheel_joint">
+        <command_interface name="velocity"/>
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+      </joint>
+    </ros2_control>
+  </xacro:macro>
+</robot>
diff --git a/src/elephant_robotics/agv_pro_description/urdf/parts/inertia.urdf.xacro b/src/elephant_robotics/agv_pro_description/urdf/parts/inertia.urdf.xacro
new file mode 100644
index 0000000..81f4994
--- /dev/null
+++ b/src/elephant_robotics/agv_pro_description/urdf/parts/inertia.urdf.xacro
@@ -0,0 +1,59 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+  <!-- code from https://github.com/RobotnikAutomation/robotnik_description/blob/jazzy-devel/urdf/inertia.urdf.xacro -->
+  <!-- source en.wikipedia.org/wiki/List_of_moments_of_inertia-->
+
+  <!-- TODO Solid sphere of radius r and mass m-->
+  <xacro:macro
+    name="solid_sphere_inertia"
+    params="m r">
+    <inertia
+      ixx="${(2*r*r*m)/5}"
+      ixy="0"
+      ixz="0"
+      iyy="${(2*r*r*m)/5}"
+      iyz="0"
+      izz="${(2*r*r*m)/5}" />
+  </xacro:macro>
+
+  <!-- TODO Hollow sphere of radius r and mass m-->
+  <xacro:macro
+    name="hollow_sphere_inertia"
+    params="m r">
+    <inertia
+      ixx="${(2*r*r*m)/3}"
+      ixy="0"
+      ixz="0"
+      iyy="${(2*r*r*m)/3}"
+      iyz="0"
+      izz="${(2*r*r*m)/3}" />
+  </xacro:macro>
+
+  <!-- TODO Solid cuboid of width w, height h, depth d, and mass m -->
+  <!-- yes, axis in solid_cuboid_inertia are changed, w relates to X axis, d to Z axis, and h to Y axis -->
+  <!-- Keep this comment -->
+  <xacro:macro
+    name="solid_cuboid_inertia"
+    params="m h d w">
+    <inertia
+      ixx="${(m*(h*h+d*d))/12}"
+      ixy="0"
+      ixz="0"
+      iyy="${(m*(w*w+d*d))/12}"
+      iyz="0"
+      izz="${(m*(w*w+h*h))/12}" />
+  </xacro:macro>
+
+  <!-- TODO Solid cylinder of radius r, height h and mass m -->
+  <xacro:macro
+    name="solid_cylinder_inertia"
+    params="m r h">
+    <inertia
+      ixx="${m*(3*r*r+h*h)/12}"
+      ixy="0"
+      ixz="0"
+      iyy="${m*(3*r*r+h*h)/12}"
+      iyz="0"
+      izz="${m*r*r/2}" />
+  </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro b/src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro
new file mode 100644
index 0000000..4e7196e
--- /dev/null
+++ b/src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro
@@ -0,0 +1,113 @@
+<?xml version="1.0"?>
+<robot
+    name="wheel_macro"
+    xmlns:xacro="http://www.ros.org/wiki/xacro"
+    xmlns:gz="http://gazebosim.org/schema">
+    <!--
+    AGV Pro wheel macro.
+
+    Builds a continuous-rotation wheel (link + joint) from a mesh, mass,
+    inertia tensor and mounting origin.
+
+    Params:
+      prefix               base name, e.g. 'left_front'
+      mesh                 STL filename in agv_pro_description/meshes
+      reflect              reflect the model and the physics
+      wheel_radius          
+      wheel_height
+      wheel_mass           link mass (kg)
+      origin_xyz           joint origin relative to base_link
+  -->
+    <xacro:macro
+        name="agv_pro_wheel"
+        params="prefix
+            parent
+            origin_xyz
+            reflect
+            mesh
+            wheel_radius:=0.072
+            wheel_height:=0.04
+            wheel_mass:=0.25
+            gazebo_ignition:=false">
+
+        <xacro:macro name="cylinder_inertia" params="m r h">
+            <inertia
+                ixx="${m*(3*r*r+h*h)/12}"
+                ixy="0"
+                ixz="0"
+                iyy="${m*r*r/2}"
+                iyz="0"
+                izz="${m*(3*r*r+h*h)/12}" />
+        </xacro:macro>
+
+        <joint name="${prefix}wheel_joint" type="continuous">
+            <origin xyz="${origin_xyz}" rpy="0 0 0" />
+            <parent link="${parent}" />
+            <child link="${prefix}wheel_link" />
+            <axis xyz="0 1 0" />
+            <limit effort="100000" velocity="100" />
+        </joint>
+
+
+        <link name="${prefix}wheel_link">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh
+                        filename="package://agv_pro_description/meshes/wheels/${mesh}" />
+                </geometry>
+                <material name="darkgrey">
+                    <color rgba="0.2 0.2 0.2 1" />
+                </material>
+            </visual>
+
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <sphere radius="${wheel_radius}" />
+                </geometry>
+            </collision>
+
+            <inertial>
+                <mass value="${wheel_mass}" />
+                <origin xyz="0 0 0" />
+                <xacro:cylinder_inertia
+                    m="${wheel_mass}"
+                    r="${wheel_radius}"
+                    h="${wheel_height}" />
+            </inertial>
+
+        </link>
+
+
+        <xacro:if value="${gazebo_ignition}">
+            <gazebo reference="${prefix}wheel_link">
+                <collision>
+                    <surface>
+                        <friction>
+                            <ode>
+                                <mu>1.0</mu>
+                                <mu2>0.0</mu2>
+                                <xacro:if value="${reflect}">
+                                    <fdir1
+                                        gz:expressed_in="$(arg prefix)base_footprint">1 -1 0</fdir1>
+                                </xacro:if>
+                                <xacro:unless value="${reflect}">
+                                    <fdir1
+                                        gz:expressed_in="$(arg prefix)base_footprint">1 1 0</fdir1>
+                                </xacro:unless>
+                            </ode>
+                        </friction>
+                        <contact>
+                            <ode>
+                                <kp>100000.0</kp>
+                                <kd>10.0</kd>
+                            </ode>
+                        </contact>
+                    </surface>
+                </collision>
+            </gazebo>
+        </xacro:if>
+
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml b/src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml
index 83ce34c..cc4b7be 100644
--- a/src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml
+++ b/src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml
@@ -17,23 +17,13 @@ controller_manager:
 # (gz_ros2_control/GazeboSimSystem here vs agv_pro_hardware/AgvProHardwareInterface
 # on the real robot); the controller parameters are identical.
 #
-# NOTE: The URDF joint names do not match the physical wheel positions due to a
-# naming inconsistency in the shared robot model. The controller compensates via
-# the front/rear/left/right command-joint mapping below:
-#
-#   Physical position │ URDF joint name
-#   ──────────────────┼────────────────────────────
-#   front-left  (FL)  │ left_front_wheel_joint   ✓
-#   front-right (FR)  │ left_rear_wheel_joint     ← physically front-right
-#   rear-left   (RL)  │ right_front_wheel_joint   ← physically rear-left
-#   rear-right  (RR)  │ right_rear_wheel_joint   ✓
 # ─────────────────────────────────────────────────────────────────────────────
 mecanum_drive_controller:
   ros__parameters:
-    front_left_wheel_command_joint_name:  left_front_wheel_joint
-    front_right_wheel_command_joint_name: left_rear_wheel_joint
-    rear_left_wheel_command_joint_name:   right_front_wheel_joint
-    rear_right_wheel_command_joint_name:  right_rear_wheel_joint
+    front_left_wheel_command_joint_name:  front_left_wheel_joint
+    front_right_wheel_command_joint_name: front_right_wheel_joint
+    rear_left_wheel_command_joint_name:   rear_left_wheel_joint
+    rear_right_wheel_command_joint_name:  rear_right_wheel_joint
 
     odom_frame_id:  odom
     base_frame_id:  base_footprint