Fix patch application in container.

src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml

@@ -11,21 +11,26 @@ controller_manager:
 # ─────────────────────────────────────────────────────────────────────────────
 # Mecanum drive controller
 # ─────────────────────────────────────────────────────────────────────────────
-# Wheel link names match their physical corners:
+# NOTE: The URDF joint names do not match the physical wheel positions due to a
-#   front-left  (FL) -> left_front_wheel_joint
+# naming inconsistency in agv_pro.urdf.  The mapping between the controller's
-#   front-right (FR) -> right_front_wheel_joint
+# logical positions and the URDF joint names is as follows:
-#   rear-left   (RL) -> left_rear_wheel_joint
+#
-#   rear-right  (RR) -> right_rear_wheel_joint
+#   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   ✓
 #
 # 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:  front_left_wheel_joint
+    front_left_wheel_command_joint_name:  left_front_wheel_joint
-    front_right_wheel_command_joint_name: front_right_wheel_joint
+    front_right_wheel_command_joint_name: left_rear_wheel_joint
-    rear_left_wheel_command_joint_name:   rear_left_wheel_joint
+    rear_left_wheel_command_joint_name:   right_front_wheel_joint
-    rear_right_wheel_command_joint_name:  rear_right_wheel_joint
+    rear_right_wheel_command_joint_name:  right_rear_wheel_joint
 
     odom_frame_id:  odom
     base_frame_id:  base_footprint

src/elephant_robotics/agv_pro_bringup/launch/agv_pro_bringup.launch.py

@@ -36,7 +36,7 @@ def generate_launch_description():
     urdf_file = os.path.join(
         get_package_share_directory('agv_pro_description'),
         'urdf',
-        'agv_pro.urdf.xacro'
+        'agv_pro.urdf'
     )
 
     # Pass both namespace and port_name into the xacro processor so that the
@@ -117,12 +117,9 @@ def generate_launch_description():
         output='screen',
     )
 
-    # mecanum_drive_controller subscribes to ~/reference (TwistStamped). Remap it
+    # mecanum_drive_controller subscribes to ~/reference (TwistStamped).
-    # to the standard /cmd_vel so nav2 and teleop work without extra flags
+    # --controller-ros-args remaps it to /cmd_vel so nav2 and teleop work
-    # (teleop still needs stamped:=true).
+    # without extra flags (teleop still needs stamped:=true).
-    # NOTE: the remap key MUST be the private name '~/reference' — a bare
-    # 'reference:=/cmd_vel' is silently ignored and the controller stays on
-    # /mecanum_drive_controller/reference.
     # Delayed slightly so controller_manager is ready before spawning.
     mecanum_drive_controller_spawner = TimerAction(
         period=2.0,
@@ -133,13 +130,29 @@ def generate_launch_description():
                 arguments=[
                     'mecanum_drive_controller',
                     '--controller-manager', 'controller_manager',
-                    '--controller-ros-args', '-r ~/reference:=/cmd_vel',
+                    '--controller-ros-args', '-r reference:=/cmd_vel',
                 ],
                 output='screen',
             )
         ]
     )
 
+    # Relay /cmd_vel (TwistStamped) → /mecanum_drive_controller/reference.
+    # This bridges the standard nav2/teleop topic to the controller's
+    # internal subscription, which ros2_control does not remap at load time.
+    # lazy:=false ensures the subscription exists before any publisher appears.
+    cmd_vel_relay = Node(
+        package='topic_tools',
+        executable='relay',
+        name='cmd_vel_relay',
+        parameters=[{
+            'input_topic':  '/cmd_vel',
+            'output_topic': '/mecanum_drive_controller/reference',
+            'lazy': False,
+        }],
+        output='screen',
+    )
+
     lidar_launchs = [
         include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
         include_lidar('agv_pro_bringup', 'MID360_launch.py', enable_lidar, lidar_type, 'mid360'),
@@ -157,6 +170,7 @@ def generate_launch_description():
             robot_state_pub,
             joint_state_broadcaster_spawner,
             mecanum_drive_controller_spawner,
+            cmd_vel_relay,
             *lidar_launchs,
         ]
     )

src/elephant_robotics/agv_pro_bringup/package.xml

@@ -12,6 +12,7 @@
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>mecanum_drive_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
   <exec_depend>agv_pro_hardware</exec_depend>
   <exec_depend>agv_pro_description</exec_depend>
   <exec_depend>rviz2</exec_depend>

src/elephant_robotics/agv_pro_description/launch/agv_pro_display.launch.py

@@ -20,7 +20,7 @@ def generate_launch_description():
     urdf_file = os.path.join(
         get_package_share_directory('agv_pro_description'),
         'urdf',
-        'agv_pro.urdf.xacro'
+        'agv_pro.urdf'
     )
 
     robot_description_content = Command([

src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro

@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-  <!--
-    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="prefix controllers_file">
-    <gazebo>
-      <plugin filename="gz_ros2_control-system"
-        name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-        <parameters>${controllers_file}</parameters>
-      </plugin>
-    </gazebo>
-  </xacro:macro>
-</robot>

src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf

@@ -0,0 +1,279 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="AGV pro" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:arg name="namespace" default=""/>
+  <xacro:property name="namespace" value="$(arg namespace)"/>
+
+  <xacro:arg name="port_name" default="/dev/agvpro_controller"/>
+
+  <link name="${namespace}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>
+
+  <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>
+
+    <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>
+
+  <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>
+
+    <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>
+
+    <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>
+  
+  <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>
+  
+  <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>
+
+    <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>
+  
+  <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>
+  
+  <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>
+
+    <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>
+
+    <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>
+  
+  <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>
+  
+  <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>
+
+    <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>
+
+    <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>
+  
+  <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>
+  
+  <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 hardware interface for the real robot.
+
+       Joint-name note: the URDF joint origins show that two wheels are
+       physically at the 'wrong' position relative to their names:
+         left_rear_wheel_joint  is physically at front-right (+x, -y)
+         right_front_wheel_joint is physically at rear-left  (-x, +y)
+       The hardware plugin and mecanum_drive_controller both use the
+       front_left/front_right/rear_left/rear_right params to resolve this.
+       ════════════════════════════════════════════════════════════════════════ -->
+  <ros2_control name="AgvProHardwareInterface" type="system">
+    <hardware>
+      <plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
+      <param name="port_name">$(arg 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>
+    </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>
+
+</robot>

src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro

@@ -1,152 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<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="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)" />
-
-    <!-- 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)" />
-
-    <link name="${prefix}base_footprint" />
-
-    <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="${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>
-
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://agv_pro_description/meshes/base_link.stl" />
-            </geometry>
-        </collision>
-    </link>
-
-    <!-- Wheels (built from the shared wheel macro) -->
-    <xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro" />
-
-    <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}" />
-
-    <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}" />
-
-    <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}" />
-
-    <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}" />
-
-
-    <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>
-
-        <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="${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>
-
-    <link name="${prefix}camera_link" />
-
-    <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="${prefix}imu_link" />
-
-    <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>
-
-    <!-- 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}" />
-
-    <!-- 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>
-
-</robot>

src/elephant_robotics/agv_pro_description/urdf/control/agv_pro.ros2_control.xacro

@@ -1,55 +0,0 @@
-<?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>

src/elephant_robotics/agv_pro_description/urdf/parts/inertia.urdf.xacro

@@ -1,59 +0,0 @@
-<?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>

src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro

@@ -1,113 +0,0 @@
-<?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>

src/elephant_robotics/agv_pro_gazebo/CMakeLists.txt

@@ -7,13 +7,14 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(urdf REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-install(DIRECTORY meshes launch rviz config worlds
+install(DIRECTORY meshes urdf launch rviz config
   DESTINATION share/${PROJECT_NAME}
 )
 

src/elephant_robotics/agv_pro_gazebo/config/agv_control copy.yaml

@@ -0,0 +1,23 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    diff_drive_controller:
+      type: diff_drive_controller/DiffDriveController
+      left_wheel_names: ["left_front_wheel_joint", "left_rear_wheel_joint"]
+      right_wheel_names: ["right_front_wheel_joint", "right_rear_wheel_joint"]
+
+      wheel_separation: 0.36
+      wheel_radius: 0.05
+
+      base_frame_id: base_link
+      use_stamped_vel: false
+      publish_rate: 50
+
+      enable_odom_tf: true
+      odom_frame_id: odom
+      pose_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
+      twist_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]

src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml

@@ -1,36 +1,28 @@
 controller_manager:
   ros__parameters:
-    update_rate: 50  # 控制器更新频率 (Hz) — matches the physical robot (ESP32 auto-report rate)
+    update_rate: 100  # 控制器更新频率 (Hz)
     use_sim_time: true  # 使用仿真时间
 
     # 定义关节状态广播器
-    joint_state_broadcaster:
+    fishbot_joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
+      use_sim_time: true
 
-    # 定义麦克纳姆轮驱动控制器
+    # 定义全向驱动控制器
-    mecanum_drive_controller:
+    fishbot_omni_drive_controller:
-      type: mecanum_drive_controller/MecanumDriveController
+      type: omni_drive_controller/OmniDriveController
 
-# ─────────────────────────────────────────────────────────────────────────────
+# 四轮全向控制器配置
-# Mecanum drive controller — kept IN SYNC with the physical robot config at
+fishbot_omni_drive_controller:
-# agv_pro_bringup/config/ros2_controllers.yaml. Only the hardware plugin differs
-# (gz_ros2_control/GazeboSimSystem here vs agv_pro_hardware/AgvProHardwareInterface
-# on the real robot); the controller parameters are identical.
-#
-# ─────────────────────────────────────────────────────────────────────────────
-mecanum_drive_controller:
   ros__parameters:
-    front_left_wheel_command_joint_name:  front_left_wheel_joint
+    front_left_wheel_joint: front_left_wheel_joint
-    front_right_wheel_command_joint_name: front_right_wheel_joint
+    front_right_wheel_joint: front_right_wheel_joint
-    rear_left_wheel_command_joint_name:   rear_left_wheel_joint
+    rear_left_wheel_joint: rear_left_wheel_joint
-    rear_right_wheel_command_joint_name:  rear_right_wheel_joint
+    rear_right_wheel_joint: rear_right_wheel_joint
-
+    wheel_separation: 0.36  # 轮距
-    odom_frame_id:  odom
+    wheel_diameter: 0.1  # 轮子直径
-    base_frame_id:  base_footprint
+    publish_rate: 50.0  # 发布频率
-    enable_odom_tf: true
+    odom_frame_id: odom
-
+    base_frame_id: base_link
-    kinematics:
+    pose_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
-      wheels_radius: 0.072  # 轮子半径 (wheel centre height above ground)
+    twist_covariance_diagonal: [0.001, 0.001, 99999.0, 99999.0, 99999.0, 0.03]
-      # sum_of_robot_center_projection_on_X_Y_axis = lx + ly (0.172 + 0.180)
-      sum_of_robot_center_projection_on_X_Y_axis: 0.352
-

src/elephant_robotics/agv_pro_gazebo/launch/agv_pro.launch.py

@@ -0,0 +1,40 @@
+import os
+ 
+from ament_index_python.packages import get_package_share_directory
+ 
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+ 
+import xacro
+ 
+ 
+def generate_launch_description():
+ 
+    # Check if we're told to use sim time
+    use_sim_time = LaunchConfiguration('use_sim_time')
+ 
+    # Process the URDF file
+    pkg_path = os.path.join(get_package_share_directory('agv_pro_gazebo'))
+    xacro_file = os.path.join(pkg_path,'urdf','agv_pro.xacro')
+    robot_description_config = xacro.process_file(xacro_file)
+    
+    # Create a robot_state_publisher node
+    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+ 
+    # Launch!
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use sim time if true'),
+ 
+        node_robot_state_publisher
+    ])

src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_display.launch.py

@@ -0,0 +1,48 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+
+    use_rviz = LaunchConfiguration('use_rviz', default='true')
+    rviz_config_dir = os.path.join(
+        get_package_share_directory('agv_pro_gazebo'),
+        'rviz',
+        'agvpro_display.rviz')
+
+    urdf_file = os.path.join(
+        get_package_share_directory('agv_pro_gazebo'),
+        'urdf',
+        'agv_pro.urdf'
+    )
+
+    with open(urdf_file, 'r') as file:
+        robot_description_content = file.read()
+
+    return LaunchDescription([
+
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher'
+        ),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            parameters=[{'robot_description': robot_description_content}]
+        ),
+        
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config_dir],
+            condition=IfCondition(use_rviz),
+            output='screen')
+
+    ])

src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py

@@ -1,178 +1,65 @@
 import os
 from launch import LaunchDescription
-from launch.actions import (
+from launch.actions import IncludeLaunchDescription
-    IncludeLaunchDescription,
-    DeclareLaunchArgument,
-    RegisterEventHandler,
-    SetEnvironmentVariable,
-    TimerAction,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import (
+from launch.substitutions import Command
-    Command,
-    EnvironmentVariable,
-    LaunchConfiguration,
-    PythonExpression,
-)
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from ament_index_python.packages import get_package_share_directory
 
-
 def generate_launch_description():
-    pkg_gazebo = get_package_share_directory('agv_pro_gazebo')
+    pkg_name = 'agv_pro_gazebo'
-    pkg_description = get_package_share_directory('agv_pro_description')
+    pkg_dir = get_package_share_directory(pkg_name)
-    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+    xacro_file = os.path.join(pkg_dir, 'urdf', 'agv_pro.xacro')
+    world_file = os.path.join(pkg_dir, 'worlds', 'empty.world')
+    rviz_config = os.path.join(pkg_dir, 'rviz', 'agvpro_display.rviz')
 
-    # Shared robot description (agv_pro_description) built for simulation.
+    robot_description_content = ParameterValue(
-    xacro_file = os.path.join(pkg_description, 'urdf', 'agv_pro.urdf.xacro')
+        Command(['xacro ', xacro_file]),
-    controllers_file = os.path.join(pkg_gazebo, 'config', 'agv_control.yaml')
+        value_type=str
-    world_file = os.path.join(pkg_gazebo, 'worlds', 'empty.world')
-    rviz_config = os.path.join(pkg_gazebo, 'rviz', 'agvpro_display.rviz')
-
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    use_rviz = LaunchConfiguration('use_rviz')
-    headless = LaunchConfiguration('headless')
-
-    robot_description = {
-        'robot_description': ParameterValue(
-            Command([
-                'xacro ', xacro_file,
-                ' sim:=true',
-                ' controllers_file:=', controllers_file,
-            ]),
-            value_type=str,
-        ),
-        'use_sim_time': use_sim_time,
-    }
-
-    # Let gz-sim resolve the package:// mesh URIs in the shared description.
-    # URDF->SDF conversion rewrites package://agv_pro_description/... to
-    # model://agv_pro_description/..., which gz finds by searching
-    # GZ_SIM_RESOURCE_PATH for a directory named 'agv_pro_description'.
-    gz_resource_path = SetEnvironmentVariable(
-        name='GZ_SIM_RESOURCE_PATH',
-        value=[
-            os.path.dirname(pkg_description),
-            ':',
-            EnvironmentVariable('GZ_SIM_RESOURCE_PATH', default_value=''),
-        ],
-    )
-
-    # Start gz-sim (Gazebo Harmonic) with the world.
-    # '-s' (server only) is added when headless:=true.
-    gz_args = PythonExpression([
-        "'-r -v4 -s ' + ", repr(world_file),
-        " if '", headless, "' == 'true' ",
-        "else '-r -v4 ' + ", repr(world_file),
-    ])
-    gz_sim = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
-        ),
-        launch_arguments={'gz_args': gz_args}.items(),
-    )
-
-    # Publish the robot description (also consumed by gz_ros2_control)
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
-        parameters=[robot_description],
-    )
-
-    # Spawn the robot into gz-sim from the robot_description topic.
-    # Delayed so robot_state_publisher has latched /robot_description before the
-    # gz_ros2_control plugin (loaded on spawn) reads it — avoids a startup race
-    # where the in-sim controller_manager sees no <ros2_control> tag.
-    spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-topic', 'robot_description', '-name', 'agv_pro', '-z', '0.1'],
-        output='screen',
-    )
-    delayed_spawn = TimerAction(period=3.0, actions=[spawn_entity])
-
-    # Bridge the simulation clock so ROS nodes get /clock
-    clock_bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-        output='screen',
-    )
-
-    # Controllers (controller_manager runs inside the gz_ros2_control plugin)
-    joint_state_broadcaster_spawner = Node(
-        package='controller_manager',
-        executable='spawner',
-        arguments=[
-            'joint_state_broadcaster',
-            '--controller-manager', '/controller_manager',
-        ],
-        output='screen',
-    )
-
-    # Remap the controller's reference topic (~/reference) to the standard
-    # /cmd_vel so teleop and nav2 (which publish geometry_msgs/TwistStamped)
-    # drive the robot directly. NOTE: the remap key must be the private name
-    # '~/reference' — a bare 'reference:=/cmd_vel' is silently ignored.
-    mecanum_drive_controller_spawner = Node(
-        package='controller_manager',
-        executable='spawner',
-        arguments=[
-            'mecanum_drive_controller',
-            '--controller-manager', '/controller_manager',
-            '--controller-ros-args', '-r ~/reference:=/cmd_vel',
-        ],
-        output='screen',
-    )
-
-    rviz = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        arguments=['-d', rviz_config],
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(use_rviz),
-        output='screen',
     )
+    robot_description = {'robot_description': robot_description_content}
 
     return LaunchDescription([
-        DeclareLaunchArgument(
+        # Launch Gazebo
-            'use_sim_time',
+        IncludeLaunchDescription(
-            default_value='true',
+            PythonLaunchDescriptionSource(
-            description='Use the Gazebo simulation clock',
+                os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+            ),
+            launch_arguments={'world': world_file}.items()
         ),
-        DeclareLaunchArgument(
+
-            'use_rviz',
+        # Spawn robot into Gazebo
-            default_value='true',
+        Node(
-            description='Launch RViz2',
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=['-topic', 'robot_description',
+                       '-entity', 'agv_pro'],
+            output='screen'
         ),
-        DeclareLaunchArgument(
+
-            'headless',
+        # State publisher
-            default_value='false',
+        Node(
-            description='Run gz-sim without the GUI (server only)',
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[robot_description]
         ),
-        gz_resource_path,
+
-        gz_sim,
+        Node(
-        robot_state_publisher,
+            package='joint_state_publisher',
-        clock_bridge,
+            executable='joint_state_publisher',
-        delayed_spawn,
+            name='joint_state_publisher',
-        # Load controllers only after the robot has been spawned
+            output='screen',
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=spawn_entity,
-                on_exit=[joint_state_broadcaster_spawner],
-            )
         ),
-        RegisterEventHandler(
+
-            OnProcessExit(
+        # Optional: RViz
-                target_action=joint_state_broadcaster_spawner,
+        Node(
-                on_exit=[mecanum_drive_controller_spawner],
+            package='rviz2',
-            )
+            executable='rviz2',
+            name='rviz2',
+            output='screen',
+            arguments=['-d', rviz_config],
         ),
-        rviz,
     ])

src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launchcopy.py

@@ -0,0 +1,71 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    pkg_name = 'agv_pro_description'
+
+    # Paths
+    pkg_dir = get_package_share_directory(pkg_name)
+    xacro_file = os.path.join(pkg_dir, 'urdf', 'agv_pro.xacro')
+    world_file = os.path.join(pkg_dir, 'worlds', 'empty.world')  # 创建一个空 world 即可
+    rviz_config = os.path.join(pkg_dir, 'rviz', 'agvpro_display.rviz')
+
+    robot_description_content = Command(['xacro ', xacro_file])
+    robot_description = {'robot_description': robot_description_content}
+
+    return LaunchDescription([
+
+        # Start Gazebo with empty world
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]
+            ),
+            launch_arguments={'world': world_file}.items()
+        ),
+
+        # Spawn robot into Gazebo
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=['-topic', 'robot_description',
+                       '-entity', 'agv_pro'],
+            output='screen'
+        ),
+
+        # Robot state publisher
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[robot_description]
+        ),
+
+        # Optionally publish joint states if not using controllers
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher',
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_state_broadcaster'],
+            output='screen',
+        ),
+
+        # RViz (optional, visualize TF & model)
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            output='screen',
+            arguments=['-d', rviz_config],
+        ),
+    ]) 

src/elephant_robotics/agv_pro_gazebo/launch/teleop_launch.py

@@ -2,7 +2,6 @@ import os
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-
 def generate_launch_description():
     return LaunchDescription([
         Node(
@@ -11,14 +10,8 @@ def generate_launch_description():
             name='teleop_keyboard',
             output='screen',
             prefix='xterm -e',  # 或 'gnome-terminal --' 替换为你的终端命令
-            # mecanum_drive_controller expects a stamped reference; teleop must
+            remappings=[
-            # publish geometry_msgs/TwistStamped with a fresh timestamp.
+                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
-            # It publishes on the standard /cmd_vel topic, which the sim launch
+            ]
-            # feeds to the controller (spawner remap + cmd_vel_relay), mirroring
-            # the physical robot.
-            parameters=[{
-                'stamped': True,
-                'frame_id': 'base_link',
-            }],
         )
     ])

src/elephant_robotics/agv_pro_gazebo/launch/test.launch.py

@@ -0,0 +1,35 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.substitutions import Command
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('agv_pro_description')
+    xacro_file = os.path.join(pkg_dir, 'urdf', 'minimal_robot.xacro')
+    world_file = os.path.join(pkg_dir, 'worlds', 'empty.world')
+
+    robot_description = {'robot_description': Command(['xacro ', xacro_file])}
+
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+            ),
+            launch_arguments={'world': world_file}.items()
+        ),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[robot_description],
+            output='screen'
+        ),
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=['-topic', 'robot_description', '-entity', 'minimal_bot'],
+            output='screen'
+        )
+    ])

src/elephant_robotics/agv_pro_gazebo/package.xml

@@ -12,14 +12,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>xacro</buildtool_depend>
   
-  <!-- Runtime dependencies -->
+  <!-- Build dependencies -->
-  <!-- Shared robot description (single source of truth for sim + real) -->
+  <depend>gazebo_ros_pkgs</depend>
-  <depend>agv_pro_description</depend>
-  <!-- Gazebo (gz-sim Harmonic) integration for ROS 2 Jazzy -->
-  <exec_depend>ros_gz_sim</exec_depend>
-  <exec_depend>ros_gz_bridge</exec_depend>
-  <depend>gz_ros2_control</depend>
-
   <depend>robot_state_publisher</depend>
   <depend>joint_state_publisher</depend>
   <depend>geometry_msgs</depend>
@@ -30,8 +24,9 @@
   <depend>ros2_control</depend>
   <depend>controller_manager</depend>
   <depend>joint_state_broadcaster</depend>
-  <depend>mecanum_drive_controller</depend>
+  <depend>diff_drive_controller</depend>
   <depend>teleop_twist_keyboard</depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/elephant_robotics/agv_pro_gazebo/urdf/agv_pro copy.xacro

@@ -0,0 +1,123 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
+
+  <!-- Gazebo-specific properties -->
+  <xacro:property name="wheel_damping" value="0.1"/>
+  <xacro:property name="wheel_axis" value="0 1 0"/>
+
+  <!-- Base footprint -->
+  <link name="base_footprint"/>
+
+  <joint name="base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="base_link" />
+    <origin xyz="0 0 0.020" rpy="0 0 0"/>
+  </joint>
+
+  <link name="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>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file://$(find 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="file://$(find agv_pro_description)/meshes/base_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Gazebo plugin for control -->
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so"/>
+  </gazebo>
+
+  <gazebo reference="base_link">
+    <material>Gazebo/White</material>
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <kp>100000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+
+  <!-- Include wheel macros -->
+  <xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro"/>
+  <xacro:include filename="$(find agv_pro_description)/urdf/parts/gazebo_control_plugin.xacro"/>
+  <!-- Add all four wheels using macro -->
+  <xacro:wheel name="right_rear_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <xacro:wheel name="right_front_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <xacro:wheel name="left_front_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
+
+  <xacro:wheel name="left_rear_wheel" mesh="file://$(find agv_pro_description)/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
+
+  <!-- Lidar -->
+  <link name="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="file://$(find 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="file://$(find agv_pro_description)/meshes/laser_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="lidar_joint" type="fixed">
+    <origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="laser_link" />
+  </joint>
+
+  <!-- ros2_control tag -->
+  <ros2_control name="AGVHardware" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+
+    <joint name="right_rear_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="right_front_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="left_front_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="left_rear_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/agv_pro.xacro

@@ -0,0 +1,92 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="agv_pro">
+
+  <!-- Define vehicle dimensions -->
+  <xacro:property name="vehicle_width" value="0.36"/>  <!-- 车辆宽度 -->
+  <xacro:property name="wheel_radius" value="0.05"/>    <!-- 轮子半径 -->
+
+  <!-- Gazebo-specific properties -->
+  <xacro:property name="wheel_damping" value="0.1"/>
+  <xacro:property name="wheel_axis" value="0 1 0"/>
+
+  <!-- Base footprint -->
+  <link name="base_footprint"/>
+
+  <joint name="base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="base_link" />
+    <origin xyz="0 0 0.020" rpy="0 0 0"/>
+  </joint>
+
+  <link name="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>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file://$(find agv_pro_gazebo)/meshes/base_link.stl" />
+      </geometry>
+      <material name=""/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file://$(find agv_pro_gazebo)/meshes/base_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Include wheel macros & controller definitions -->
+  <xacro:include filename="$(find agv_pro_gazebo)/urdf/parts/wheel_macro.xacro"/>
+
+  <!-- Wheels definition -->
+  <!-- Right Rear Wheel -->
+  <xacro:wheel name="right_rear_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <!-- Right Front Wheel -->
+  <xacro:wheel name="right_front_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <!-- Left Front Wheel -->
+  <xacro:wheel name="left_front_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
+
+  <!-- Left Rear Wheel -->
+  <xacro:wheel name="left_rear_wheel" mesh="file://$(find agv_pro_gazebo)/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
+
+
+  <!-- Lidar definition -->
+  <link name="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="file://$(find agv_pro_gazebo)/meshes/laser_link.stl" />
+      </geometry>
+      <material name=""/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="file://$(find agv_pro_gazebo)/meshes/laser_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="lidar_joint" type="fixed">
+    <origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="laser_link" />
+  </joint>
+
+  <!-- Include ros2_controller.xacro to define controllers -->
+  <xacro:include filename="$(find agv_pro_gazebo)/urdf/parts/gazebo_control_plugin.xacro"/>
+  <xacro:include filename="$(find agv_pro_gazebo)/urdf/ros2_controller.xacro"/>
+  <xacro:ros2_controller/>
+  <xacro:gazebo_control_plugin/>
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/agv_pro1.xacro

@@ -0,0 +1,144 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
+
+  <xacro:property name="wheel_axis" value="0 1 0"/>
+  <xacro:property name="wheel_damping" value="0.1"/>
+
+  <!-- Macro: wheel with Gazebo plugin -->
+  <xacro:macro name="wheel" params="name mesh origin_xyz origin_rpy mass ixx ixy ixz iyy iyz izz">
+    <link name="${name}_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value="${mass}" />
+        <inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="${mesh}" />
+        </geometry>
+        <material name=""><color rgba="1 1 1 1"/></material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="${mesh}" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${name}_joint" type="continuous">
+      <origin xyz="${origin_xyz}" rpy="${origin_rpy}"/>
+      <parent link="base_link"/>
+      <child link="${name}_link"/>
+      <axis xyz="${wheel_axis}"/>
+      <dynamics damping="${wheel_damping}"/>
+    </joint>
+
+    <transmission name="${name}_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <actuator name="${name}_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+      <joint name="${name}_joint">
+        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+      </joint>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <mu1>0.8</mu1>
+      <mu2>0.8</mu2>
+      <kp>100000.0</kp>
+      <kd>1.0</kd>
+      <material>Gazebo/Grey</material>
+    </gazebo>
+  </xacro:macro>
+
+  <!-- Base links -->
+  <link name="base_footprint"/>
+
+  <joint name="base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0.020" rpy="0 0 0"/>
+  </joint>
+
+  <link name="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>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://agv_pro_description/meshes/base_link.stl"/>
+      </geometry>
+    </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>
+
+  <!-- Gazebo plugin for ros2_control -->
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so"/>
+  </gazebo>
+
+  <!-- Wheels -->
+  <xacro:wheel name="right_rear_wheel"
+               mesh="package://agv_pro_description/meshes/wheel_rb_link.stl"
+               origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0"
+               mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
+               iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <xacro:wheel name="right_front_wheel"
+               mesh="package://agv_pro_description/meshes/wheel_rf_link.stl"
+               origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0"
+               mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
+               iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+
+  <xacro:wheel name="left_front_wheel"
+               mesh="package://agv_pro_description/meshes/wheel_lf_link.stl"
+               origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0"
+               mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
+               iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
+
+  <xacro:wheel name="left_rear_wheel"
+               mesh="package://agv_pro_description/meshes/wheel_lb_link.stl"
+               origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0"
+               mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
+               iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
+
+  <!-- Lidar -->
+  <link name="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>
+    </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="lidar_joint" type="fixed">
+    <origin xyz="0.17891 0 0.20928" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="laser_link"/>
+  </joint>
+
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/minimal_robot.xacro

@@ -0,0 +1,91 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="agv_pro">
+
+  <!-- Gazebo-specific properties -->
+  <xacro:property name="wheel_damping" value="0.1"/>
+  <xacro:property name="wheel_axis" value="0 1 0"/>
+
+  <!-- Base footprint -->
+  <link name="base_footprint"/>
+
+  <joint name="base_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="base_link" />
+    <origin xyz="0 0 0.020" rpy="0 0 0"/>
+  </joint>
+
+  <link name="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>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="model://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="model://agv_pro_description/meshes/base_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="base_link">
+    <material>Gazebo/White</material>
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
+    <kp>100000.0</kp>
+    <kd>1.0</kd>
+  </gazebo>
+
+  <!-- Include wheel macros & plugin -->
+  <xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro"/>
+  <xacro:include filename="$(find agv_pro_description)/urdf/parts/gazebo_control_plugin.xacro"/>
+
+  <!-- All four wheels with corrected mesh paths -->
+  <xacro:wheel name="right_rear_wheel" mesh="model://agv_pro_description/meshes/wheel_rb_link.stl" origin_xyz="-0.1718 -0.1799 0.05188" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+  <xacro:wheel name="right_front_wheel" mesh="model://agv_pro_description/meshes/wheel_rf_link.stl" origin_xyz="-0.17181 0.1799 0.051884" origin_rpy="0 0 0" mass="0.21659" ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07" iyy="0.00097519" iyz="-2.3635E-07" izz="0.00051178"/>
+  <xacro:wheel name="left_front_wheel" mesh="model://agv_pro_description/meshes/wheel_lf_link.stl" origin_xyz="0.17128 0.1799 0.052" origin_rpy="0 0 0" mass="0.3015" ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07" iyy="0.00099948" iyz="7.5332E-08" izz="0.00052362"/>
+  <xacro:wheel name="left_rear_wheel" mesh="model://agv_pro_description/meshes/wheel_lb_link.stl" origin_xyz="0.17128 -0.1799 0.052" origin_rpy="0 0 0" mass="0.29613" ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07" iyy="0.0009752" iyz="1.173E-07" izz="0.00051131"/>
+
+  <!-- Lidar -->
+  <link name="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="model://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="model://agv_pro_description/meshes/laser_link.stl" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="lidar_joint" type="fixed">
+    <origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="laser_link" />
+  </joint>
+
+  <!-- 插件调用 -->
+  <xacro:gazebo_control_plugin/>
+
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/parts/gazebo_control_plugin.xacro

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="gazebo_control_plugin">
+    <gazebo>
+      <!-- 使用全向控制插件 -->
+      <plugin filename="libgazebo_ros_planar_move.so" name="mecanum_drive_controller">
+        <ros>
+          <remapping>cmd_vel:=/cmd_vel</remapping>
+          <remapping>odom:=/odom</remapping>
+        </ros>
+
+        <!-- 配置全向控制 -->
+        <frontLeftJoint>front_left_wheel_joint</frontLeftJoint>    <!-- 前左轮 -->
+        <frontRightJoint>front_right_wheel_joint</frontRightJoint>  <!-- 前右轮 -->
+        <rearLeftJoint>rear_left_wheel_joint</rearLeftJoint>        <!-- 后左轮 -->
+        <rearRightJoint>rear_right_wheel_joint</rearRightJoint>     <!-- 后右轮 -->
+        <wheelDiameter>0.1</wheelDiameter>   <!-- 轮子直径 -->
+        <wheelSeparation>0.36</wheelSeparation>  <!-- 轮距（车辆宽度） -->
+
+        <torque>20</torque>  <!-- 轮子扭矩 -->
+        <topicName>cmd_vel</topicName>  <!-- 控制命令话题 -->
+        <odometryFrame>odom</odometryFrame>  <!-- 里程计坐标系 -->
+        <odometryTopic>odom</odometryTopic>  <!-- 里程计话题 -->
+        <robotBaseFrame>base_footprint</robotBaseFrame>  <!-- 机器人基础坐标系 -->
+        <publishOdomTF>true</publishOdomTF>  <!-- 发布里程计变换 -->
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/parts/wheel_macro.xacro

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="wheel" params="name mesh origin_xyz origin_rpy mass ixx ixy ixz iyy iyz izz">
+
+    <link name="${name}_link">
+      <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value="${mass}" />
+        <inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="${mesh}" />
+        </geometry>
+        <material name="gray">
+          <color rgba="0.3 0.3 0.3 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="${mesh}" />
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_joint" type="continuous">
+      <origin xyz="${origin_xyz}" rpy="${origin_rpy}" />
+      <parent link="base_link" />
+      <child link="${name}_link" />
+      <axis xyz="0 1 0"/>
+      <dynamics damping="0.1"/>
+    </joint>
+
+    <!-- Correct transmission for ROS2 -->
+    <transmission name="${name}_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${name}_joint">
+        <hardwareInterface>hardware_interface/velocity</hardwareInterface>
+      </joint>
+      <actuator name="${name}_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+        <hardwareInterface>hardware_interface/velocity</hardwareInterface>
+      </actuator>
+    </transmission>
+
+    <gazebo reference="${name}_link">
+      <mu1>0.8</mu1>
+      <mu2>0.8</mu2>
+      <kp>100000.0</kp>
+      <kd>1.0</kd>
+      <material>Gazebo/Black</material>
+    </gazebo>
+
+  </xacro:macro>
+</robot>

src/elephant_robotics/agv_pro_gazebo/urdf/ros2_controller.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="ros2_controller">
+        <ros2_control name="FishBotGazeboSystem" type="system">
+            <hardware>
+                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+            </hardware>
+            
+            <!-- 配置所有轮子的控制接口 -->
+            <joint name="front_left_wheel_joint">
+                <command_interface name="position" />
+                <command_interface name="velocity" />
+                <command_interface name="effort" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+            </joint>
+
+            <joint name="front_right_wheel_joint">
+                <command_interface name="position" />
+                <command_interface name="velocity" />
+                <command_interface name="effort" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+            </joint>
+
+            <joint name="rear_left_wheel_joint">
+                <command_interface name="position" />
+                <command_interface name="velocity" />
+                <command_interface name="effort" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+            </joint>
+
+            <joint name="rear_right_wheel_joint">
+                <command_interface name="position" />
+                <command_interface name="velocity" />
+                <command_interface name="effort" />
+                <state_interface name="position" />
+                <state_interface name="velocity" />
+                <state_interface name="effort" />
+            </joint>
+        </ros2_control>
+        <gazebo>
+            <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+                <parameters>$(find agv_pro_gazebo)/config/agv_control.yaml</parameters>
+                <ros>
+                    <remapping>/omni_drive_controller/cmd_vel:=/cmd_vel</remapping>
+                    <remapping>/omni_drive_controller/odom:=/odom</remapping>
+                </ros>
+            </plugin>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/elephant_robotics/agv_pro_gazebo/worlds/empty.world

@@ -1,69 +1,11 @@
 <?xml version="1.0" ?>
-<sdf version="1.10">
+<sdf version="1.6">
   <world name="empty_world">
-
+    <include>
-    <!-- Required gz-sim (Gazebo Harmonic) system plugins -->
+      <uri>model://ground_plane</uri>
-    <plugin filename="gz-sim-physics-system"
+    </include>
-            name="gz::sim::systems::Physics">
+    <include>
-    </plugin>
+      <uri>model://sun</uri>
-    <plugin filename="gz-sim-user-commands-system"
+    </include>
-            name="gz::sim::systems::UserCommands">
-    </plugin>
-    <plugin filename="gz-sim-scene-broadcaster-system"
-            name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-    <plugin filename="gz-sim-sensors-system"
-            name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <physics name="1ms" type="ignored">
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-    </physics>
-
-    <!-- Sun -->
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>0.8 0.8 0.8 1</diffuse>
-      <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-
-    <!-- Ground plane -->
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
   </world>
 </sdf>