Add gripper control for simulation.

Update reference_timeout for physical robot.
Add reference timeout to get robot moving quicker, and switch to pose progress checker
2026-08-06 18:07:11 +00:00 · 2026-08-02 16:08:35 +00:00 · 2026-08-02 13:20:30 +00:00 · 2026-08-01 13:55:36 +00:00 · 2026-07-31 17:49:20 +00:00 · 2026-07-31 14:54:04 +00:00
54 changed files with 3021 additions and 1250 deletions
@@ -1,3 +1,4 @@
+# syntax=docker/dockerfile:1.7-labs
 FROM osrf/ros:jazzy-desktop-full
 ARG USERNAME=USERNAME
 ARG USER_UID=1000
@@ -19,13 +20,15 @@ RUN groupadd --gid $USER_GID $USERNAME \
    && rm -rf /var/lib/apt/lists/*

 # Make sure the base image is completely up to date
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
    && apt-get upgrade -y \
    && rm -rf /var/lib/apt/lists/*


 # C++ development tools
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
    && apt-get install -y --no-install-recommends \
    build-essential \
    cmake \
@@ -37,7 +40,8 @@ RUN apt-get update \
    && rm -rf /var/lib/apt/lists/*

 # Python development tools
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
    && apt-get install -y --no-install-recommends \
    python3-pip \
    python3-dev \
@@ -48,21 +52,20 @@ RUN apt-get update \
    python3-vcstool \
    && rm -rf /var/lib/apt/lists/*

-# system deps from frozen rosdep manifest — no rosdep at build or startup
-# NOTE: There should be a check in your CI that this file is up to date.
-# Something like: 
-#   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
-COPY rosdep-packages.txt /tmp/rosdep-packages.txt
-# The --mount=type=cache option caches the apt packages with buildkit
-# The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
-# this enables parallel builds for different architectures without cache conflicts
+# Copy boilerplate project config into workspace so it can be parsed by rosdep.
+# This will be obscured by the volume mount or the workspace in the devcontainer
+# Don't include all sources, because the rosdep will re-process on any file change
+COPY --parents src/**/package.xml src/**/COLCON_IGNORE src/
+
 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
-    rm -f /etc/apt/apt.conf.d/docker-clean \
-    && apt-get update \
-    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    apt-get update \
+    && rosdep update --rosdistro $ROS_DISTRO \
+    && rosdep install --from-paths src --ignore-src -y \
    && rm -rf /var/lib/apt/lists/*

-RUN apt-get update \
+
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
    && apt-get install -y --no-install-recommends \
    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
    && rm -rf /var/lib/apt/lists/*
@@ -32,15 +32,17 @@
        "CYCLONEDDS_URI": "/workspaces/agv_pro_ros2/.devcontainer/cyclonedds.xml"
    },
    "runArgs": [
-        "--network=robot-sim",
+        // "--network=robot-sim",
+        "--network=host",
        "--pid=host",
        "--ipc=host",
+        "--group-add=dialout",
        "-e",
        "DISPLAY=${env:DISPLAY}"
    ],
    "mounts": [
+        "source=/dev,target=/dev,type=bind",
        "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached",
-        "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached",
        "source=/usr/local/share/ca-certificates,target=/usr/local/share/host-certificates,type=bind,consistency=cached",
        "source=${localEnv:HOME}/.ssh,target=/home/${localEnv:USER}/.ssh,type=bind,consistency=cached"
    ],
@@ -18,19 +18,31 @@ RUN ./patches/apply_patches.sh
 # NOTE: There should be a check in your CI that this file is up to date.
 # Something like: 
 #   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
-COPY rosdep-packages.txt /tmp/rosdep-packages.txt
+# COPY rosdep-packages.txt /tmp/rosdep-packages.txt
 # The --mount=type=cache option caches the apt packages with buildkit
 # The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
 # this enables parallel builds for different architectures without cache conflicts
+# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+#     rm -f /etc/apt/apt.conf.d/docker-clean \
+#     && apt-get update \
+#     && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+#     && rm -rf /var/lib/apt/lists/*
+
+# Make sure the base image is completely up to date
+# ros:jazzy ships /etc/apt/apt.conf.d/docker-clean which purges downloaded .debs;
+# remove it so the --mount=type=cache on /var/cache/apt actually retains archives.
 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
    rm -f /etc/apt/apt.conf.d/docker-clean \
    && apt-get update \
-    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    && apt-get upgrade -y \
    && rm -rf /var/lib/apt/lists/*

+
 # Install dependencies with rosdep
 # This will catch anything that wasn't installed via .deb's from the frozen rosdep manifest.
-RUN apt-get update && \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    rm -f /etc/apt/apt.conf.d/docker-clean && \
+    apt-get update && \
    rosdep update && \
    rosdep install --from-paths src --ignore-src -r -y && \
    rm -rf /var/lib/apt/lists/*
@@ -38,9 +50,7 @@ RUN apt-get update && \
 # Build the workspace with symlink install
 # livox_sdk is now an ament_cmake package built by colcon (no manual /usr/local install needed).
 RUN . /opt/ros/jazzy/setup.sh && \
-    colcon build --symlink-install \
-    --cmake-args -DROS_EDITION=ROS2 -DDISTRO_ROS=jazzy
-
+    colcon build --symlink-install
 # Source the overlay on container startup
 RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \
    echo "source /ros2_ws/install/setup.bash" >> /root/.bashrc
@@ -1,3 +1,30 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 7344d08..3d531a9 100644
+--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+@@ -1,3 +1,9 @@
+# Default to the ROS2 build unless ROS1 is explicitly requested, so the else()
+# (ROS2) branch below is taken when ROS_EDITION is unset or set to "ROS2".
+if(NOT ROS_EDITION)
+  set(ROS_EDITION "ROS2")
+endif()
+
+ # judge which cmake codes to use 
+ if(ROS_EDITION STREQUAL "ROS1")
+ 
+@@ -191,6 +197,12 @@ else(ROS_EDITION STREQUAL "ROS2")
+   cmake_minimum_required(VERSION 3.14)
+   project(livox_ros_driver2)
+ 
+  # Fall back to the sourced ROS distro when -DDISTRO_ROS was not passed, so a
+  # plain `colcon build` behaves the same as the flagged Dockerfile build.
+  if(NOT DISTRO_ROS)
+    set(DISTRO_ROS "$ENV{ROS_DISTRO}")
+  endif()
+
+   # Default to C99
+   if(NOT CMAKE_C_STANDARD)
+     set(CMAKE_C_STANDARD 99)
 diff --git a/launch b/launch
 new file mode 120000
 index 0000000..163fb01
@@ -7,13 +34,47 @@ index 0000000..163fb01
 +launch_ROS2
 \ No newline at end of file
 diff --git a/package.xml b/package.xml
-new file mode 120000
-index 0000000..36804a4
+new file mode 100644
+index 0000000..0000000
 --- /dev/null
 +++ b/package.xml
-@@ -0,0 +1 @@
-+package_ROS2.xml
-\ No newline at end of file
+@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>livox_ros_driver2</name>
+  <version>1.0.0</version>
+  <description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
+  <maintainer email="dev@livoxtech.com">feng</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rcutils</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>libpcl-all-dev</depend>
+
+  <exec_depend>rosbag2</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>git</depend>
+  <depend>apr</depend>
+  <depend>livox_sdk2</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
 diff --git a/package_ROS2.xml b/package_ROS2.xml
 index 96f5762..c66785b 100644
 --- a/package_ROS2.xml
@@ -1,49 +1,24 @@
-diff --git a/CMakeLists.txt b/CMakeLists.txt
-index 05f84c5..4fb6f31 100644
--- a/CMakeLists.txt
-+++ b/CMakeLists.txt
-@@ -1,7 +1,9 @@
-cmake_minimum_required(VERSION 3.0)
-+cmake_minimum_required(VERSION 3.5)
- 
- project(livox_sdk2)
- 
-+find_package(ament_cmake REQUIRED)
+diff --git a/package.xml b/package.xml
+new file mode 100644
+index 0000000..ffdf3ce
+--- /dev/null
+++ b/package.xml
+@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>livox_sdk2</name>
+  <version>1.3.1</version>
+  <description>
+    Livox SDK2: official C++ library for Livox LiDAR sensors.
+    Provides the livox_sdk2 ament target consumed by livox_ros_driver2.
+  </description>
+  <maintainer email="sdk@livoxtech.com">Livox</maintainer>
+  <license>Apache-2.0</license>
 +
- set(CMAKE_CXX_STANDARD 11)
- 
- message(STATUS "main project dir: " ${PROJECT_SOURCE_DIR})
-@@ -18,3 +20,9 @@ endif(UNIX)
- 
- add_subdirectory(sdk_core)
- add_subdirectory(samples)
+  <buildtool_depend>ament_cmake</buildtool_depend>
 +
-+# Export the installed headers/libraries so ament packages (e.g. livox_ros_driver2)
-+# find the SDK from the colcon install space via CMAKE_PREFIX_PATH.
-+ament_export_include_directories(include)
-+ament_export_libraries(livox_lidar_sdk_shared)
-+ament_package()
-diff --git a/sdk_core/comm/define.h b/sdk_core/comm/define.h
-index 0872328..28d989b 100644
--- a/sdk_core/comm/define.h
-+++ b/sdk_core/comm/define.h
-@@ -31,6 +31,7 @@
- #include <functional>
- #include <vector>
- #include <atomic>
-+#include <cstdint>
- 
- #include "livox_lidar_def.h"
- 
-diff --git a/sdk_core/logger_handler/file_manager.h b/sdk_core/logger_handler/file_manager.h
-index 242a431..14cd76a 100644
--- a/sdk_core/logger_handler/file_manager.h
-+++ b/sdk_core/logger_handler/file_manager.h
-@@ -28,6 +28,7 @@
- #include <string>
- #include <vector>
- #include <map>
-+#include <cstdint>
- 
- namespace livox {
- namespace lidar {
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -1,3 +1,6 @@
+diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/COLCON_IGNORE b/unitree_lidar_ros/src/unitree_lidar_ros/COLCON_IGNORE
+new file mode 100644
+index 0000000..e69de29
 diff --git a/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt b/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
 index 041ef0d..dd57d47 100644
 --- a/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
@@ -37,10 +40,15 @@ index 041ef0d..dd57d47 100644
 
 install(TARGETS
 diff --git a/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml b/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
-index 74539aa..b5febb2 100644
+index 74539aa..8b186d8 100644
 --- a/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
 +++ b/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
-@@ -21,6 +21,7 @@
+@@ -17,10 +17,11 @@
+   <depend>sensor_msgs</depend>
+   
+   <depend>pcl_conversions</depend>
+-  <depend>pcl</depend>
+  <depend>libpcl-all-dev</depend>
 
   <depend>geometry_msgs</depend>
   <depend>tf2_ros</depend>
@@ -49,7 +57,7 @@ index 74539aa..b5febb2 100644
   <export>
     <build_type>ament_cmake</build_type>
 diff --git a/unitree_lidar_sdk/CMakeLists.txt b/unitree_lidar_sdk/CMakeLists.txt
-index 80af9c9..0d9a334 100644
+index 80af9c9..00725ea 100644
 --- a/unitree_lidar_sdk/CMakeLists.txt
 +++ b/unitree_lidar_sdk/CMakeLists.txt
@@ -1,39 +1,14 @@
@@ -102,4 +110,24 @@ index 80af9c9..0d9a334 100644
 -target_link_libraries(set_to_udp_mode  libunilidar_sdk2.a )
 \ No newline at end of file
 +ament_package()
-\ No newline at end of file
+diff --git a/unitree_lidar_sdk/package.xml b/unitree_lidar_sdk/package.xml
+new file mode 100644
+index 0000000..ef88d9a
+--- /dev/null
+++ b/unitree_lidar_sdk/package.xml
+@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>unitree_lidar_sdk</name>
+  <version>0.0.0</version>
+  <description>Unitree LiDAR SDK (prebuilt static library and headers)</description>
+  <maintainer email="lukelbmeng@gmail.com">jo</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,36 @@
+#!/usr/bin/env bash
+#
+# setup_upstream.sh — fetch the upstream sources agv_pro_ros2 depends on
+# and apply any required patches.
+#
+# Sources are declared in upstream.jazzy.repos and cloned under src/upstream/.
+# Patches in patches/ are applied idempotently (already-applied patches are
+# silently skipped).
+#
+# Usage:
+#   ./setup_upstream.sh
+set -euo pipefail
+
+REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+cd "$REPO_ROOT"
+
+echo "[setup_upstream] importing sources into src/upstream ..."
+mkdir -p src/upstream
+
+# Reset working-tree changes in already-cloned repos so that vcs import can
+# pull upstream updates without hitting dirty-working-tree conflicts.
+# Use -x so that gitignored files (e.g. generated package.xml) are also removed.
+# Patches are re-applied unconditionally after the import step below.
+for repo_dir in src/upstream/*/; do
+    if [[ -d "${repo_dir}/.git" ]]; then
+        git -C "${repo_dir}" checkout -- . 2>/dev/null || true
+        git -C "${repo_dir}" clean -fdx 2>/dev/null || true
+    fi
+done
+
+vcs import src/upstream < upstream.jazzy.repos
+
+echo "[setup_upstream] applying patches ..."
+bash patches/apply_patches.sh
+
+echo "[setup_upstream] done."
@@ -7,9 +7,6 @@
  <maintainer email="your-email@example.com">Your Name</maintainer>
  <license>MIT</license>

-  <!-- Build dependencies -->
-  <buildtool_depend>ament_python</buildtool_depend>
-  
  <!-- Runtime dependencies -->
  <depend>rclpy</depend>
  <depend>geometry_msgs</depend>
@@ -0,0 +1,47 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz — matches ESP32 auto-report rate
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    mecanum_drive_controller:
+      type: mecanum_drive_controller/MecanumDriveController
+
+# ─────────────────────────────────────────────────────────────────────────────
+# Mecanum drive controller
+# ─────────────────────────────────────────────────────────────────────────────
+# 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:  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
+    enable_odom_tf: true
+
+    # How long a /cmd_vel (reference) command stays valid, in seconds. The
+    # controller default is 0.0, which treats every command as instantly expired
+    # so the controller halts between messages. 0.5 s holds the last command
+    # between messages (standard cmd_vel behaviour) so the base runs at speed.
+    reference_timeout: 0.5
+
+    kinematics:
+      # wheel_radius: height of wheel centre above ground from URDF joint origin
+      # (base_footprint→base_link = 0.020 m + wheel joint z-offset ≈ 0.052 m = 0.072 m)
+      wheels_radius: 0.072
+      # sum_of_robot_center_projection_on_X_Y_axis = lx + ly
+      # lx ≈ 0.172 m (half wheelbase from URDF joint x-origins)
+      # ly ≈ 0.180 m (half track from URDF joint y-origins)
+      sum_of_robot_center_projection_on_X_Y_axis: 0.352
@@ -1,10 +1,11 @@
 import os
 from launch import LaunchDescription
 from launch.conditions import IfCondition
-from launch_ros.actions import Node,PushRosNamespace
-from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
-from launch.substitutions import Command,LaunchConfiguration,PythonExpression
+from launch_ros.actions import Node, PushRosNamespace
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.parameter_descriptions import ParameterValue
 from ament_index_python.packages import get_package_share_directory

 def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type):
@@ -35,20 +36,27 @@ def generate_launch_description():
    urdf_file = os.path.join(
        get_package_share_directory('agv_pro_description'),
        'urdf',
-        'agv_pro.urdf'
+        'agv_pro.urdf.xacro'
    )

-    robot_description_content = Command([
-        'xacro ',
-        urdf_file,
-        ' namespace:=',
-        PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
-    ])
+    # Pass both namespace and port_name into the xacro processor so that the
+    # <ros2_control> block in the URDF picks up the correct serial port.
+    robot_description_content = ParameterValue(
+        Command([
+            'xacro ',
+            urdf_file,
+            ' namespace:=',
+            PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
+            ' port_name:=',
+            port_name_arg,
+        ]),
+        value_type=str
+    )

    declare_port_name_arg = DeclareLaunchArgument(
        'port_name',
        default_value='/dev/agvpro_controller',
-        description='port name, e.g. /dev/ttyACM0'
+        description='Serial port for the AGV Pro base controller'
    )

    declare_namespace_arg = DeclareLaunchArgument(
@@ -71,22 +79,22 @@ def generate_launch_description():

    ns_action = PushRosNamespace(namespace)

-    agv_pro_node = Node(
-        package='agv_pro_base',
-        executable='agv_pro_node',
-        name='agv_pro_node',
-        output='screen',
-        parameters=[{
-            'port_name': port_name_arg,
-            'namespace': namespace,             
-        }],
-        remappings=[('cmd_vel', '/cmd_vel')]
+    ros2_controllers_yaml = os.path.join(
+        get_package_share_directory('agv_pro_bringup'),
+        'config',
+        'ros2_controllers.yaml'
    )

-    joint_state_pub = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher'
+    # controller_manager loads the hardware plugin (serial comms, wheel states)
+    # and manages the controllers.
+    controller_manager = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[
+            {'robot_description': robot_description_content},
+            ros2_controllers_yaml,
+        ],
+        output='screen',
    )

    robot_state_pub = Node(
@@ -97,9 +105,44 @@ def generate_launch_description():
        output='screen'
    )

+    # joint_state_broadcaster publishes /joint_states from the hardware interface.
+    # Replaces the old static joint_state_publisher.
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=[
+            'joint_state_broadcaster',
+            '--controller-manager', 'controller_manager',
+        ],
+        output='screen',
+    )
+
+    # mecanum_drive_controller subscribes to ~/reference (TwistStamped). Remap it
+    # to the standard /cmd_vel so nav2 and teleop work 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,
+        actions=[
+            Node(
+                package='controller_manager',
+                executable='spawner',
+                arguments=[
+                    'mecanum_drive_controller',
+                    '--controller-manager', 'controller_manager',
+                    '--controller-ros-args', '-r ~/reference:=/cmd_vel',
+                ],
+                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'),
+        include_lidar('agv_pro_bringup', 'MID360_launch.py', enable_lidar, lidar_type, 'mid360'),
        include_lidar('agv_pro_bringup', 'unitree_l2_launch.py', enable_lidar, lidar_type, 'l2'),
    ]

@@ -110,9 +153,10 @@ def generate_launch_description():
            declare_enable_lidar_arg,
            declare_lidar_type_arg,
            ns_action,
-            agv_pro_node,
-            joint_state_pub,
+            controller_manager,
            robot_state_pub,
+            joint_state_broadcaster_spawner,
+            mecanum_drive_controller_spawner,
            *lidar_launchs,
        ]
    )
@@ -9,13 +9,14 @@

  <buildtool_depend>ament_cmake</buildtool_depend>
  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>rviz2</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>mecanum_drive_controller</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>agv_pro_hardware</exec_depend>
  <exec_depend>agv_pro_description</exec_depend>
-  <exec_depend>agv_pro_base</exec_depend>
+  <exec_depend>rviz2</exec_depend>
  <exec_depend>cartographer_ros</exec_depend>
  <exec_depend>livox_ros_driver2</exec_depend>
-  <exec_depend>unitree_lidar_ros2</exec_depend>
  <exec_depend>lslidar_driver</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
@@ -20,7 +20,7 @@ def generate_launch_description():
    urdf_file = os.path.join(
        get_package_share_directory('agv_pro_description'),
        'urdf',
-        'agv_pro.urdf'
+        'agv_pro.urdf.xacro'
    )

    robot_description_content = Command([
@@ -0,0 +1,78 @@
+<?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.
+
+    controllers_file         : primary controller params (the AGV base controllers).
+    controllers_file_arm     : OPTIONAL per-function params files, each loaded into
+    controllers_file_gripper   the SAME controller_manager. Used when the AGV is
+                             composed into a larger robot (e.g. a mobile
+                             manipulator) so the additional controllers (e.g. an
+                             arm_controller / gripper_action_controller) share the
+                             single gz_ros2_control controller_manager. Named by
+                             function for clarity; empty by default -> standalone
+                             AGV unaffected.
+  -->
+  <xacro:macro name="agv_pro_gazebo" params="prefix controllers_file controllers_file_arm:='' controllers_file_gripper:=''">
+    <gazebo>
+      <plugin filename="gz_ros2_control-system"
+        name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+        <parameters>${controllers_file}</parameters>
+        <xacro:if value="${controllers_file_arm != ''}">
+          <parameters>${controllers_file_arm}</parameters>
+        </xacro:if>
+        <xacro:if value="${controllers_file_gripper != ''}">
+          <parameters>${controllers_file_gripper}</parameters>
+        </xacro:if>
+      </plugin>
+    </gazebo>
+
+    <!--
+      2D lidar sensor on laser_link. Publishes a gz LaserScan on the gz topic
+      'scan'; gazebo.launch bridges it to the ROS /scan (sensor_msgs/LaserScan)
+      that Nav2 / SLAM consume. The gz-sim-sensors-system plugin in the world
+      SDF renders it. gz_frame_id stamps the scan with the laser_link frame so
+      TF lines up. Mirrors a generic 360deg 2D planar lidar; on the real robot
+      the physical LSLiDAR/Livox/Unitree driver publishes the same /scan.
+    -->
+    <gazebo reference="${prefix}laser_link">
+      <sensor name="${prefix}lidar" type="gpu_lidar">
+        <pose>0 0 0 0 0 0</pose>
+        <topic>scan</topic>
+        <gz_frame_id>${prefix}laser_link</gz_frame_id>
+        <update_rate>10</update_rate>
+        <always_on>1</always_on>
+        <visualize>true</visualize>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>360</samples>
+              <resolution>1</resolution>
+              <min_angle>-3.141592653589793</min_angle>
+              <max_angle>3.141592653589793</max_angle>
+            </horizontal>
+          </scan>
+          <!--
+            The front-mounted 360 deg lidar physically sees the robot's own
+            chassis behind it (returns from ~0.19 m out to ~0.38 m at the rear
+            angles). Those self-returns are removed downstream by a
+            laser_filters box filter (see agv_pro_gazebo scan_filter.yaml) in the
+            base_footprint frame, which keeps full forward/side sensing while
+            dropping any point inside the chassis outline. Both simulation and
+            the physical robot run the same filter, so /scan stays identical.
+          -->
+          <range>
+            <min>0.15</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>
@@ -1,232 +0,0 @@
-<?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)"/>
-
-  <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>
-
-</robot>
@@ -0,0 +1,165 @@
+<?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)" />
+
+    <!-- Optional per-function controller params files, each loaded into the
+         SAME gz_ros2_control controller_manager. Used when composing the AGV
+         into a larger robot (e.g. a mobile manipulator) so its controllers
+         share the single controller_manager. Named by function for clarity;
+         empty -> standalone behaviour unchanged. -->
+    <xacro:arg name="controllers_file_arm" default="" />
+    <xacro:property name="controllers_file_arm" value="$(arg controllers_file_arm)" />
+
+    <xacro:arg name="controllers_file_gripper" default="" />
+    <xacro:property name="controllers_file_gripper" value="$(arg controllers_file_gripper)" />
+
+    <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}"
+            controllers_file_arm="${controllers_file_arm}"
+            controllers_file_gripper="${controllers_file_gripper}" />
+    </xacro:if>
+
+</robot>
@@ -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>
@@ -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>
@@ -0,0 +1,108 @@
+<?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>5.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>
+                    </surface>
+                </collision>
+            </gazebo>
+        </xacro:if>
+
+    </xacro:macro>
+</robot>
@@ -7,14 +7,13 @@ 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 urdf launch rviz config
+install(DIRECTORY meshes launch rviz config worlds
  DESTINATION share/${PROJECT_NAME}
 )

@@ -1,23 +0,0 @@
-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]
@@ -1,28 +1,43 @@
 controller_manager:
  ros__parameters:
-    update_rate: 100  # 控制器更新频率 (Hz)
+    update_rate: 50  # 控制器更新频率 (Hz) — matches the physical robot (ESP32 auto-report rate)
    use_sim_time: true  # 使用仿真时间

    # 定义关节状态广播器
-    fishbot_joint_state_broadcaster:
+    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
-      use_sim_time: true

-    # 定义全向驱动控制器
-    fishbot_omni_drive_controller:
-      type: omni_drive_controller/OmniDriveController
+    # 定义麦克纳姆轮驱动控制器
+    mecanum_drive_controller:
+      type: mecanum_drive_controller/MecanumDriveController

-# 四轮全向控制器配置
-fishbot_omni_drive_controller:
+# ─────────────────────────────────────────────────────────────────────────────
+# Mecanum drive controller — kept IN SYNC with the physical robot config at
+# 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_joint: front_left_wheel_joint
-    front_right_wheel_joint: front_right_wheel_joint
-    rear_left_wheel_joint: rear_left_wheel_joint
-    rear_right_wheel_joint: rear_right_wheel_joint
-    wheel_separation: 0.36  # 轮距
-    wheel_diameter: 0.1  # 轮子直径
-    publish_rate: 50.0  # 发布频率
-    odom_frame_id: odom
-    base_frame_id: base_link
-    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]
+    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
+    enable_odom_tf: true
+
+    # How long a /cmd_vel (reference) command stays valid, in seconds. The
+    # controller default is 0.0, which treats every command as instantly expired
+    # and so mostly halts between messages — the wheels only reached ~16-31% of
+    # the commanded speed and the robot crawled. 0.5 s holds the last command
+    # between messages (standard cmd_vel behaviour) so the wheels run at speed.
+    reference_timeout: 0.5
+
+    kinematics:
+      wheels_radius: 0.072  # 轮子半径 (wheel centre height above ground)
+      # 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
+
@@ -0,0 +1,30 @@
+# laser_filters chain for the AGV Pro 2D lidar.
+#
+# The 360 deg lidar is mounted at the front of the robot (laser_link is ~0.179 m
+# ahead of base_link), so its rearward beams strike the robot's own chassis and
+# report returns from ~0.19 m out to ~0.38 m. Left unfiltered, those self-returns
+# were baked into the SLAM map and both Nav2 costmaps, marking the robot's own
+# cell lethal (cost 253) so the planner refused to plan from an in-collision
+# start ("Failed to create plan with tolerance").
+#
+# The box filter drops any scan point that falls inside the chassis outline
+# (expressed in base_footprint), which removes the self-returns while keeping all
+# real obstacles in front of and beside the robot. The physical robot runs the
+# same filter, so the /scan consumed by SLAM/Nav2 is identical to simulation.
+scan_filter_chain:
+  ros__parameters:
+    filter1:
+      name: chassis_box_filter
+      type: laser_filters/LaserScanBoxFilter
+      params:
+        box_frame: base_footprint
+        # Chassis extent in base_footprint (metres). The laser sits at x=+0.179,
+        # so the body occupies roughly x in [-0.35, 0.18] and y in [-0.30, 0.30].
+        min_x: -0.35
+        max_x: 0.18
+        min_y: -0.30
+        max_y: 0.30
+        min_z: -1.0
+        max_z: 1.0
+        # false => remove points that fall INSIDE the box (the chassis).
+        invert: false
@@ -1,40 +0,0 @@
-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
-    ])
@@ -1,48 +0,0 @@
-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')
-
-    ])
@@ -1,65 +1,219 @@
 import os
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import (
+    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 Command
+from launch.substitutions import (
+    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_name = 'agv_pro_gazebo'
-    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')
-    rviz_config = os.path.join(pkg_dir, 'rviz', 'agvpro_display.rviz')

-    robot_description_content = ParameterValue(
-        Command(['xacro ', xacro_file]),
-        value_type=str
+def generate_launch_description():
+    pkg_gazebo = get_package_share_directory('agv_pro_gazebo')
+    pkg_description = get_package_share_directory('agv_pro_description')
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    # Shared robot description (agv_pro_description) built for simulation.
+    xacro_file = os.path.join(pkg_description, 'urdf', 'agv_pro.urdf.xacro')
+    controllers_file = os.path.join(pkg_gazebo, 'config', 'agv_control.yaml')
+    default_world = 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')
+    world = LaunchConfiguration('world')
+
+    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 ' + '", world, "' if '", headless,
+        "' == 'true' else '-r -v4 ' + '", world, "'",
+    ])
+    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',
+    )
+
+    # Bridge the 2D lidar: gz LaserScan -> ROS /scan_raw (sensor_msgs/LaserScan).
+    # The sensor is defined on laser_link in agv_pro.gazebo.xacro. The raw scan
+    # goes to /scan_raw and is cleaned by the box filter below before Nav2/SLAM
+    # consume /scan. use_sim_time so the scan stamps use the Gazebo clock.
+    scan_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan'],
+        parameters=[{'use_sim_time': use_sim_time}],
+        remappings=[('/scan', '/scan_raw')],
+        output='screen',
+    )
+
+    # Remove the robot's own chassis returns from the front-mounted lidar.
+    # The box filter (config/scan_filter.yaml) drops any point inside the chassis
+    # outline in base_footprint, publishing the cleaned scan on /scan. Without it
+    # the self-returns mark the robot's own cell lethal and Nav2 cannot plan.
+    scan_filter = Node(
+        package='laser_filters',
+        executable='scan_to_scan_filter_chain',
+        name='scan_filter_chain',
+        parameters=[
+            os.path.join(pkg_gazebo, 'config', 'scan_filter.yaml'),
+            {'use_sim_time': use_sim_time},
+        ],
+        remappings=[('scan', '/scan_raw'), ('scan_filtered', '/scan')],
+        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. Also remap the controller's private odometry
+    # outputs to the global names the rest of the stack expects:
+    #   ~/tf_odometry -> /tf   (so odom->base_footprint reaches the TF tree)
+    #   ~/odometry    -> /odom (nav2 odom_topic, robot_localization, etc.)
+    # NOTE: the remap keys must be the private names ('~/...'); 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 -r ~/tf_odometry:=/tf -r ~/odometry:=/odom',
+        ],
+        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([
-        # Launch Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
-            ),
-            launch_arguments={'world': world_file}.items()
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use the Gazebo simulation clock',
        ),
-
-        # Spawn robot into Gazebo
-        Node(
-            package='gazebo_ros',
-            executable='spawn_entity.py',
-            arguments=['-topic', 'robot_description',
-                       '-entity', 'agv_pro'],
-            output='screen'
+        DeclareLaunchArgument(
+            'use_rviz',
+            default_value='true',
+            description='Launch RViz2',
        ),
-
-        # State publisher
-        Node(
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            name='robot_state_publisher',
-            output='screen',
-            parameters=[robot_description]
+        DeclareLaunchArgument(
+            'headless',
+            default_value='false',
+            description='Run gz-sim without the GUI (server only)',
        ),
-
-        Node(
-            package='joint_state_publisher',
-            executable='joint_state_publisher',
-            name='joint_state_publisher',
-            output='screen',
+        DeclareLaunchArgument(
+            'world',
+            default_value=default_world,
+            description='Full path to the Gazebo world (SDF) to load',
        ),
-
-        # Optional: RViz
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz2',
-            output='screen',
-            arguments=['-d', rviz_config],
+        gz_resource_path,
+        gz_sim,
+        robot_state_publisher,
+        clock_bridge,
+        scan_bridge,
+        scan_filter,
+        delayed_spawn,
+        # Load controllers only after the robot has been spawned
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[joint_state_broadcaster_spawner],
+            )
        ),
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=joint_state_broadcaster_spawner,
+                on_exit=[mecanum_drive_controller_spawner],
+            )
+        ),
+        rviz,
    ])
@@ -1,71 +0,0 @@
-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],
-        ),
-    ]) 
@@ -2,6 +2,7 @@ import os
 from launch import LaunchDescription
 from launch_ros.actions import Node

+
 def generate_launch_description():
    return LaunchDescription([
        Node(
@@ -10,8 +11,14 @@ def generate_launch_description():
            name='teleop_keyboard',
            output='screen',
            prefix='xterm -e',  # 或 'gnome-terminal --' 替换为你的终端命令
-            remappings=[
-                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
-            ]
+            # mecanum_drive_controller expects a stamped reference; teleop must
+            # publish geometry_msgs/TwistStamped with a fresh timestamp.
+            # 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',
+            }],
        )
    ])
@@ -1,35 +0,0 @@
-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'
-        )
-    ])
@@ -12,8 +12,15 @@
  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>xacro</buildtool_depend>
  
-  <!-- Build dependencies -->
-  <depend>gazebo_ros_pkgs</depend>
+  <!-- Runtime dependencies -->
+  <!-- Shared robot description (single source of truth for sim + real) -->
+  <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>
+  <exec_depend>laser_filters</exec_depend>
+  <depend>gz_ros2_control</depend>
+
  <depend>robot_state_publisher</depend>
  <depend>joint_state_publisher</depend>
  <depend>geometry_msgs</depend>
@@ -24,9 +31,8 @@
  <depend>ros2_control</depend>
  <depend>controller_manager</depend>
  <depend>joint_state_broadcaster</depend>
-  <depend>diff_drive_controller</depend>
+  <depend>mecanum_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>
@@ -1,123 +0,0 @@
-<?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>
@@ -1,92 +0,0 @@
-<?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>
@@ -1,144 +0,0 @@
-<?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>
@@ -1,91 +0,0 @@
-<?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>
@@ -1,29 +0,0 @@
-<?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>
@@ -1,58 +0,0 @@
-<?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>
@@ -1,56 +0,0 @@
-<?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>
@@ -1,11 +1,69 @@
 <?xml version="1.0" ?>
-<sdf version="1.6">
+<sdf version="1.10">
  <world name="empty_world">
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-    <include>
-      <uri>model://sun</uri>
-    </include>
+
+    <!-- Required gz-sim (Gazebo Harmonic) system plugins -->
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+            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>
@@ -0,0 +1,133 @@
+<?xml version="1.0" ?>
+<!--
+  room.world - a simple enclosed room for testing SLAM and Nav2 in simulation.
+  Same required gz-sim system plugins as empty.world (physics, scene broadcaster,
+  user commands, sensors) plus four perimeter walls so the 2D lidar has features
+  to map and localise against.
+-->
+<sdf version="1.10">
+  <world name="room_world">
+
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+            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>
+
+    <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>
+
+    <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>
+
+    <!-- 8m x 8m room: four walls, 0.1m thick, 1m tall, centred on origin -->
+    <model name="walls">
+      <static>true</static>
+      <link name="link">
+        <!-- North wall (+X) -->
+        <collision name="north_col">
+          <pose>4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+        </collision>
+        <visual name="north_vis">
+          <pose>4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- South wall (-X) -->
+        <collision name="south_col">
+          <pose>-4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+        </collision>
+        <visual name="south_vis">
+          <pose>-4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- East wall (+Y) -->
+        <collision name="east_col">
+          <pose>0 4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+        </collision>
+        <visual name="east_vis">
+          <pose>0 4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- West wall (-Y) -->
+        <collision name="west_col">
+          <pose>0 -4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+        </collision>
+        <visual name="west_vis">
+          <pose>0 -4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- A couple of interior obstacles to make mapping/localisation non-trivial -->
+    <model name="pillar_1">
+      <static>true</static>
+      <link name="link">
+        <pose>1.5 1.0 0.5 0 0 0</pose>
+        <collision name="c"><geometry><cylinder><radius>0.25</radius><length>1.0</length></cylinder></geometry></collision>
+        <visual name="v"><geometry><cylinder><radius>0.25</radius><length>1.0</length></cylinder></geometry>
+          <material><ambient>0.7 0.5 0.3 1</ambient><diffuse>0.7 0.5 0.3 1</diffuse></material></visual>
+      </link>
+    </model>
+    <model name="pillar_2">
+      <static>true</static>
+      <link name="link">
+        <pose>-1.8 -1.5 0.5 0 0 0</pose>
+        <collision name="c"><geometry><box><size>0.5 0.5 1.0</size></box></geometry></collision>
+        <visual name="v"><geometry><box><size>0.5 0.5 1.0</size></box></geometry>
+          <material><ambient>0.3 0.5 0.7 1</ambient><diffuse>0.3 0.5 0.7 1</diffuse></material></visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>
@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.8)
+project(agv_pro_hardware)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(agv_pro_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+add_library(agv_pro_hardware SHARED
+  src/agv_pro_hardware_interface.cpp
+)
+
+target_include_directories(agv_pro_hardware PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_compile_features(agv_pro_hardware PUBLIC c_std_99 cxx_std_17)
+
+ament_target_dependencies(agv_pro_hardware
+  rclcpp
+  hardware_interface
+  pluginlib
+  sensor_msgs
+  std_msgs
+  agv_pro_msgs
+  tf2
+)
+
+target_link_libraries(agv_pro_hardware Boost::system)
+
+pluginlib_export_plugin_description_file(hardware_interface agv_pro_hardware.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS agv_pro_hardware
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  rclcpp
+  hardware_interface
+  pluginlib
+  sensor_msgs
+  std_msgs
+  agv_pro_msgs
+  tf2
+)
+
+ament_package()
@@ -0,0 +1,13 @@
+<library path="agv_pro_hardware">
+  <class
+    name="agv_pro_hardware/AgvProHardwareInterface"
+    type="agv_pro_hardware::AgvProHardwareInterface"
+    base_class_type="hardware_interface::SystemInterface">
+    <description>
+      ros2_control hardware interface for the AGV Pro mecanum-drive platform.
+      Communicates with the ESP32 base controller over serial, exposes per-wheel
+      velocity command and position/velocity state interfaces, and publishes IMU
+      and battery voltage data.
+    </description>
+  </class>
+</library>
@@ -0,0 +1,188 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <condition_variable>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <boost/asio.hpp>
+
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_component_interface_params.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "agv_pro_msgs/srv/set_digital_output.hpp"
+#include "agv_pro_msgs/srv/get_digital_input.hpp"
+#include "agv_pro_msgs/srv/set_led_color.hpp"
+#include "agv_pro_msgs/srv/set_led_mode.hpp"
+
+namespace agv_pro_hardware
+{
+
+// ── ESP32 serial protocol constants (firmware >= V1.0.8) ─────────────────────
+// Send frame: 0xFE 0xFE 0x0B <cmd_id> <8-byte payload> <CRC16-MSB> <CRC16-LSB>
+// Recv frame: 0xFE 0xFE 0x1C <28-byte payload incl cmd_id and CRC>
+static constexpr size_t SEND_FRAME_SIZE = 14;
+static constexpr size_t RECV_FRAME_SIZE = 31;
+static constexpr uint8_t RECV_PAYLOAD_LEN = RECV_FRAME_SIZE - 3;   // 28  (0x1C)
+
+static constexpr uint8_t CMD_POWER_ON      = 0x10;
+static constexpr uint8_t CMD_GET_POWER     = 0x12;
+static constexpr uint8_t CMD_AUTO_REPORT   = 0x23;
+static constexpr uint8_t CMD_VELOCITY_CMD  = 0x21;
+static constexpr uint8_t CMD_VELOCITY_RPT  = 0x25;
+static constexpr uint8_t CMD_SET_LED_COLOR = 0x34;
+static constexpr uint8_t CMD_SET_LED_MODE  = 0x3A;
+static constexpr uint8_t CMD_SET_IO_OUT    = 0x40;
+static constexpr uint8_t CMD_GET_IO_IN     = 0x41;
+
+
+class AgvProHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(AgvProHardwareInterface)
+
+  // ── ros2_control lifecycle ──────────────────────────────────────────────────
+  hardware_interface::CallbackReturn on_init(
+    const hardware_interface::HardwareComponentInterfaceParams & params) override;
+
+  hardware_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  hardware_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  hardware_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  hardware_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  // ── Interface export ────────────────────────────────────────────────────────
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr>
+  on_export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface::SharedPtr>
+  on_export_command_interfaces() override;
+
+  // ── Control loop callbacks ──────────────────────────────────────────────────
+  hardware_interface::return_type read(
+    const rclcpp::Time & time,
+    const rclcpp::Duration & period) override;
+
+  hardware_interface::return_type write(
+    const rclcpp::Time & time,
+    const rclcpp::Duration & period) override;
+
+private:
+  // ── Serial protocol helpers ─────────────────────────────────────────────────
+  uint16_t crc16_ibm(const uint8_t * data, size_t length);
+  std::vector<uint8_t> build_frame(uint8_t cmd_id, const std::vector<uint8_t> & payload);
+
+  // All send/receive helpers acquire write_mutex_ internally (or the caller must
+  // already hold it, indicated by the _locked suffix).
+  bool send_frame(const std::vector<uint8_t> & frame);
+  std::vector<uint8_t> send_and_receive(
+    const std::vector<uint8_t> & cmd_frame,
+    const std::vector<uint8_t> & expected_header,
+    size_t payload_size,
+    double timeout_sec);
+
+  bool power_on();
+  void set_auto_report(bool enable);
+  void clear_serial_buffer(int fd);
+  void disable_dtr_rts(int fd);
+
+  // ── Background serial reader ────────────────────────────────────────────────
+  void reader_thread_func();
+  void process_byte(uint8_t byte);
+  void on_complete_frame(const std::vector<uint8_t> & frame);
+
+  enum class ParseState { SEEK_FE1, SEEK_FE2, SEEK_LEN, READ_PAYLOAD };
+  ParseState parse_state_{ParseState::SEEK_FE1};
+  std::vector<uint8_t> frame_payload_;
+
+  // ── Service handlers ────────────────────────────────────────────────────────
+  void pause_reader_for_service();
+  void resume_reader_after_service();
+
+  void handle_set_digital_output(
+    const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response);
+  void handle_get_digital_input(
+    const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response);
+  void handle_set_led_color(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> response);
+  void handle_set_led_mode(
+    const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> request,
+    std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> response);
+
+  // ── Configuration (from <hardware><param> in URDF) ─────────────────────────
+  std::string port_name_;
+  double wheel_radius_;   // metres
+  double lx_;             // half wheelbase: robot centre → axle (x-axis), metres
+  double ly_;             // half track:     robot centre → wheel (y-axis), metres
+
+  // Index of each physical wheel position in the info_.joints array.
+  // Resolved in on_init() by matching joint names from hardware_parameters.
+  size_t fl_idx_{0};      // physical Front-Left
+  size_t fr_idx_{1};      // physical Front-Right
+  size_t rl_idx_{2};      // physical Rear-Left
+  size_t rr_idx_{3};      // physical Rear-Right
+
+  // ── Serial port ─────────────────────────────────────────────────────────────
+  boost::asio::io_service io_;
+  std::unique_ptr<boost::asio::serial_port> serial_port_;
+
+  // Protects all writes to the serial port (shared between write(), service
+  // handlers, and power_on/set_auto_report calls from the lifecycle methods).
+  std::mutex write_mutex_;
+
+  // ── Reader thread ────────────────────────────────────────────────────────────
+  std::thread reader_thread_;
+  std::atomic<bool> reader_running_{false};
+
+  // Pause/resume: service handlers request a pause so they can own the serial
+  // port for a request-response exchange.
+  std::atomic<bool> reader_pause_req_{false};
+  std::mutex reader_pause_mutex_;
+  std::condition_variable reader_pause_cv_;
+  bool reader_is_paused_{false};
+
+  // ── Latest data from auto-report (reader thread → read()) ───────────────────
+  std::mutex data_mutex_;
+  double latest_vx_{0.0};
+  double latest_vy_{0.0};
+  double latest_vtheta_{0.0};
+  sensor_msgs::msg::Imu latest_imu_;
+  float latest_voltage_{0.0f};
+  bool new_data_{false};
+
+  // ── Joint state storage — these doubles are the backing store for the
+  //    StateInterfaces and CommandInterfaces exported to ros2_control.
+  //    Indexed by position in info_.joints (not by FL/FR/RL/RR directly).
+  std::array<double, 4> wheel_positions_{0.0, 0.0, 0.0, 0.0};
+  std::array<double, 4> wheel_velocities_{0.0, 0.0, 0.0, 0.0};
+  std::array<double, 4> wheel_commands_{0.0, 0.0, 0.0, 0.0};
+
+  // ── ROS publishers and services (created in on_configure via get_node()) ─────
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr         imu_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr         voltage_pub_;
+  rclcpp::Service<agv_pro_msgs::srv::SetDigitalOutput>::SharedPtr set_output_srv_;
+  rclcpp::Service<agv_pro_msgs::srv::GetDigitalInput>::SharedPtr  get_input_srv_;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedColor>::SharedPtr      set_led_color_srv_;
+  rclcpp::Service<agv_pro_msgs::srv::SetLedMode>::SharedPtr       set_led_mode_srv_;
+};
+
+}  // namespace agv_pro_hardware
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>agv_pro_hardware</name>
+  <version>1.0.0</version>
+  <description>ros2_control hardware interface plugin for the AGV Pro mecanum-drive platform</description>
+  <maintainer email="todo@example.com">User</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>agv_pro_msgs</depend>
+  <depend>tf2</depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
@@ -0,0 +1,800 @@
+#include "agv_pro_hardware/agv_pro_hardware_interface.hpp"
+
+#include <poll.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <cmath>
+#include <cstring>
+#include <iomanip>
+#include <sstream>
+#include <stdexcept>
+
+#include "pluginlib/class_list_macros.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/utils.h"
+
+PLUGINLIB_EXPORT_CLASS(
+  agv_pro_hardware::AgvProHardwareInterface,
+  hardware_interface::SystemInterface)
+
+namespace agv_pro_hardware
+{
+
+// ── CRC / frame helpers ───────────────────────────────────────────────────────
+
+uint16_t AgvProHardwareInterface::crc16_ibm(const uint8_t * data, size_t length)
+{
+  uint16_t crc = 0xFFFF;
+  for (size_t i = 0; i < length; ++i) {
+    crc ^= static_cast<uint16_t>(data[i]);
+    for (int j = 0; j < 8; ++j) {
+      crc = (crc & 0x0001) ? (crc >> 1) ^ 0xA001 : (crc >> 1);
+    }
+  }
+  return crc;
+}
+
+std::vector<uint8_t> AgvProHardwareInterface::build_frame(
+  uint8_t cmd_id, const std::vector<uint8_t> & payload)
+{
+  std::vector<uint8_t> frame(SEND_FRAME_SIZE, 0x00);
+  frame[0] = 0xFE;
+  frame[1] = 0xFE;
+  frame[2] = 0x0B;
+  frame[3] = cmd_id;
+  for (size_t i = 0; i < payload.size() && i < 8; ++i) {
+    frame[4 + i] = payload[i];
+  }
+  uint16_t crc = crc16_ibm(frame.data(), 12);
+  frame[12] = (crc >> 8) & 0xFF;
+  frame[13] = crc & 0xFF;
+  return frame;
+}
+
+bool AgvProHardwareInterface::send_frame(const std::vector<uint8_t> & frame)
+{
+  std::lock_guard<std::mutex> lock(write_mutex_);
+  try {
+    boost::asio::write(*serial_port_, boost::asio::buffer(frame));
+    return true;
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(get_logger(), "Serial write error: %s", ex.what());
+    return false;
+  }
+}
+
+// Called while the reader thread is paused (service context).
+// write_mutex_ must NOT be held by the caller.
+std::vector<uint8_t> AgvProHardwareInterface::send_and_receive(
+  const std::vector<uint8_t> & cmd_frame,
+  const std::vector<uint8_t> & expected_header,
+  size_t payload_size,
+  double timeout_sec)
+{
+  {
+    std::lock_guard<std::mutex> lock(write_mutex_);
+    try {
+      boost::asio::write(*serial_port_, boost::asio::buffer(cmd_frame));
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(get_logger(), "Serial write error in service: %s", ex.what());
+      return {};
+    }
+  }
+
+  // Read response with sliding-window header search.
+  int fd = serial_port_->native_handle();
+  auto start = std::chrono::steady_clock::now();
+  auto timeout = std::chrono::duration<double>(timeout_sec);
+
+  std::vector<uint8_t> window;
+  while (std::chrono::steady_clock::now() - start < timeout) {
+    struct pollfd pfd = {fd, POLLIN, 0};
+    if (::poll(&pfd, 1, 10) <= 0) {
+      continue;
+    }
+    uint8_t byte;
+    boost::system::error_code ec;
+    if (serial_port_->read_some(boost::asio::buffer(&byte, 1), ec) == 1 && !ec) {
+      window.push_back(byte);
+      if (window.size() > expected_header.size()) {
+        window.erase(window.begin());
+      }
+      if (window == expected_header) {
+        size_t remain = payload_size + 2;  // payload + 2-byte CRC
+        std::vector<uint8_t> rest(remain);
+        boost::system::error_code ec2;
+        boost::asio::read(*serial_port_, boost::asio::buffer(rest), ec2);
+        if (ec2) {
+          RCLCPP_WARN(get_logger(), "Service response read error: %s", ec2.message().c_str());
+          return {};
+        }
+        std::vector<uint8_t> full = expected_header;
+        full.insert(full.end(), rest.begin(), rest.end());
+        return full;
+      }
+    }
+  }
+  RCLCPP_WARN(get_logger(), "Timeout waiting for service response");
+  return {};
+}
+
+// ── Serial port setup ─────────────────────────────────────────────────────────
+
+void AgvProHardwareInterface::clear_serial_buffer(int fd)
+{
+  if (tcflush(fd, TCIOFLUSH) < 0) {
+    RCLCPP_WARN(get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  }
+}
+
+void AgvProHardwareInterface::disable_dtr_rts(int fd)
+{
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(get_logger(), "Failed to clear DTR/RTS: %s", std::strerror(errno));
+    }
+  }
+}
+
+bool AgvProHardwareInterface::power_on()
+{
+  auto query = build_frame(CMD_GET_POWER, {});
+  auto resp = send_and_receive(query, {0xFE, 0xFE, 0x0B, CMD_GET_POWER}, 8, 12.0);
+
+  if (resp.size() != 14) {
+    RCLCPP_ERROR(get_logger(), "power_on: no response to GET_POWER query");
+    return false;
+  }
+  uint16_t rx_crc = (resp[12] << 8) | resp[13];
+  if (rx_crc != crc16_ibm(resp.data(), 12)) {
+    RCLCPP_ERROR(get_logger(), "power_on: CRC mismatch in GET_POWER response");
+    return false;
+  }
+
+  int state = static_cast<int8_t>(resp[4]);
+  RCLCPP_INFO(get_logger(), "GET_POWER status: %d", state);
+
+  if (state == 0) {
+    // Motors are off — send POWER_ON
+    auto on_cmd = build_frame(CMD_POWER_ON, {});
+    send_frame(on_cmd);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+    auto on_resp = send_and_receive(on_cmd, {0xFE, 0xFE, 0x0B, CMD_POWER_ON}, 8, 5.0);
+    if (on_resp.size() != 14) {
+      RCLCPP_ERROR(get_logger(), "power_on: no response to POWER_ON command");
+      return false;
+    }
+    uint16_t crc2 = (on_resp[12] << 8) | on_resp[13];
+    if (crc2 != crc16_ibm(on_resp.data(), 12)) {
+      RCLCPP_ERROR(get_logger(), "power_on: CRC mismatch in POWER_ON response");
+      return false;
+    }
+    int ps = static_cast<int8_t>(on_resp[4]);
+    if (ps == 1) {
+      RCLCPP_INFO(get_logger(), "Motors powered on successfully");
+      return true;
+    }
+    RCLCPP_ERROR(get_logger(), "power_on: firmware error code %d", ps);
+    return false;
+  }
+
+  RCLCPP_INFO(get_logger(), "Motors already on (status=%d)", state);
+  return true;
+}
+
+void AgvProHardwareInterface::set_auto_report(bool enable)
+{
+  auto frame = build_frame(CMD_AUTO_REPORT, {static_cast<uint8_t>(enable ? 1 : 0)});
+  send_frame(frame);
+}
+
+// ── Background reader thread ──────────────────────────────────────────────────
+
+void AgvProHardwareInterface::reader_thread_func()
+{
+  int fd = serial_port_->native_handle();
+
+  while (reader_running_) {
+    // ── Check for pause request (service handler needs serial) ──────────────
+    if (reader_pause_req_) {
+      std::unique_lock<std::mutex> lock(reader_pause_mutex_);
+      reader_is_paused_ = true;
+      reader_pause_cv_.notify_all();
+      reader_pause_cv_.wait(lock, [this] { return !reader_pause_req_.load(); });
+      reader_is_paused_ = false;
+      // Reset state machine: any partial frame in progress is discarded.
+      parse_state_ = ParseState::SEEK_FE1;
+      frame_payload_.clear();
+      continue;
+    }
+
+    // ── Poll with 5 ms timeout so we can check pause_req regularly ──────────
+    struct pollfd pfd = {fd, POLLIN, 0};
+    if (::poll(&pfd, 1, 5) <= 0) {
+      continue;
+    }
+
+    uint8_t byte;
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(boost::asio::buffer(&byte, 1), ec);
+    if (ec || n != 1) {
+      if (ec != boost::asio::error::would_block) {
+        RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
+          "Serial read error in reader thread: %s", ec.message().c_str());
+      }
+      continue;
+    }
+
+    process_byte(byte);
+  }
+}
+
+void AgvProHardwareInterface::process_byte(uint8_t byte)
+{
+  switch (parse_state_) {
+    case ParseState::SEEK_FE1:
+      if (byte == 0xFE) {
+        parse_state_ = ParseState::SEEK_FE2;
+      }
+      break;
+
+    case ParseState::SEEK_FE2:
+      if (byte == 0xFE) {
+        parse_state_ = ParseState::SEEK_LEN;
+      } else {
+        parse_state_ = ParseState::SEEK_FE1;
+      }
+      break;
+
+    case ParseState::SEEK_LEN:
+      if (byte == RECV_PAYLOAD_LEN) {
+        frame_payload_.clear();
+        frame_payload_.reserve(RECV_PAYLOAD_LEN);
+        parse_state_ = ParseState::READ_PAYLOAD;
+      } else if (byte == 0xFE) {
+        // Could still be a valid header sequence (two consecutive 0xFE bytes)
+        parse_state_ = ParseState::SEEK_LEN;
+      } else {
+        parse_state_ = ParseState::SEEK_FE1;
+      }
+      break;
+
+    case ParseState::READ_PAYLOAD:
+      frame_payload_.push_back(byte);
+      if (frame_payload_.size() == RECV_PAYLOAD_LEN) {
+        // Reassemble and dispatch
+        std::vector<uint8_t> frame = {0xFE, 0xFE, RECV_PAYLOAD_LEN};
+        frame.insert(frame.end(), frame_payload_.begin(), frame_payload_.end());
+        on_complete_frame(frame);
+        parse_state_ = ParseState::SEEK_FE1;
+        frame_payload_.clear();
+      }
+      break;
+  }
+}
+
+void AgvProHardwareInterface::on_complete_frame(const std::vector<uint8_t> & frame)
+{
+  if (frame.size() != RECV_FRAME_SIZE) {
+    return;
+  }
+
+  // Only handle velocity auto-report frames
+  if (frame[3] != CMD_VELOCITY_RPT) {
+    return;
+  }
+
+  // CRC check (over all bytes except the last two)
+  uint16_t rx_crc = (static_cast<uint16_t>(frame[RECV_FRAME_SIZE - 2]) << 8) |
+                     frame[RECV_FRAME_SIZE - 1];
+  if (rx_crc != crc16_ibm(frame.data(), RECV_FRAME_SIZE - 2)) {
+    RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000,
+      "CRC mismatch in auto-report frame — discarding");
+    return;
+  }
+
+  // ── Parse velocity ─────────────────────────────────────────────────────────
+  double vx     = static_cast<double>(static_cast<int8_t>(frame[4]))  * 0.01;
+  double vy     = static_cast<double>(static_cast<int8_t>(frame[5]))  * 0.01;
+  double vtheta = static_cast<double>(static_cast<int8_t>(frame[6]))  * 0.01;
+
+  float battery = static_cast<float>(frame[9]) / 10.0f;
+
+  // ── Parse IMU ──────────────────────────────────────────────────────────────
+  sensor_msgs::msg::Imu imu;
+  imu.header.stamp = get_clock()->now();
+  imu.header.frame_id = "imu_link";
+
+  auto i16 = [&frame](size_t hi) -> double {
+    return static_cast<double>(
+      static_cast<int16_t>((frame[hi] << 8) | frame[hi + 1])) * 0.01;
+  };
+
+  imu.linear_acceleration.x = i16(11);
+  imu.linear_acceleration.y = i16(13);
+  imu.linear_acceleration.z = i16(15);
+  imu.angular_velocity.x    = i16(17);
+  imu.angular_velocity.y    = i16(19);
+  imu.angular_velocity.z    = i16(21);
+
+  double yaw_deg = i16(27);
+  tf2::Quaternion q;
+  q.setRPY(0.0, 0.0, yaw_deg * M_PI / 180.0);
+  imu.orientation.x = q.x();
+  imu.orientation.y = q.y();
+  imu.orientation.z = q.z();
+  imu.orientation.w = q.w();
+
+  imu.orientation_covariance[8]      = 1e-6;
+  imu.angular_velocity_covariance[8] = 1e-6;
+
+  // ── Store latest data ──────────────────────────────────────────────────────
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    latest_vx_      = vx;
+    latest_vy_      = vy;
+    latest_vtheta_  = vtheta;
+    latest_imu_     = imu;
+    latest_voltage_ = battery;
+    new_data_       = true;
+  }
+
+  // ── Publish (rclcpp publishers are thread-safe for publish()) ─────────────
+  if (imu_pub_) {
+    imu_pub_->publish(imu);
+  }
+  if (voltage_pub_) {
+    std_msgs::msg::Float32 v;
+    v.data = battery;
+    voltage_pub_->publish(v);
+  }
+}
+
+// ── Service pause/resume ──────────────────────────────────────────────────────
+
+void AgvProHardwareInterface::pause_reader_for_service()
+{
+  reader_pause_req_ = true;
+  std::unique_lock<std::mutex> lock(reader_pause_mutex_);
+  // Wait up to 50 ms for the reader to pause (it polls every 5 ms)
+  reader_pause_cv_.wait_for(
+    lock, std::chrono::milliseconds(50),
+    [this] { return reader_is_paused_; });
+}
+
+void AgvProHardwareInterface::resume_reader_after_service()
+{
+  reader_pause_req_ = false;
+  reader_pause_cv_.notify_all();
+}
+
+// ── Service handlers ──────────────────────────────────────────────────────────
+
+void AgvProHardwareInterface::handle_set_digital_output(
+  const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> req,
+  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> res)
+{
+  if (req->pin < 1 || req->pin > 6) {
+    res->success = false;
+    res->message = "Invalid pin number (must be 1–6)";
+    return;
+  }
+  pause_reader_for_service();
+  auto resp = send_and_receive(
+    build_frame(CMD_SET_IO_OUT, {
+      static_cast<uint8_t>(req->pin),
+      static_cast<uint8_t>(req->state)}),
+    {0xFE, 0xFE, 0x0B, CMD_SET_IO_OUT}, 8, 5.0);
+  resume_reader_after_service();
+
+  if (resp.size() == 14 && resp[4] == 0x01) {
+    res->success = true;
+    res->message = "Success";
+  } else {
+    res->success = false;
+    res->message = "Hardware reported failure";
+  }
+}
+
+void AgvProHardwareInterface::handle_get_digital_input(
+  const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> req,
+  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> res)
+{
+  if (req->pin < 1 || req->pin > 6) {
+    res->success = false;
+    res->message = "Invalid pin number (must be 1–6)";
+    return;
+  }
+  pause_reader_for_service();
+  auto resp = send_and_receive(
+    build_frame(CMD_GET_IO_IN, {static_cast<uint8_t>(req->pin)}),
+    {0xFE, 0xFE, 0x0B, CMD_GET_IO_IN}, 8, 5.0);
+  resume_reader_after_service();
+
+  if (resp.size() == 14 && resp[5] != 0xFF) {
+    res->state   = static_cast<int32_t>(resp[5]);
+    res->success = true;
+    res->message = "Success";
+  } else {
+    res->success = false;
+    res->message = "Hardware reported failure";
+  }
+}
+
+void AgvProHardwareInterface::handle_set_led_color(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Request> req,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedColor::Response> res)
+{
+  if (req->position < 0 || req->position > 1 ||
+      req->brightness < 0 || req->brightness > 255 ||
+      req->r < 0 || req->r > 255 ||
+      req->g < 0 || req->g > 255 ||
+      req->b < 0 || req->b > 255)
+  {
+    res->success = false;
+    res->message = "Invalid parameter range";
+    return;
+  }
+  pause_reader_for_service();
+  auto resp = send_and_receive(
+    build_frame(CMD_SET_LED_COLOR, {
+      static_cast<uint8_t>(req->position),
+      static_cast<uint8_t>(req->brightness),
+      static_cast<uint8_t>(req->r),
+      static_cast<uint8_t>(req->g),
+      static_cast<uint8_t>(req->b)}),
+    {0xFE, 0xFE, 0x0B, CMD_SET_LED_COLOR}, 8, 5.0);
+  resume_reader_after_service();
+
+  if (resp.size() == 14 && resp[4] == 0x01) {
+    res->success = true;
+    res->message = "Success";
+  } else {
+    res->success = false;
+    res->message = "Hardware reported failure";
+  }
+}
+
+void AgvProHardwareInterface::handle_set_led_mode(
+  const std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Request> req,
+  std::shared_ptr<agv_pro_msgs::srv::SetLedMode::Response> res)
+{
+  pause_reader_for_service();
+  auto resp = send_and_receive(
+    build_frame(0x3A, {static_cast<uint8_t>(req->mode ? 1 : 0)}),
+    {0xFE, 0xFE, 0x0B, 0x3A}, 8, 5.0);
+  resume_reader_after_service();
+
+  if (resp.size() == 14 && resp[4] == 0x01) {
+    res->success = true;
+    res->message = "Success";
+  } else {
+    res->success = false;
+    res->message = "Hardware reported failure";
+  }
+}
+
+// ── Lifecycle methods ─────────────────────────────────────────────────────────
+
+hardware_interface::CallbackReturn AgvProHardwareInterface::on_init(
+  const hardware_interface::HardwareComponentInterfaceParams & params)
+{
+  if (hardware_interface::SystemInterface::on_init(params) !=
+      hardware_interface::CallbackReturn::SUCCESS)
+  {
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  if (info_.joints.size() != 4) {
+    RCLCPP_FATAL(get_logger(),
+      "Expected exactly 4 joints in <ros2_control>, got %zu", info_.joints.size());
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  // ── Parse hardware parameters ──────────────────────────────────────────────
+  auto get_param = [&](const std::string & key, const std::string & def) -> std::string {
+    auto it = info_.hardware_parameters.find(key);
+    return (it != info_.hardware_parameters.end()) ? it->second : def;
+  };
+
+  port_name_    = get_param("port_name",    "/dev/agvpro_controller");
+  wheel_radius_ = std::stod(get_param("wheel_radius", "0.072"));
+  lx_           = std::stod(get_param("lx",           "0.172"));
+  ly_           = std::stod(get_param("ly",           "0.180"));
+
+  std::string fl_name = get_param("front_left_joint",  "left_front_wheel_joint");
+  std::string fr_name = get_param("front_right_joint", "left_rear_wheel_joint");
+  std::string rl_name = get_param("rear_left_joint",   "right_front_wheel_joint");
+  std::string rr_name = get_param("rear_right_joint",  "right_rear_wheel_joint");
+
+  // ── Map joint names → array indices ───────────────────────────────────────
+  bool found_fl = false, found_fr = false, found_rl = false, found_rr = false;
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
+    const std::string & name = info_.joints[i].name;
+    if      (name == fl_name) { fl_idx_ = i; found_fl = true; }
+    else if (name == fr_name) { fr_idx_ = i; found_fr = true; }
+    else if (name == rl_name) { rl_idx_ = i; found_rl = true; }
+    else if (name == rr_name) { rr_idx_ = i; found_rr = true; }
+  }
+
+  if (!found_fl || !found_fr || !found_rl || !found_rr) {
+    RCLCPP_FATAL(get_logger(),
+      "Could not find all four wheel joints.\n"
+      "  front_left='%s' (found=%d)\n"
+      "  front_right='%s' (found=%d)\n"
+      "  rear_left='%s' (found=%d)\n"
+      "  rear_right='%s' (found=%d)",
+      fl_name.c_str(), found_fl,
+      fr_name.c_str(), found_fr,
+      rl_name.c_str(), found_rl,
+      rr_name.c_str(), found_rr);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  RCLCPP_INFO(get_logger(), "Wheel mapping: FL=%s[%zu]  FR=%s[%zu]  RL=%s[%zu]  RR=%s[%zu]",
+    fl_name.c_str(), fl_idx_,
+    fr_name.c_str(), fr_idx_,
+    rl_name.c_str(), rl_idx_,
+    rr_name.c_str(), rr_idx_);
+  RCLCPP_INFO(get_logger(), "Kinematics: radius=%.4f m  lx=%.4f m  ly=%.4f m",
+    wheel_radius_, lx_, ly_);
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn AgvProHardwareInterface::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  auto node = get_node();
+  if (!node) {
+    RCLCPP_FATAL(get_logger(), "on_configure: get_node() returned null");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  imu_pub_     = node->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
+  voltage_pub_ = node->create_publisher<std_msgs::msg::Float32>("voltage", 10);
+
+  set_output_srv_ = node->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
+    "set_digital_output",
+    std::bind(&AgvProHardwareInterface::handle_set_digital_output, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  get_input_srv_ = node->create_service<agv_pro_msgs::srv::GetDigitalInput>(
+    "get_digital_input",
+    std::bind(&AgvProHardwareInterface::handle_get_digital_input, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  set_led_color_srv_ = node->create_service<agv_pro_msgs::srv::SetLedColor>(
+    "set_led_color",
+    std::bind(&AgvProHardwareInterface::handle_set_led_color, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  set_led_mode_srv_ = node->create_service<agv_pro_msgs::srv::SetLedMode>(
+    "set_led_mode",
+    std::bind(&AgvProHardwareInterface::handle_set_led_mode, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn AgvProHardwareInterface::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // ── Open serial port ───────────────────────────────────────────────────────
+  try {
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
+    serial_port_->open(port_name_);
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(
+      boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(
+      boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(
+      boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    clear_serial_buffer(fd);
+    disable_dtr_rts(fd);
+
+    RCLCPP_INFO(get_logger(), "Serial port %s opened at 1 Mbaud", port_name_.c_str());
+  } catch (const std::exception & ex) {
+    RCLCPP_FATAL(get_logger(), "Failed to open serial port %s: %s",
+      port_name_.c_str(), ex.what());
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  // ── Allow ESP32 time to reset after DTR/RTS were cleared ──────────────────
+  RCLCPP_INFO(get_logger(), "Waiting 3 s for ESP32 to initialise …");
+  std::this_thread::sleep_for(std::chrono::seconds(3));
+
+  // ── Power on motors ────────────────────────────────────────────────────────
+  if (!power_on()) {
+    RCLCPP_FATAL(get_logger(), "Motor power-on failed");
+    serial_port_->close();
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  // ── Start auto-report stream from ESP32 ───────────────────────────────────
+  set_auto_report(true);
+
+  // ── Reset state and start reader thread ───────────────────────────────────
+  wheel_positions_.fill(0.0);
+  wheel_velocities_.fill(0.0);
+  wheel_commands_.fill(0.0);
+  parse_state_ = ParseState::SEEK_FE1;
+  frame_payload_.clear();
+  new_data_ = false;
+
+  reader_running_ = true;
+  reader_thread_  = std::thread(&AgvProHardwareInterface::reader_thread_func, this);
+
+  RCLCPP_INFO(get_logger(), "AGV Pro hardware interface activated");
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn AgvProHardwareInterface::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // ── Send zero velocity before deactivating ─────────────────────────────────
+  auto zero_frame = build_frame(CMD_VELOCITY_CMD, {0, 0, 0, 0, 0, 0, 0, 0});
+  send_frame(zero_frame);
+
+  // ── Stop reader thread ─────────────────────────────────────────────────────
+  reader_running_   = false;
+  reader_pause_req_ = false;   // in case it was paused
+  reader_pause_cv_.notify_all();
+  if (reader_thread_.joinable()) {
+    reader_thread_.join();
+  }
+
+  // ── Disable auto-report ────────────────────────────────────────────────────
+  set_auto_report(false);
+
+  RCLCPP_INFO(get_logger(), "AGV Pro hardware interface deactivated");
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn AgvProHardwareInterface::on_cleanup(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  if (serial_port_ && serial_port_->is_open()) {
+    serial_port_->close();
+  }
+  serial_port_.reset();
+  RCLCPP_INFO(get_logger(), "Serial port closed");
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+// ── Interface export ──────────────────────────────────────────────────────────
+
+std::vector<hardware_interface::StateInterface::ConstSharedPtr>
+AgvProHardwareInterface::on_export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface::ConstSharedPtr> interfaces;
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
+    interfaces.push_back(std::make_shared<const hardware_interface::StateInterface>(
+      info_.joints[i].name,
+      hardware_interface::HW_IF_POSITION,
+      &wheel_positions_[i]));
+    interfaces.push_back(std::make_shared<const hardware_interface::StateInterface>(
+      info_.joints[i].name,
+      hardware_interface::HW_IF_VELOCITY,
+      &wheel_velocities_[i]));
+  }
+  return interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface::SharedPtr>
+AgvProHardwareInterface::on_export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface::SharedPtr> interfaces;
+  for (size_t i = 0; i < info_.joints.size(); ++i) {
+    interfaces.push_back(std::make_shared<hardware_interface::CommandInterface>(
+      info_.joints[i].name,
+      hardware_interface::HW_IF_VELOCITY,
+      &wheel_commands_[i]));
+  }
+  return interfaces;
+}
+
+// ── Control loop ──────────────────────────────────────────────────────────────
+
+hardware_interface::return_type AgvProHardwareInterface::read(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & period)
+{
+  double vx, vy, vtheta;
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    if (!new_data_) {
+      return hardware_interface::return_type::OK;
+    }
+    vx      = latest_vx_;
+    vy      = latest_vy_;
+    vtheta  = latest_vtheta_;
+    new_data_ = false;
+  }
+
+  // Mecanum inverse kinematics: body frame → individual wheel angular velocities
+  //
+  //   ω_FL = (vx - vy - (lx+ly)·ωz) / r
+  //   ω_FR = (vx + vy + (lx+ly)·ωz) / r
+  //   ω_RL = (vx + vy - (lx+ly)·ωz) / r
+  //   ω_RR = (vx - vy + (lx+ly)·ωz) / r
+  //
+  // The ESP32 reports body-frame velocities (not per-wheel encoders), so these
+  // derived wheel velocities are kinematically consistent but not independently
+  // measured. Odometry quality is unchanged vs. the original driver.
+
+  const double r    = wheel_radius_;
+  const double lxy  = lx_ + ly_;
+
+  wheel_velocities_[fl_idx_] = (vx - vy - lxy * vtheta) / r;
+  wheel_velocities_[fr_idx_] = (vx + vy + lxy * vtheta) / r;
+  wheel_velocities_[rl_idx_] = (vx + vy - lxy * vtheta) / r;
+  wheel_velocities_[rr_idx_] = (vx - vy + lxy * vtheta) / r;
+
+  // Integrate positions
+  const double dt = period.seconds();
+  for (size_t i = 0; i < 4; ++i) {
+    wheel_positions_[i] += wheel_velocities_[i] * dt;
+  }
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type AgvProHardwareInterface::write(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  // Mecanum forward kinematics: wheel velocity commands → body frame
+  //
+  //   vx    = (r/4) · (ω_FL + ω_FR + ω_RL + ω_RR)
+  //   vy    = (r/4) · (-ω_FL + ω_FR + ω_RL - ω_RR)
+  //   ωz    = r/(4·(lx+ly)) · (-ω_FL + ω_FR - ω_RL + ω_RR)
+
+  const double r   = wheel_radius_;
+  const double lxy = lx_ + ly_;
+
+  const double cmd_fl = wheel_commands_[fl_idx_];
+  const double cmd_fr = wheel_commands_[fr_idx_];
+  const double cmd_rl = wheel_commands_[rl_idx_];
+  const double cmd_rr = wheel_commands_[rr_idx_];
+
+  double vx     = (r / 4.0) * (cmd_fl + cmd_fr + cmd_rl + cmd_rr);
+  double vy     = (r / 4.0) * (-cmd_fl + cmd_fr + cmd_rl - cmd_rr);
+  double vtheta = (r / (4.0 * lxy)) * (-cmd_fl + cmd_fr - cmd_rl + cmd_rr);
+
+  vx     = std::clamp(vx,     -1.5, 1.5);
+  vy     = std::clamp(vy,     -1.0, 1.0);
+  vtheta = std::clamp(vtheta, -1.0, 1.0);
+
+  const int16_t x_s   = static_cast<int16_t>(vx     * 100.0);
+  const int16_t y_s   = static_cast<int16_t>(vy     * 100.0);
+  const int16_t rot_s = static_cast<int16_t>(vtheta * 100.0);
+
+  uint8_t buf[SEND_FRAME_SIZE] = {0xFE, 0xFE, 0x0B, CMD_VELOCITY_CMD};
+  buf[4]  = (x_s   >> 8) & 0xFF;
+  buf[5]  =  x_s         & 0xFF;
+  buf[6]  = (y_s   >> 8) & 0xFF;
+  buf[7]  =  y_s         & 0xFF;
+  buf[8]  = (rot_s >> 8) & 0xFF;
+  buf[9]  =  rot_s       & 0xFF;
+  buf[10] = 0x00;
+  buf[11] = 0x00;
+  uint16_t crc = crc16_ibm(buf, 12);
+  buf[12] = (crc >> 8) & 0xFF;
+  buf[13] =  crc       & 0xFF;
+
+  send_frame(std::vector<uint8_t>(buf, buf + SEND_FRAME_SIZE));
+
+  return hardware_interface::return_type::OK;
+}
+
+}  // namespace agv_pro_hardware
@@ -24,7 +24,7 @@ if(BUILD_TESTING)
 endif()

 install(
-  DIRECTORY launch map param rviz scripts
+  DIRECTORY config launch map param rviz scripts
  DESTINATION share/${PROJECT_NAME}
 )

@@ -0,0 +1,46 @@
+# slam_toolbox online-async mapping config for the AGV Pro in simulation.
+# Builds a map from /scan and publishes the map -> odom transform that Nav2
+# needs. Frames match the robot: odom (from mecanum_drive_controller) and
+# base_footprint (robot root). use_sim_time is supplied by the launch file.
+slam_toolbox:
+  ros__parameters:
+    # Frames / topics
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    mode: mapping
+
+    # Solver
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # Mapping behaviour
+    map_update_interval: 1.0
+    resolution: 0.05
+    max_laser_range: 12.0
+    minimum_time_interval: 0.2
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true
+
+    # Scan matching
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.3
+    minimum_travel_heading: 0.3
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 12.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
@@ -0,0 +1,115 @@
+"""
+simulation.launch.py — Nav2 + SLAM in Gazebo for the AGV Pro.
+
+Brings up the full autonomous-navigation stack in simulation:
+
+  1. Gazebo (agv_pro_gazebo.launch.py) in the walled `room.world`, with the
+     2D lidar publishing /scan and the mecanum_drive_controller publishing
+     odom -> base_footprint (TF) and /odom, consuming /cmd_vel.
+  2. slam_toolbox (online async) — builds a map from /scan and publishes
+     map -> odom.
+  3. Nav2 (navigation_launch.py) — planner / controller (MPPI, Omni motion
+     model for the holonomic base) / behaviours / bt_navigator, using
+     param/nav2_sim.yaml.
+  4. RViz with the navigation view.
+
+Everything runs on the Gazebo clock (use_sim_time:=true).
+
+Usage:
+  ros2 launch agv_pro_navigation2 simulation.launch.py
+  ros2 launch agv_pro_navigation2 simulation.launch.py headless:=true
+  ros2 launch agv_pro_navigation2 simulation.launch.py use_rviz:=false
+"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    TimerAction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_gazebo = get_package_share_directory('agv_pro_gazebo')
+    pkg_nav2 = get_package_share_directory('agv_pro_navigation2')
+    pkg_slam = get_package_share_directory('slam_toolbox')
+    pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
+
+    use_rviz = LaunchConfiguration('use_rviz')
+    headless = LaunchConfiguration('headless')
+    world = LaunchConfiguration('world')
+
+    default_world = os.path.join(pkg_gazebo, 'worlds', 'room.world')
+    slam_params = os.path.join(pkg_nav2, 'config', 'slam_toolbox_sim.yaml')
+    nav2_params = os.path.join(pkg_nav2, 'param', 'nav2_sim.yaml')
+    rviz_config = os.path.join(pkg_nav2, 'rviz', 'agvpro_navigation2.rviz')
+
+    # 1. Gazebo + robot + lidar + controllers
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo, 'launch', 'agv_pro_gazebo.launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'use_rviz': 'false',
+            'headless': headless,
+            'world': world,
+        }.items(),
+    )
+
+    # 2. SLAM (map -> odom). Delayed so /scan and odom TF are up first.
+    slam = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_slam, 'launch', 'online_async_launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'slam_params_file': slam_params,
+        }.items(),
+    )
+
+    # 3. Nav2 navigation stack (no localization; SLAM provides map -> odom).
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_nav2_bringup, 'launch', 'navigation_launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'params_file': nav2_params,
+        }.items(),
+    )
+
+    # 4. RViz
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config],
+        parameters=[{'use_sim_time': True}],
+        condition=IfCondition(use_rviz),
+        output='screen',
+    )
+
+    # Give Gazebo time to spawn the robot and activate the controllers (which
+    # publish odom -> base_footprint) before SLAM and Nav2 start looking for TF.
+    delayed_bringup = TimerAction(period=8.0, actions=[slam, nav2, rviz])
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_rviz', default_value='true',
+            description='Launch RViz with the navigation view'),
+        DeclareLaunchArgument(
+            'headless', default_value='false',
+            description='Run gz-sim without the GUI (server only)'),
+        DeclareLaunchArgument(
+            'world', default_value=default_world,
+            description='Full path to the Gazebo world (SDF) to load'),
+        gazebo,
+        delayed_bringup,
+    ])
@@ -12,6 +12,10 @@
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+  <exec_depend>agv_pro_gazebo</exec_depend>
+  <exec_depend>agv_pro_description</exec_depend>
+  <exec_depend>rviz2</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
@@ -109,6 +109,9 @@ bt_navigator_rclcpp_node:
 controller_server:
  ros__parameters:
    use_sim_time: False
+    # Publish /cmd_vel as geometry_msgs/TwistStamped to match the
+    # mecanum_drive_controller's ~/reference input (both sim and real robot).
+    enable_stamped_cmd_vel: true
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
@@ -0,0 +1,482 @@
+amcl:
+  ros__parameters:
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+bt_navigator:
+  ros__parameters:
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
+
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
+
+controller_server:
+  ros__parameters:
+    # Mecanum controller consumes geometry_msgs/TwistStamped on ~/reference.
+    enable_stamped_cmd_vel: true
+    controller_frequency: 20.0
+    costmap_update_timeout: 0.30
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugins: ["progress_checker"]
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
+
+    # Progress checker parameters
+    # PoseProgressChecker (not SimpleProgressChecker) so that in-place rotation
+    # counts as progress. At the goal the holonomic base often must rotate to
+    # meet yaw_goal_tolerance; SimpleProgressChecker only credits *translation*
+    # (required_movement_radius) so that final yaw alignment tripped "Failed to
+    # make progress" -> recovery spin -> rotated further off goal -> Goal failed.
+    progress_checker:
+      plugin: "nav2_controller::PoseProgressChecker"
+      required_movement_radius: 0.5
+      required_movement_angle: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Omni"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.30
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.70
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      robot_radius: 0.30
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.7
+      always_send_full_costmap: True
+
+# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
+# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
+# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
+# map_server:
+#   ros__parameters:
+#     yaml_filename: ""
+
+map_saver:
+  ros__parameters:
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
+    GridBased:
+      plugin: "nav2_navfn_planner::NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: true
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors::Spin"
+    backup:
+      plugin: "nav2_behaviors::BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors::DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors::Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: odom
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.1
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    action_server_result_timeout: 900.0
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
+velocity_smoother:
+  ros__parameters:
+    enable_stamped_cmd_vel: true
+    smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.5, 2.0]
+    min_velocity: [-0.5, -0.5, -2.0]
+    max_accel: [2.5, 2.5, 3.2]
+    max_decel: [-2.5, -2.5, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    # Publish the final /cmd_vel as TwistStamped to match the mecanum controller.
+    enable_stamped_cmd_vel: true
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true
@@ -2,33 +2,33 @@ repositories:
    FAST_LIO:
        type: git
        url: https://github.com/hku-mars/FAST_LIO.git
-        version: ROS2
+        version: a4743b095409588842a5b30ddfa27e29d2f99164
    livox_ros_driver2:
        type: git
        url: https://github.com/Livox-SDK/livox_ros_driver2.git
-        version: master
+        version: 13eb05e4e6dd7a765b934d0c5fd6236676a57b49
    livox_sdk:
        type: git
        url: https://github.com/Livox-SDK/Livox-SDK2.git
-        version: master
+        version: 68ae1e1dc77f61f03c95d7c2809831e198d0aedd
    Lslidar_ROS2_driver:
        type: git
        url: https://github.com/Lslidar/Lslidar_ROS2_driver.git
-        version: LS-S1_V1.0
+        version: e23139673c34c51d07053837910020aaa81736fa
    pcd2pgm:
        type: git
        url: https://github.com/LihanChen2004/pcd2pgm.git
-        version: main
+        version: 885561552baae33685e59eef41a5d8c996538702
    point_lio_ros2:
        type: git
        url: https://github.com/dfloreaa/point_lio_ros2.git
-        version: main
+        version: a8e2d0d5090af97ead8dd4fac3d37cf3dbb33ff7
    slam_gmapping:
        type: git
        url: https://github.com/Project-MANAS/slam_gmapping.git
-        version: eloquent-devel
+        version: 3c3de50c071d2c64ffe516e1ed84a574fc447b97
    unilidar_sdk2:
        type: git
        url: https://github.com/unitreerobotics/unilidar_sdk2.git
-        version: main
+        version: 0e3c51f512e6b8ff60b8c32f160b412cb48445c2
Author	SHA1	Message	Date
Matt Spencer	40620300ed	Add gripper control for simulation.	2026-08-06 18:07:11 +00:00
Matt Spencer	a27c7147c3	Update reference_timeout for physical robot.	2026-08-02 16:08:35 +00:00
Matt Spencer	e50d0e2c27	Add reference timeout to get robot moving quicker, and switch to pose progress checker	2026-08-02 13:20:30 +00:00
Matt Spencer	7e1cad1536	Get nav2 running in simulation	2026-08-01 13:55:36 +00:00
Matt Spencer	44e039beac	Fix upstream versions to enable patches to always apply	2026-07-31 17:49:20 +00:00
Matt Spencer	93e9291e19	Fix upstream for rodep install in devcontainer	2026-07-31 14:54:04 +00:00
Matt Spencer	6cf51a3b67	Fix livox driver build Also add caching to the apt-get installs	2026-07-18 10:23:36 +00:00
Matt Spencer	48eb4d10fd	Fix patch application in container.	2026-07-17 14:44:44 +00:00
Matt Spencer	bd531ee9cb	Update the livox patches.	2026-07-17 14:44:44 +00:00
Matt Spencer	02e944d1b1	Get fast_lio cmake execute the git submodule init	2026-07-17 14:44:44 +00:00
Matt Spencer	e869d4d023	Patch to add symlinks to livox_ros_driver2	2026-07-17 14:44:44 +00:00
Matt Spencer	152451b134	Fix unilidar_sdk2 for jazzy colcon build	2026-07-17 14:44:44 +00:00
Matt Spencer	3d692c0083	Fix fast_lio for jazzy build	2026-07-17 14:44:44 +00:00
Matt Spencer	47274dcb36	Refactor the robot description, and include mecanum control.	2026-07-17 08:28:02 +00:00
Matt Spencer	ecdbe00d07	Staging checkin Moved to mechanum controller Use a single URDF from the robot description TODO: Wheel physics is not yet correct for mecanum movement.	2026-07-16 09:28:07 +00:00
Matt Spencer	9d62ca663f	Staging checkin for the simulation migration	2026-07-16 07:50:16 +00:00
Matt Spencer	08bf9b81bc	Add the ARG TARGETARCH to the deploy image build	2026-07-16 07:49:07 +00:00
Matt Spencer	f97df2273b	Create better caching of rosdep dependencies For both devcontainer and deployable container. Also add ability to use docker from inside the devcontainer.	2026-07-16 07:47:35 +00:00
Matthew Spencer	76701e81c0	Add initial	2026-07-15 14:10:00 +00:00