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,17 +32,15 @@
         "CYCLONEDDS_URI": "/workspaces/agv_pro_ros2/.devcontainer/cyclonedds.xml"
     },
     "runArgs": [
-        // "--network=robot-sim",
-        "--network=host",
+        "--network=robot-sim",
         "--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

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

@@ -1,11 +1,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, TimerAction
-from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node,PushRosNamespace
+from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
+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):
@@ -39,24 +38,17 @@ def generate_launch_description():
         'agv_pro.urdf'
     )
 
-    # 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
-    )
+    robot_description_content = Command([
+        'xacro ',
+        urdf_file,
+        ' namespace:=',
+        PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
+    ])
 
     declare_port_name_arg = DeclareLaunchArgument(
         'port_name', 
         default_value='/dev/agvpro_controller',
-        description='Serial port for the AGV Pro base controller'
+        description='port name, e.g. /dev/ttyACM0'
     )
 
     declare_namespace_arg = DeclareLaunchArgument(
@@ -79,22 +71,22 @@ def generate_launch_description():
 
     ns_action = PushRosNamespace(namespace)
 
-    ros2_controllers_yaml = os.path.join(
-        get_package_share_directory('agv_pro_bringup'),
-        'config',
-        'ros2_controllers.yaml'
+    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')]
     )
 
-    # 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',
+    joint_state_pub = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher'
     )
 
     robot_state_pub = Node(
@@ -105,57 +97,9 @@ 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'),
+        include_lidar('agv_pro_bringup', 'MID360_launch.py',enable_lidar, lidar_type, 'mid360'),
         include_lidar('agv_pro_bringup', 'unitree_l2_launch.py', enable_lidar, lidar_type, 'l2'),
     ]
 
@@ -166,11 +110,9 @@ def generate_launch_description():
             declare_enable_lidar_arg,
             declare_lidar_type_arg,
             ns_action,
-            controller_manager,
+            agv_pro_node,
+            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,15 +9,13 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <exec_depend>robot_state_publisher</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>joint_state_publisher</exec_depend>
   <exec_depend>rviz2</exec_depend>
+  <exec_depend>agv_pro_description</exec_depend>
+  <exec_depend>agv_pro_base</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,8 +4,6 @@
   <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">
@@ -231,49 +229,4 @@
     <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

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

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

@@ -1,188 +0,0 @@
-#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

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

@@ -1,800 +0,0 @@
-#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