Fix patch application in container.

For both  devcontainer and deployable container.
Also add ability to use docker from inside the devcontainer.

.devcontainer/jazzy/devcontainer.json

@@ -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"
     ],

src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml

@@ -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

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

@@ -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(
+    ros2_controllers_yaml = os.path.join(
-        package='agv_pro_base',
+        get_package_share_directory('agv_pro_bringup'),
-        executable='agv_pro_node',
+        'config',
-        name='agv_pro_node',
+        'ros2_controllers.yaml'
-        output='screen',
-        parameters=[{
-            'port_name': port_name_arg,
-            'namespace': namespace,             
-        }],
-        remappings=[('cmd_vel', '/cmd_vel')]
     )
 
-    joint_state_pub = Node(
+    # controller_manager loads the hardware plugin (serial comms, wheel states)
-        package='joint_state_publisher',
+    # and manages the controllers.
-        executable='joint_state_publisher',
+    controller_manager = Node(
-        name='joint_state_publisher'
+        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,
+            controller_manager,
-            joint_state_pub,
             robot_state_pub,
+            joint_state_broadcaster_spawner,
+            mecanum_drive_controller_spawner,
+            cmd_vel_relay,
             *lidar_launchs,
         ]
     )

src/elephant_robotics/agv_pro_bringup/package.xml

@@ -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>controller_manager</exec_depend>
-  <exec_depend>rviz2</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>

src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf

@@ -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>

src/elephant_robotics/agv_pro_hardware/CMakeLists.txt

@@ -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()

src/elephant_robotics/agv_pro_hardware/agv_pro_hardware.xml

@@ -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>

src/elephant_robotics/agv_pro_hardware/include/agv_pro_hardware/agv_pro_hardware_interface.hpp

@@ -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

src/elephant_robotics/agv_pro_hardware/package.xml

@@ -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>

src/elephant_robotics/agv_pro_hardware/src/agv_pro_hardware_interface.cpp

@@ -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