Add initial

2026-07-15 14:10:00 +00:00
20 changed files with 1319 additions and 374 deletions
@@ -2,7 +2,6 @@ FROM osrf/ros:jazzy-desktop-full
 ARG USERNAME=USERNAME
 ARG USER_UID=1000
 ARG USER_GID=$USER_UID
-ARG TARGETARCH

 # Delete user if it exists in container (e.g Ubuntu Noble: ubuntu)
 RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi
@@ -48,18 +47,13 @@ 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
-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 \
+# Optional: pre-install apt dependencies resolved from rosdep on a previous run.
+COPY .devcontainer/jazzy/ /tmp/devcontainer-jazzy/
+
+RUN apt-get update \
+    && if [ -f /tmp/devcontainer-jazzy/rosdep-apt-packages.txt ]; then \
+    grep -Ev '^\s*($|#)' /tmp/devcontainer-jazzy/rosdep-apt-packages.txt | xargs -r apt-get install -y --no-install-recommends; \
+    fi \
    && rm -rf /var/lib/apt/lists/*

 RUN apt-get update \
@@ -67,6 +61,13 @@ RUN apt-get update \
    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
    && rm -rf /var/lib/apt/lists/*

+# ROS 2 ament build tools
+# RUN apt-get install -y \
+#     ros-${ROS_DISTRO}-ament-cmake \
+#     ros-${ROS_DISTRO}-ament-cmake-auto \
+#     ros-${ROS_DISTRO}-ament-python \
+#     ros-${ROS_DISTRO}-ament-lint-auto \
+#     ros-${ROS_DISTRO}-ament-lint-common

 ENV SHELL=/bin/bash

@@ -1,9 +0,0 @@
-{
-  "features": {
-    "ghcr.io/devcontainers/features/docker-from-docker:1": {
-      "version": "1.10.0",
-      "resolved": "ghcr.io/devcontainers/features/docker-from-docker@sha256:c2c2cf829505ead8e4892c88c31b6594ae94a2bbb209e16e1fac456c1a3a624e",
-      "integrity": "sha256:c2c2cf829505ead8e4892c88c31b6594ae94a2bbb209e16e1fac456c1a3a624e"
-    }
-  }
-}
@@ -10,12 +10,6 @@
        }
    },
    "workspaceFolder": "/workspaces/agv_pro_ros2",
-    "features": {
-        "ghcr.io/devcontainers/features/docker-from-docker:1": {
-            "version": "latest",
-            "moby": true
-        }
-    },
    "customizations": {
        "vscode": {
            "extensions": [
@@ -32,15 +26,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"
    ],
@@ -2,43 +2,46 @@ FROM ros:jazzy

 # Create the workspace and copy all source directories into it
 WORKDIR /ros2_ws
-ARG TARGETARCH
-
 COPY src ./src/
 COPY patches ./patches/
 COPY upstream.jazzy.repos ./upstream.jazzy.repos

+# Stage any dual ROS1/ROS2 packages (e.g. livox_ros_driver2) for ROS2 without
+# modifying their upstream source: generate package.xml from package_ROS2.xml so
+# colcon can discover them. The ROS2 build is selected via -DROS_EDITION below.
+# RUN for d in $(find /ros2_ws/src -type f -name package_ROS2.xml -exec dirname {} \;); do \
+#       cp -f "$d/package_ROS2.xml" "$d/package.xml"; \
+#     done
+
 # Import all upstream repostories
-RUN mkdir -p src/upstream && vcs import ./src/upstream < ./upstream.jazzy.repos
+RUN vcs import ./src/upstream < ./upstream.jazzy.repos
+
+# Fixup the livox package for ros2 build
+RUN cd ./src/upstream/livox_ros_driver2 && \
+    rm -rf launch package.xml && \
+    ln -sfn launch_ROS2 launch && \
+    ln -sfn package_ROS2.xml package.xml

 # Apply patches to upstream
-RUN ./patches/apply_patches.sh
-
-# 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
-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/*
+# RUN ./patches/apply_patches.sh

 # Install dependencies with rosdep
-# This will catch anything that wasn't installed via .deb's from the frozen rosdep manifest.
 RUN apt-get update && \
    rosdep update && \
    rosdep install --from-paths src --ignore-src -r -y && \
    rm -rf /var/lib/apt/lists/*

+RUN cd src/upstream/livox_sdk && \
+    mkdir -p build && \
+    cd build && rm -rf * && \
+    cmake .. -DCMAKE_BUILD_TYPE=Release && \
+    make -j && \
+    make install
+
 # 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 \
+    --packages-ignore unitree_lidar_ros2 unitree_lidar_ros unitree_lidar_sdk fast_lio \
    --cmake-args -DROS_EDITION=ROS2 -DDISTRO_ROS=jazzy

 # Source the overlay on container startup
@@ -1,8 +1,7 @@
 #!/usr/bin/env bash
 set -u

-PATCH_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-ROOT_DIR="$(dirname "$PATCH_DIR")"
+ROOT_DIR="/workspaces/agv_pro_ros2"
 PATCH_DIR="$ROOT_DIR/patches"

 apply_patch_once() {
@@ -24,8 +23,5 @@ apply_patch_once() {
    || git -C "$target_dir" apply --reverse --check "$patch_file" >/dev/null 2>&1
 }

-apply_patch_once "$ROOT_DIR/src/upstream/FAST_LIO" "$PATCH_DIR/fast_lio.patch"
 apply_patch_once "$ROOT_DIR/src/upstream/livox_sdk" "$PATCH_DIR/livox_sdk.patch"
-apply_patch_once "$ROOT_DIR/src/upstream/livox_ros_driver2" "$PATCH_DIR/livox_ros_driver2.patch"
 apply_patch_once "$ROOT_DIR/src/upstream/slam_gmapping" "$PATCH_DIR/slam_gmapping.patch"
-apply_patch_once "$ROOT_DIR/src/upstream/unilidar_sdk2" "$PATCH_DIR/unilidar_sdk2.patch"
@@ -1,49 +0,0 @@
-diff --git a/CMakeLists.txt b/CMakeLists.txt
-index be3040b..3176847 100644
--- a/CMakeLists.txt
-+++ b/CMakeLists.txt
-@@ -5,17 +5,39 @@ if(NOT CMAKE_BUILD_TYPE)
-   set(CMAKE_BUILD_TYPE Release)
- endif()
- 
-ADD_COMPILE_OPTIONS(-std=c++14)
-ADD_COMPILE_OPTIONS(-std=c++14)
-set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
-+# Ensure the bundled ikd-Tree submodule (include/ikd-Tree/ikd_Tree.cpp) is present.
-+# Auto-initialize it when building from a git checkout; otherwise fail with guidance.
-+if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include/ikd-Tree/ikd_Tree.cpp")
-+  find_package(Git QUIET)
-+  if(GIT_FOUND AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/.git")
-+    message(STATUS "fast_lio: initializing git submodule include/ikd-Tree")
-+    execute_process(
-+      COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive
-+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-+      RESULT_VARIABLE GIT_SUBMOD_RESULT)
-+    if(NOT GIT_SUBMOD_RESULT EQUAL "0")
-+      message(FATAL_ERROR
-+        "git submodule update --init failed with ${GIT_SUBMOD_RESULT}. "
-+        "Run it manually in ${CMAKE_CURRENT_SOURCE_DIR}.")
-+    endif()
-+  endif()
-+  if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include/ikd-Tree/ikd_Tree.cpp")
-+    message(FATAL_ERROR
-+      "include/ikd-Tree/ikd_Tree.cpp is missing and could not be fetched automatically "
-+      "(no git checkout available). Run: git submodule update --init --recursive")
-+  endif()
-+endif()
-+
-+ADD_COMPILE_OPTIONS(-std=c++17)
-+set(CMAKE_CXX_FLAGS "-std=c++17 -O3")
- 
- add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
- 
- set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions")
-set(CMAKE_CXX_STANDARD 14)
-+set(CMAKE_CXX_STANDARD 17)
- set(CMAKE_CXX_STANDARD_REQUIRED ON)
- set(CMAKE_CXX_EXTENSIONS OFF)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
-+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -fexceptions")
- set(CMAKE_POSITION_INDEPENDENT_CODE ON)
- 
- message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
@@ -1,28 +0,0 @@
-diff --git a/launch b/launch
-new file mode 120000
-index 0000000..163fb01
--- /dev/null
-+++ b/launch
-@@ -0,0 +1 @@
-+launch_ROS2
-\ No newline at end of file
-diff --git a/package.xml b/package.xml
-new file mode 120000
-index 0000000..36804a4
--- /dev/null
-+++ b/package.xml
-@@ -0,0 +1 @@
-+package_ROS2.xml
-\ No newline at end of file
-diff --git a/package_ROS2.xml b/package_ROS2.xml
-index 96f5762..c66785b 100644
--- a/package_ROS2.xml
-+++ b/package_ROS2.xml
-@@ -28,6 +28,7 @@
- 
-   <depend>git</depend>
-   <depend>apr</depend>
-+  <depend>livox_sdk2</depend>
- 
-   <export>
-     <build_type>ament_cmake</build_type>
@@ -1,28 +1,3 @@
-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)
-+
- 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)
-+
-+# 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
@@ -1,105 +0,0 @@
-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
-+++ b/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
-@@ -23,16 +23,15 @@ find_package(pcl_conversions REQUIRED)
- find_package(PCL REQUIRED)
- find_package(tf2_ros REQUIRED)
- find_package(sensor_msgs REQUIRED)
-+find_package(unitree_lidar_sdk REQUIRED)
- 
- include_directories(
-   ${PCL_INCLUDE_DIRS} 
-   include
-  ../../../unitree_lidar_sdk/include
- )
- 
- link_directories(
-   ${PCL_LIBRARY_DIRS}
-  ../../../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
- )
- 
- add_definitions(${PCL_DEFINITIONS})
-@@ -42,7 +41,6 @@ add_executable(unitree_lidar_ros2_node src/unitree_lidar_ros2_node.cpp)
- target_link_libraries( unitree_lidar_ros2_node
-   ${Boost_SYSTEM_LIBRARY}
-   ${PCL_LIBRARIES}
-  libunilidar_sdk2.a
- )
- 
- ament_target_dependencies(
-@@ -52,6 +50,7 @@ ament_target_dependencies(
-   geometry_msgs 
-   tf2_ros
-   pcl_conversions
-+  unitree_lidar_sdk
- )
- 
- 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
--- a/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
-+++ b/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
-@@ -21,6 +21,7 @@
- 
-   <depend>geometry_msgs</depend>
-   <depend>tf2_ros</depend>
-+  <depend>unitree_lidar_sdk</depend>
- 
-   <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
--- a/unitree_lidar_sdk/CMakeLists.txt
-+++ b/unitree_lidar_sdk/CMakeLists.txt
-@@ -1,39 +1,14 @@
-cmake_minimum_required(VERSION 3.0)
-+cmake_minimum_required(VERSION 3.5)
- project(unitree_lidar_sdk)
- 
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++17")
-set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb")
-set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -DNDEBUG") 
-+find_package(ament_cmake REQUIRED)
- 
-include_directories(include)
-+# Install the SDK headers and the architecture-specific prebuilt static library.
-+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
-+install(FILES lib/${CMAKE_SYSTEM_PROCESSOR}/libunilidar_sdk2.a DESTINATION lib)
- 
-link_directories(lib/${CMAKE_SYSTEM_PROCESSOR})
-+# Export them so ament packages can consume the SDK via find_package().
-+ament_export_include_directories(include/${PROJECT_NAME})
-+ament_export_libraries(unilidar_sdk2)
- 
-SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib/${CMAKE_SYSTEM_PROCESSOR})
-
-add_executable(example_lidar_udp
-  examples/example_lidar_udp.cpp
-)
-target_link_libraries(example_lidar_udp  libunilidar_sdk2.a )
-
-add_executable(example_lidar_serial
-    examples/example_lidar_serial.cpp
-)
-target_link_libraries(example_lidar_serial  libunilidar_sdk2.a  )
-
-add_executable(set_ip_address
-    examples/set_ip_address.cpp
-)
-target_link_libraries(set_ip_address  libunilidar_sdk2.a )
-
-add_executable(set_to_serial_mode
-    examples/set_to_serial_mode.cpp
-)
-target_link_libraries(set_to_serial_mode  libunilidar_sdk2.a )
-
-add_executable(set_to_udp_mode
-    examples/set_to_udp_mode.cpp
-)
-target_link_libraries(set_to_udp_mode  libunilidar_sdk2.a )
-\ No newline at end of file
-+ament_package()
-\ No newline at end of file
@@ -1,73 +0,0 @@
-git
-libapr1-dev libaprutil1-dev
-libboost-all-dev
-libpcap0.8-dev
-libpcl-apps1.14 libpcl-common1.14 libpcl-features1.14 libpcl-filters1.14 libpcl-io1.14 libpcl-kdtree1.14 libpcl-keypoints1.14 libpcl-ml1.14 libpcl-octree1.14 libpcl-outofcore1.14 libpcl-people1.14 libpcl-recognition1.14 libpcl-registration1.14 libpcl-sample-consensus1.14 libpcl-search1.14 libpcl-segmentation1.14 libpcl-stereo1.14 libpcl-surface1.14 libpcl-tracking1.14 libpcl-visualization1.14
-libpcl-dev
-libqt5core5t64
-libqt5gui5t64
-libqt5opengl5t64
-libqt5widgets5t64
-python3-pytest
-qtbase5-dev
-ros-jazzy-ament-cmake
-ros-jazzy-ament-cmake-auto
-ros-jazzy-ament-cmake-black
-ros-jazzy-ament-cmake-clang-format
-ros-jazzy-ament-cmake-clang-tidy
-ros-jazzy-ament-cmake-copyright
-ros-jazzy-ament-cmake-xmllint
-ros-jazzy-ament-copyright
-ros-jazzy-ament-flake8
-ros-jazzy-ament-index-cpp
-ros-jazzy-ament-lint-auto
-ros-jazzy-ament-lint-common
-ros-jazzy-ament-pep257
-ros-jazzy-asio-cmake-module
-ros-jazzy-builtin-interfaces
-ros-jazzy-cartographer-ros
-ros-jazzy-common-interfaces
-ros-jazzy-controller-manager
-ros-jazzy-diff-drive-controller
-ros-jazzy-geometry-msgs
-ros-jazzy-io-context
-ros-jazzy-joint-state-broadcaster
-ros-jazzy-joint-state-publisher
-ros-jazzy-message-filters
-ros-jazzy-nav-msgs
-ros-jazzy-nav2-bringup
-ros-jazzy-nav2-simple-commander
-ros-jazzy-orbbec-camera
-ros-jazzy-pcl-conversions
-ros-jazzy-pcl-ros
-ros-jazzy-pluginlib
-ros-jazzy-rcl-interfaces
-ros-jazzy-rclcpp
-ros-jazzy-rclcpp-components
-ros-jazzy-rclpy
-ros-jazzy-rcutils
-ros-jazzy-resource-retriever
-ros-jazzy-robot-state-publisher
-ros-jazzy-ros2-control
-ros-jazzy-rosbag2
-ros-jazzy-rosidl-default-generators
-ros-jazzy-rosidl-default-runtime
-ros-jazzy-rtabmap-demos
-ros-jazzy-rtabmap-slam
-ros-jazzy-rtabmap-sync
-ros-jazzy-rtabmap-util
-ros-jazzy-rtabmap-viz
-ros-jazzy-rviz-common
-ros-jazzy-rviz-default-plugins
-ros-jazzy-rviz-ogre-vendor
-ros-jazzy-rviz-rendering
-ros-jazzy-rviz2
-ros-jazzy-sensor-msgs
-ros-jazzy-serial-driver
-ros-jazzy-std-msgs
-ros-jazzy-teleop-twist-keyboard
-ros-jazzy-tf2
-ros-jazzy-tf2-geometry-msgs
-ros-jazzy-tf2-ros
-ros-jazzy-visualization-msgs
-ros-jazzy-xacro
@@ -1,7 +0,0 @@
-#!/usr/bin/env bash
-set -euo pipefail
-ROS_DISTRO="${ROS_DISTRO:-jazzy}"
-rosdep update --rosdistro "$ROS_DISTRO" >/dev/null 2>&1 || true
-rosdep keys --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" 2>/dev/null \
-  | xargs rosdep resolve --rosdistro "$ROS_DISTRO" 2>/dev/null \
-  | awk '/^#apt/{a=1;next} /^#/{a=0} a' | sort -u > rosdep-packages.txt
@@ -0,0 +1,46 @@
+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
+# ─────────────────────────────────────────────────────────────────────────────
+# NOTE: The URDF joint names do not match the physical wheel positions due to a
+# naming inconsistency in agv_pro.urdf.  The mapping between the controller's
+# logical positions and the URDF joint names is as follows:
+#
+#   Physical position │ URDF joint name
+#   ──────────────────┼────────────────────────────
+#   front-left  (FL)  │ left_front_wheel_joint   ✓
+#   front-right (FR)  │ left_rear_wheel_joint     ← physically front-right
+#   rear-left   (RL)  │ right_front_wheel_joint   ← physically rear-left
+#   rear-right  (RR)  │ right_rear_wheel_joint   ✓
+#
+# The same mapping is used by the hardware interface (front_left_joint /
+# front_right_joint / rear_left_joint / rear_right_joint params in the URDF
+# <ros2_control> block).
+mecanum_drive_controller:
+  ros__parameters:
+    front_left_wheel_command_joint_name:  left_front_wheel_joint
+    front_right_wheel_command_joint_name: left_rear_wheel_joint
+    rear_left_wheel_command_joint_name:   right_front_wheel_joint
+    rear_right_wheel_command_joint_name:  right_rear_wheel_joint
+
+    odom_frame_id:  odom
+    base_frame_id:  base_footprint
+    enable_odom_tf: true
+
+    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
@@ -2,9 +2,10 @@ 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.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):
@@ -38,17 +39,24 @@ def generate_launch_description():
        'agv_pro.urdf'
    )

-    robot_description_content = Command([
+    # 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,6 +105,54 @@ 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).
+    # --controller-ros-args remaps it to /cmd_vel so nav2 and teleop work
+    # without extra flags (teleop still needs stamped:=true).
+    # 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',
+            )
+        ]
+    )
+
+    # Relay /cmd_vel (TwistStamped) → /mecanum_drive_controller/reference.
+    # This bridges the standard nav2/teleop topic to the controller's
+    # internal subscription, which ros2_control does not remap at load time.
+    # lazy:=false ensures the subscription exists before any publisher appears.
+    cmd_vel_relay = Node(
+        package='topic_tools',
+        executable='relay',
+        name='cmd_vel_relay',
+        parameters=[{
+            'input_topic':  '/cmd_vel',
+            'output_topic': '/mecanum_drive_controller/reference',
+            'lazy': False,
+        }],
+        output='screen',
+    )
+
    lidar_launchs = [
        include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
        include_lidar('agv_pro_bringup', 'MID360_launch.py', enable_lidar, lidar_type, 'mid360'),
@@ -110,9 +166,11 @@ 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,
+            cmd_vel_relay,
            *lidar_launchs,
        ]
    )
@@ -9,13 +9,15 @@

  <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>topic_tools</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>
@@ -4,6 +4,8 @@
  <xacro:arg name="namespace" default=""/>
  <xacro:property name="namespace" value="$(arg namespace)"/>

+  <xacro:arg name="port_name" default="/dev/agvpro_controller"/>
+
  <link name="${namespace}base_footprint"/>

  <joint name="${namespace}base_joint" type="fixed">
@@ -229,4 +231,49 @@
    <child link="${namespace}imu_link" />
  </joint>

+  <!-- ════════════════════════════════════════════════════════════════════════
+       ros2_control hardware interface for the real robot.
+
+       Joint-name note: the URDF joint origins show that two wheels are
+       physically at the 'wrong' position relative to their names:
+         left_rear_wheel_joint  is physically at front-right (+x, -y)
+         right_front_wheel_joint is physically at rear-left  (-x, +y)
+       The hardware plugin and mecanum_drive_controller both use the
+       front_left/front_right/rear_left/rear_right params to resolve this.
+       ════════════════════════════════════════════════════════════════════════ -->
+  <ros2_control name="AgvProHardwareInterface" type="system">
+    <hardware>
+      <plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
+      <param name="port_name">$(arg port_name)</param>
+      <param name="wheel_radius">0.072</param>
+      <param name="lx">0.172</param>
+      <param name="ly">0.180</param>
+      <!-- Physical wheel position → URDF joint name mapping -->
+      <param name="front_left_joint">${namespace}left_front_wheel_joint</param>
+      <param name="front_right_joint">${namespace}left_rear_wheel_joint</param>
+      <param name="rear_left_joint">${namespace}right_front_wheel_joint</param>
+      <param name="rear_right_joint">${namespace}right_rear_wheel_joint</param>
+    </hardware>
+    <joint name="${namespace}left_front_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="${namespace}left_rear_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="${namespace}right_front_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="${namespace}right_rear_wheel_joint">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+
 </robot>
@@ -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