From 76701e81c00331c005e462c261645a77cc46fabb Mon Sep 17 00:00:00 2001 From: Matthew Spencer Date: Wed, 15 Jul 2026 14:10:00 +0000 Subject: [PATCH] Add initial --- .devcontainer/jazzy/devcontainer.json | 6 +- .../config/ros2_controllers.yaml | 46 + .../launch/agv_pro_bringup.launch.py | 114 ++- .../agv_pro_bringup/package.xml | 10 +- .../agv_pro_description/urdf/agv_pro.urdf | 47 + .../agv_pro_hardware/CMakeLists.txt | 67 ++ .../agv_pro_hardware/agv_pro_hardware.xml | 13 + .../agv_pro_hardware_interface.hpp | 188 ++++ .../agv_pro_hardware/package.xml | 24 + .../src/agv_pro_hardware_interface.cpp | 800 ++++++++++++++++++ 10 files changed, 1281 insertions(+), 34 deletions(-) create mode 100644 src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml create mode 100644 src/elephant_robotics/agv_pro_hardware/CMakeLists.txt create mode 100644 src/elephant_robotics/agv_pro_hardware/agv_pro_hardware.xml create mode 100644 src/elephant_robotics/agv_pro_hardware/include/agv_pro_hardware/agv_pro_hardware_interface.hpp create mode 100644 src/elephant_robotics/agv_pro_hardware/package.xml create mode 100644 src/elephant_robotics/agv_pro_hardware/src/agv_pro_hardware_interface.cpp diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index 2f9a680..e99b210 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -26,15 +26,17 @@ "CYCLONEDDS_URI": "/workspaces/agv_pro_ros2/.devcontainer/cyclonedds.xml" }, "runArgs": [ - "--network=robot-sim", + // "--network=robot-sim", + "--network=host", "--pid=host", "--ipc=host", + "--group-add=dialout", "-e", "DISPLAY=${env:DISPLAY}" ], "mounts": [ + "source=/dev,target=/dev,type=bind", "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", - "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached", "source=/usr/local/share/ca-certificates,target=/usr/local/share/host-certificates,type=bind,consistency=cached", "source=${localEnv:HOME}/.ssh,target=/home/${localEnv:USER}/.ssh,type=bind,consistency=cached" ], diff --git a/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml b/src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml new file mode 100644 index 0000000..721baa0 --- /dev/null +++ b/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 +# 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 diff --git a/src/elephant_robotics/agv_pro_bringup/launch/agv_pro_bringup.launch.py b/src/elephant_robotics/agv_pro_bringup/launch/agv_pro_bringup.launch.py index b4f246d..4ba335d 100644 --- a/src/elephant_robotics/agv_pro_bringup/launch/agv_pro_bringup.launch.py +++ b/src/elephant_robotics/agv_pro_bringup/launch/agv_pro_bringup.launch.py @@ -1,10 +1,11 @@ import os from launch import LaunchDescription from launch.conditions import IfCondition -from launch_ros.actions import Node,PushRosNamespace -from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription -from launch.substitutions import Command,LaunchConfiguration,PythonExpression +from launch_ros.actions import Node, PushRosNamespace +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.parameter_descriptions import ParameterValue from ament_index_python.packages import get_package_share_directory def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type): @@ -38,17 +39,24 @@ def generate_launch_description(): 'agv_pro.urdf' ) - robot_description_content = Command([ - 'xacro ', - urdf_file, - ' namespace:=', - PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']), - ]) + # Pass both namespace and port_name into the xacro processor so that the + # 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', + 'port_name', default_value='/dev/agvpro_controller', - description='port name, e.g. /dev/ttyACM0' + description='Serial port for the AGV Pro base controller' ) declare_namespace_arg = DeclareLaunchArgument( @@ -71,22 +79,22 @@ def generate_launch_description(): ns_action = PushRosNamespace(namespace) - agv_pro_node = Node( - package='agv_pro_base', - executable='agv_pro_node', - name='agv_pro_node', - output='screen', - parameters=[{ - 'port_name': port_name_arg, - 'namespace': namespace, - }], - remappings=[('cmd_vel', '/cmd_vel')] + ros2_controllers_yaml = os.path.join( + get_package_share_directory('agv_pro_bringup'), + 'config', + 'ros2_controllers.yaml' ) - joint_state_pub = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher' + # controller_manager loads the hardware plugin (serial comms, wheel states) + # and manages the controllers. + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ + {'robot_description': robot_description_content}, + ros2_controllers_yaml, + ], + output='screen', ) robot_state_pub = Node( @@ -97,9 +105,57 @@ 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'), ] @@ -110,9 +166,11 @@ def generate_launch_description(): declare_enable_lidar_arg, declare_lidar_type_arg, ns_action, - agv_pro_node, - joint_state_pub, + controller_manager, robot_state_pub, + joint_state_broadcaster_spawner, + mecanum_drive_controller_spawner, + cmd_vel_relay, *lidar_launchs, ] ) \ No newline at end of file diff --git a/src/elephant_robotics/agv_pro_bringup/package.xml b/src/elephant_robotics/agv_pro_bringup/package.xml index 88f4291..31f0492 100644 --- a/src/elephant_robotics/agv_pro_bringup/package.xml +++ b/src/elephant_robotics/agv_pro_bringup/package.xml @@ -9,13 +9,15 @@ ament_cmake robot_state_publisher - joint_state_publisher - rviz2 + controller_manager + mecanum_drive_controller + joint_state_broadcaster + topic_tools + agv_pro_hardware agv_pro_description - agv_pro_base + rviz2 cartographer_ros livox_ros_driver2 - unitree_lidar_ros2 lslidar_driver ament_lint_auto diff --git a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf index 9824f10..67a9359 100755 --- a/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf +++ b/src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf @@ -4,6 +4,8 @@ + + @@ -229,4 +231,49 @@ + + + + agv_pro_hardware/AgvProHardwareInterface + $(arg port_name) + 0.072 + 0.172 + 0.180 + + ${namespace}left_front_wheel_joint + ${namespace}left_rear_wheel_joint + ${namespace}right_front_wheel_joint + ${namespace}right_rear_wheel_joint + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/elephant_robotics/agv_pro_hardware/CMakeLists.txt b/src/elephant_robotics/agv_pro_hardware/CMakeLists.txt new file mode 100644 index 0000000..eff66a4 --- /dev/null +++ b/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 + $ + $ +) + +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() diff --git a/src/elephant_robotics/agv_pro_hardware/agv_pro_hardware.xml b/src/elephant_robotics/agv_pro_hardware/agv_pro_hardware.xml new file mode 100644 index 0000000..6e7fdad --- /dev/null +++ b/src/elephant_robotics/agv_pro_hardware/agv_pro_hardware.xml @@ -0,0 +1,13 @@ + + + + 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. + + + diff --git a/src/elephant_robotics/agv_pro_hardware/include/agv_pro_hardware/agv_pro_hardware_interface.hpp b/src/elephant_robotics/agv_pro_hardware/include/agv_pro_hardware/agv_pro_hardware_interface.hpp new file mode 100644 index 0000000..5eea835 --- /dev/null +++ b/src/elephant_robotics/agv_pro_hardware/include/agv_pro_hardware/agv_pro_hardware_interface.hpp @@ -0,0 +1,188 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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 <8-byte payload> +// 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 + on_export_state_interfaces() override; + + std::vector + 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 build_frame(uint8_t cmd_id, const std::vector & 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 & frame); + std::vector send_and_receive( + const std::vector & cmd_frame, + const std::vector & 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 & frame); + + enum class ParseState { SEEK_FE1, SEEK_FE2, SEEK_LEN, READ_PAYLOAD }; + ParseState parse_state_{ParseState::SEEK_FE1}; + std::vector frame_payload_; + + // ── Service handlers ──────────────────────────────────────────────────────── + void pause_reader_for_service(); + void resume_reader_after_service(); + + void handle_set_digital_output( + const std::shared_ptr request, + std::shared_ptr response); + void handle_get_digital_input( + const std::shared_ptr request, + std::shared_ptr response); + void handle_set_led_color( + const std::shared_ptr request, + std::shared_ptr response); + void handle_set_led_mode( + const std::shared_ptr request, + std::shared_ptr response); + + // ── Configuration (from 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 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 reader_running_{false}; + + // Pause/resume: service handlers request a pause so they can own the serial + // port for a request-response exchange. + std::atomic 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 wheel_positions_{0.0, 0.0, 0.0, 0.0}; + std::array wheel_velocities_{0.0, 0.0, 0.0, 0.0}; + std::array wheel_commands_{0.0, 0.0, 0.0, 0.0}; + + // ── ROS publishers and services (created in on_configure via get_node()) ───── + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr voltage_pub_; + rclcpp::Service::SharedPtr set_output_srv_; + rclcpp::Service::SharedPtr get_input_srv_; + rclcpp::Service::SharedPtr set_led_color_srv_; + rclcpp::Service::SharedPtr set_led_mode_srv_; +}; + +} // namespace agv_pro_hardware diff --git a/src/elephant_robotics/agv_pro_hardware/package.xml b/src/elephant_robotics/agv_pro_hardware/package.xml new file mode 100644 index 0000000..d2395f9 --- /dev/null +++ b/src/elephant_robotics/agv_pro_hardware/package.xml @@ -0,0 +1,24 @@ + + + + agv_pro_hardware + 1.0.0 + ros2_control hardware interface plugin for the AGV Pro mecanum-drive platform + User + Apache-2.0 + + ament_cmake + + rclcpp + hardware_interface + pluginlib + sensor_msgs + std_msgs + agv_pro_msgs + tf2 + + + + ament_cmake + + diff --git a/src/elephant_robotics/agv_pro_hardware/src/agv_pro_hardware_interface.cpp b/src/elephant_robotics/agv_pro_hardware/src/agv_pro_hardware_interface.cpp new file mode 100644 index 0000000..5aa8dca --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +#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(data[i]); + for (int j = 0; j < 8; ++j) { + crc = (crc & 0x0001) ? (crc >> 1) ^ 0xA001 : (crc >> 1); + } + } + return crc; +} + +std::vector AgvProHardwareInterface::build_frame( + uint8_t cmd_id, const std::vector & payload) +{ + std::vector 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 & frame) +{ + std::lock_guard 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 AgvProHardwareInterface::send_and_receive( + const std::vector & cmd_frame, + const std::vector & expected_header, + size_t payload_size, + double timeout_sec) +{ + { + std::lock_guard 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(timeout_sec); + + std::vector 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 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 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(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(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(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 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 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 & 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(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(static_cast(frame[4])) * 0.01; + double vy = static_cast(static_cast(frame[5])) * 0.01; + double vtheta = static_cast(static_cast(frame[6])) * 0.01; + + float battery = static_cast(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( + static_cast((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 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 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 req, + std::shared_ptr 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(req->pin), + static_cast(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 req, + std::shared_ptr 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(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(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 req, + std::shared_ptr 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(req->position), + static_cast(req->brightness), + static_cast(req->r), + static_cast(req->g), + static_cast(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 req, + std::shared_ptr res) +{ + pause_reader_for_service(); + auto resp = send_and_receive( + build_frame(0x3A, {static_cast(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 , 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("imu", 20); + voltage_pub_ = node->create_publisher("voltage", 10); + + set_output_srv_ = node->create_service( + "set_digital_output", + std::bind(&AgvProHardwareInterface::handle_set_digital_output, this, + std::placeholders::_1, std::placeholders::_2)); + + get_input_srv_ = node->create_service( + "get_digital_input", + std::bind(&AgvProHardwareInterface::handle_get_digital_input, this, + std::placeholders::_1, std::placeholders::_2)); + + set_led_color_srv_ = node->create_service( + "set_led_color", + std::bind(&AgvProHardwareInterface::handle_set_led_color, this, + std::placeholders::_1, std::placeholders::_2)); + + set_led_mode_srv_ = node->create_service( + "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(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 +AgvProHardwareInterface::on_export_state_interfaces() +{ + std::vector interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) { + interfaces.push_back(std::make_shared( + info_.joints[i].name, + hardware_interface::HW_IF_POSITION, + &wheel_positions_[i])); + interfaces.push_back(std::make_shared( + info_.joints[i].name, + hardware_interface::HW_IF_VELOCITY, + &wheel_velocities_[i])); + } + return interfaces; +} + +std::vector +AgvProHardwareInterface::on_export_command_interfaces() +{ + std::vector interfaces; + for (size_t i = 0; i < info_.joints.size(); ++i) { + interfaces.push_back(std::make_shared( + 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 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(vx * 100.0); + const int16_t y_s = static_cast(vy * 100.0); + const int16_t rot_s = static_cast(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(buf, buf + SEND_FRAME_SIZE)); + + return hardware_interface::return_type::OK; +} + +} // namespace agv_pro_hardware