Skip to content

Latest commit

 

History

History
671 lines (534 loc) · 23.6 KB

File metadata and controls

671 lines (534 loc) · 23.6 KB

ROS2 Hardware Interface — Code Conventions

This document provides the AI agent with coding conventions and templates for the robot_hardware_interface package.

1. Directory Structure

robot_hardware_interface/
├── include/robot_hardware_interface/
│   ├── common/                          # Shared components (all robots)
│   │   ├── data_buffer.hpp              # Zero-allocation double buffer
│   │   ├── types.hpp                    # MotorData, ImuData, ImuState
│   │   ├── motor_types.hpp              # MotorCommand<N,T>, MotorState<N,T>
│   │   └── monitor.hpp                  # MonitorThresholds
│   ├── {vendor}/{robot}.hpp             # Robot-specific header
├── src/{vendor}/{robot}.cpp             # Implementation
├── CMakeLists.txt
├── package.xml
└── robot_hardware_interface.xml         # Plugin descriptor

2. Naming Conventions

Item Format Example
Namespace {vendor} unitree, agibot, engineai
Class name {Robot}HardwareInterface G1HardwareInterface
Plugin name {vendor}/{Robot}HardwareInterface unitree/G1HardwareInterface
Node name {robot}_hardware_interface g1_hardware_interface

3. Shared Components (common/)

All robots share the following components to avoid code duplication.

3.1 DataBuffer (data_buffer.hpp)

#include "robot_hardware_interface/common/data_buffer.hpp"

// Usage:
robot_hardware_interface::DataBuffer<MyState> state_buffer_;
state_buffer_.setData(new_state);                    // producer thread
const MyState* state = state_buffer_.getData();      // consumer thread

3.2 Type Definitions (types.hpp)

#include "robot_hardware_interface/common/types.hpp"

// Provides:
// - CallbackReturn, return_type  (type aliases)
// - MotorData    (ros2_control data storage per joint)
// - ImuData      (ros2_control IMU interface data)
// - ImuState     (generic intermediate IMU structure)

3.3 Motor Type Templates (motor_types.hpp)

#include "robot_hardware_interface/common/motor_types.hpp"

// Templated motor command / state structures
template <size_t N, typename T = float>
struct MotorCommand { ... };

template <size_t N, typename T = float>
struct MotorState { ... };

// Examples:
using G1MotorCommand  = robot_hardware_interface::MotorCommand<29, float>;
using PM01MotorState  = robot_hardware_interface::MotorState<24, double>;

3.4 Monitor Thresholds (monitor.hpp)

#include "robot_hardware_interface/common/monitor.hpp"

// Provides MonitorThresholds constants:
// - READ_WARN_US, WRITE_WARN_US, STATE_HANDLER_WARN_US, etc.

4. Header Template (.hpp)

#pragma once

#include <atomic>
#include <map>
#include <memory>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>

// Shared components
#include "robot_hardware_interface/common/data_buffer.hpp"
#include "robot_hardware_interface/common/monitor.hpp"
#include "robot_hardware_interface/common/motor_types.hpp"
#include "robot_hardware_interface/common/types.hpp"

// SDK-specific includes...

namespace {vendor} {

using namespace robot_hardware_interface;

// ========== Constants ==========
constexpr size_t {ROBOT}_NUM_MOTOR = N;

// ========== Type Aliases ==========
using {Robot}MotorCommand = MotorCommand<{ROBOT}_NUM_MOTOR, float>;
using {Robot}MotorState   = MotorState<{ROBOT}_NUM_MOTOR, float>;

// Define a robot-specific ImuState if the SDK uses float IMU data
struct {Robot}ImuState {
  std::array<float, 4> quat  = {};
  std::array<float, 3> omega = {};
  std::array<float, 3> acc   = {};
};

// ========== URDF Joint Name -> SDK Index Map ==========
static const std::map<std::string, size_t> jointIndexMap = {
    {"left_hip_pitch_joint", 0},
    {"left_hip_roll_joint",  1},
    // ... fill in according to the robot's URDF
};

// Note: CallbackReturn, return_type, MotorData, ImuData, and MonitorThresholds
// are already provided by the common/ headers — do not redefine them here.

// ========== Main Class ==========
class {Robot}HardwareInterface : public hardware_interface::SystemInterface {
 public:
  RCLCPP_SHARED_PTR_DEFINITIONS({Robot}HardwareInterface)

  // Required overrides
  CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
  std::vector<hardware_interface::StateInterface>   export_state_interfaces()   override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
  return_type read (const rclcpp::Time& time, const rclcpp::Duration& period) override;
  return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;

  // Required when using ROS2 Topics (start/stop the executor spin thread)
  CallbackReturn on_activate  (const rclcpp_lifecycle::State& previous_state) override;
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;

 private:
  static int getJointIndex(const std::string& name);
  // SDK callback / send function declarations...

  // Thread-safe state/command exchange
  DataBuffer<{Robot}MotorState>   motor_state_buffer_;
  DataBuffer<{Robot}MotorCommand> motor_command_buffer_;
  DataBuffer<{Robot}ImuState>     imu_state_buffer_;

  std::array<MotorData, {ROBOT}_NUM_MOTOR> jointData_;
  ImuData imuData_{};

  // ========== Pre-allocated message objects (avoid heap allocation in hot paths) ==========
  {Robot}MotorState   motor_state_cache_;   // reused state parsing buffer
  {Robot}ImuState     imu_state_cache_;     // reused IMU parsing buffer
  {Robot}MotorCommand motor_cmd_cache_;     // reused command buffer
  // SDK message objects (e.g. LowCmd_) should also be pre-allocated here...

  // Safety: track whether the first state frame has been received
  std::atomic<bool> first_state_received_{false};
  std::atomic<bool> command_initialized_{false};

  // Performance monitoring
  bool monitor_enabled_{false};
  std::atomic<uint64_t> cmd_writer_last_time_us_{0};  // used for jitter detection

  // Configurable parameters (read from URDF)
  uint64_t cmd_writer_period_us_{2000};              // command send period (default 500 Hz)
  uint32_t state_decimation_{1};                     // state decimation factor (default: no decimation)
  std::atomic<uint32_t> state_frame_counter_{0};     // decimation frame counter

  rclcpp::Node::SharedPtr node_;
  // SDK communication objects...
};

}  // namespace {vendor}

5. Source Template (.cpp)

#include "robot_hardware_interface/{vendor}/{robot}.hpp"

#include <pluginlib/class_list_macros.hpp>

namespace {vendor} {

// Interface name constants
constexpr char HW_IF_STIFFNESS[] = "stiffness";
constexpr char HW_IF_DAMPING[]   = "damping";
constexpr char HW_IF_POSITION[]  = "position";
constexpr char HW_IF_VELOCITY[]  = "velocity";
constexpr char HW_IF_EFFORT[]    = "effort";

int {Robot}HardwareInterface::getJointIndex(const std::string& name) {
  auto it = jointIndexMap.find(name);
  if (it != jointIndexMap.end()) {
    return it->second;
  }
  return -1;
}

CallbackReturn {Robot}HardwareInterface::on_init(const hardware_interface::HardwareInfo& info) {
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
    return CallbackReturn::ERROR;
  }

  node_ = std::make_shared<rclcpp::Node>("{robot}_hardware_interface");

  // Zero-initialize jointData_
  for (int i = 0; i < {ROBOT}_NUM_MOTOR; ++i) {
    jointData_[i] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  }

  // Read parameters
  std::string param_value = "default";
  if (info.hardware_parameters.find("param_name") != info.hardware_parameters.end()) {
    param_value = info.hardware_parameters.at("param_name");
    RCLCPP_INFO(node_->get_logger(), "Using param: %s", param_value.c_str());
  }

  // Initialize SDK / create publishers and subscribers...

  // Performance monitoring (enabled by default)
  monitor_enabled_ = true;
  if (info.hardware_parameters.find("enable_monitor") != info.hardware_parameters.end()) {
    monitor_enabled_ = (info.hardware_parameters.at("enable_monitor") != "false");
  }

  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> {Robot}HardwareInterface::export_state_interfaces() {
  std::vector<hardware_interface::StateInterface> state_interfaces;

  // IMU interfaces
  state_interfaces.emplace_back("base_imu", "orientation.x",         &imuData_.ori_[0]);
  state_interfaces.emplace_back("base_imu", "orientation.y",         &imuData_.ori_[1]);
  state_interfaces.emplace_back("base_imu", "orientation.z",         &imuData_.ori_[2]);
  state_interfaces.emplace_back("base_imu", "orientation.w",         &imuData_.ori_[3]);
  state_interfaces.emplace_back("base_imu", "angular_velocity.x",    &imuData_.angularVel_[0]);
  state_interfaces.emplace_back("base_imu", "angular_velocity.y",    &imuData_.angularVel_[1]);
  state_interfaces.emplace_back("base_imu", "angular_velocity.z",    &imuData_.angularVel_[2]);
  state_interfaces.emplace_back("base_imu", "linear_acceleration.x", &imuData_.linearAcc_[0]);
  state_interfaces.emplace_back("base_imu", "linear_acceleration.y", &imuData_.linearAcc_[1]);
  state_interfaces.emplace_back("base_imu", "linear_acceleration.z", &imuData_.linearAcc_[2]);

  // Joint interfaces
  for (const auto& joint : info_.joints) {
    int index = getJointIndex(joint.name);
    if (index >= 0) {
      state_interfaces.emplace_back(joint.name, HW_IF_POSITION, &jointData_[index].pos_);
      state_interfaces.emplace_back(joint.name, HW_IF_VELOCITY, &jointData_[index].vel_);
      state_interfaces.emplace_back(joint.name, HW_IF_EFFORT,   &jointData_[index].tau_);
    }
  }
  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> {Robot}HardwareInterface::export_command_interfaces() {
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (const auto& joint : info_.joints) {
    int index = getJointIndex(joint.name);
    if (index >= 0) {
      command_interfaces.emplace_back(joint.name, HW_IF_POSITION,  &jointData_[index].posDes_);
      command_interfaces.emplace_back(joint.name, HW_IF_VELOCITY,  &jointData_[index].velDes_);
      command_interfaces.emplace_back(joint.name, HW_IF_STIFFNESS, &jointData_[index].kp_);
      command_interfaces.emplace_back(joint.name, HW_IF_DAMPING,   &jointData_[index].kd_);
      command_interfaces.emplace_back(joint.name, HW_IF_EFFORT,    &jointData_[index].ff_);
    }
  }
  return command_interfaces;
}

hardware_interface::return_type {Robot}HardwareInterface::read(
    const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
  auto start_time = std::chrono::steady_clock::now();

  // Pull the latest motor state from the DataBuffer
  auto state = motor_state_buffer_.getData();
  if (state) {
    for (int i = 0; i < {ROBOT}_NUM_MOTOR; ++i) {
      jointData_[i].pos_ = state->q[i];
      jointData_[i].vel_ = state->dq[i];
      jointData_[i].tau_ = state->tau[i];
    }
  }

  // Pull the latest IMU state
  auto imu = imu_state_buffer_.getData();
  if (imu) {
    imuData_.ori_[0] = imu->quat[1];  // x
    imuData_.ori_[1] = imu->quat[2];  // y
    imuData_.ori_[2] = imu->quat[3];  // z
    imuData_.ori_[3] = imu->quat[0];  // w
    imuData_.angularVel_[0] = imu->omega[0];
    imuData_.angularVel_[1] = imu->omega[1];
    imuData_.angularVel_[2] = imu->omega[2];
    imuData_.linearAcc_[0]  = imu->acc[0];
    imuData_.linearAcc_[1]  = imu->acc[1];
    imuData_.linearAcc_[2]  = imu->acc[2];
  }

  // Performance monitoring: warn only when the threshold is exceeded
  if (monitor_enabled_) {
    auto duration_us = std::chrono::duration_cast<std::chrono::microseconds>(
        std::chrono::steady_clock::now() - start_time).count();
    if (duration_us > MonitorThresholds::READ_WARN_US) {
      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000,
          "read() took %ld us (threshold: %lu us)", duration_us, MonitorThresholds::READ_WARN_US);
    }
  }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type {Robot}HardwareInterface::write(
    const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
  auto start_time = std::chrono::steady_clock::now();

  // Safety: on the first write, initialize commands to the current position
  if (!command_initialized_.load() && first_state_received_.load()) {
    for (int i = 0; i < {ROBOT}_NUM_MOTOR; ++i) {
      jointData_[i].posDes_ = jointData_[i].pos_;  // target = current position
      jointData_[i].velDes_ = 0.0;
      jointData_[i].kp_     = 0.0;                 // zero-gain safe startup
      jointData_[i].kd_     = 0.0;
      jointData_[i].ff_     = 0.0;
    }
    command_initialized_.store(true);
    RCLCPP_INFO(node_->get_logger(), "Command initialized to current position.");
  }

  // Pack jointData_ into a command and push it to the DataBuffer
  MotorCommand cmd;
  for (int i = 0; i < {ROBOT}_NUM_MOTOR; ++i) {
    cmd.q_target[i] = jointData_[i].posDes_;
    cmd.dq_target[i] = jointData_[i].velDes_;
    cmd.kp[i]       = jointData_[i].kp_;
    cmd.kd[i]       = jointData_[i].kd_;
    cmd.tau_ff[i]   = jointData_[i].ff_;
  }
  motor_command_buffer_.setData(cmd);

  // Performance monitoring
  if (monitor_enabled_) {
    auto duration_us = std::chrono::duration_cast<std::chrono::microseconds>(
        std::chrono::steady_clock::now() - start_time).count();
    if (duration_us > MonitorThresholds::WRITE_WARN_US) {
      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000,
          "write() took %ld us (threshold: %lu us)", duration_us, MonitorThresholds::WRITE_WARN_US);
    }
  }

  return hardware_interface::return_type::OK;
}

// Callback implementations...

}  // namespace {vendor}

PLUGINLIB_EXPORT_CLASS({vendor}::{Robot}HardwareInterface, hardware_interface::SystemInterface)

6. Interface Export Specification

State Interfaces (per joint):

  • {joint_name}/position
  • {joint_name}/velocity
  • {joint_name}/effort

Command Interfaces (per joint):

  • {joint_name}/position
  • {joint_name}/velocity
  • {joint_name}/stiffness
  • {joint_name}/damping
  • {joint_name}/effort

IMU State Interfaces:

  • base_imu/orientation.{x,y,z,w}
  • base_imu/angular_velocity.{x,y,z}
  • base_imu/linear_acceleration.{x,y,z}

7. Data Flow Architecture

[SDK / Topic Callback Thread]
    │
    ▼  (write)
┌───────────────────┐
│    DataBuffer     │  <── thread-safe (std::shared_mutex)
└───────────────────┘
    │  (read)
    ▼
[ros2_control main thread]
    │
    ├── read()  →  update jointData_ / imuData_  (controllers read state)
    │
    └── write() →  read jointData_  →  send to SDK / Topic

8. Plugin Registration (robot_hardware_interface.xml)

Add an entry to robot_hardware_interface.xml:

<class name="{vendor}/{Robot}HardwareInterface"
       type="{vendor}::{Robot}HardwareInterface"
       base_class_type="hardware_interface::SystemInterface">
    <description>Hardware interface for {Vendor} {Robot}</description>
</class>

9. Communication Mode Selection

A. Direct DDS Mode (e.g. Unitree SDK2)

  • Use the SDK's ChannelPublisher / ChannelSubscriber
  • Use CreateRecurrentThreadEx to create the periodic send thread
  • Handle CRC computation and verification

B. ROS2 Topic Mode (e.g. Agibot aimdk_msgs)

  • Use rclcpp::Publisher / rclcpp::Subscription
  • Start executor_->spin() in on_activate and stop it in on_deactivate
  • Recommended QoS: Best Effort + Volatile (lowest latency)

10. Parameter Configuration

Supported URDF Parameters

Parameter Type Default Description
network_interface string lo Network interface name (DDS mode)
enable_monitor bool true Enable performance monitoring
command_writer_period_us uint64 2000 Command send period (microseconds)
state_decimation uint32 1 State decimation factor

Parameter Reading Example

// Network interface
if (info.hardware_parameters.find("network_interface") != info.hardware_parameters.end()) {
  network_interface = info.hardware_parameters.at("network_interface");
}

// Command send period (optional)
if (info.hardware_parameters.find("command_writer_period_us") != info.hardware_parameters.end()) {
  cmd_writer_period_us_ = std::stoul(info.hardware_parameters.at("command_writer_period_us"));
}

// State decimation (optional)
if (info.hardware_parameters.find("state_decimation") != info.hardware_parameters.end()) {
  state_decimation_ = std::stoul(info.hardware_parameters.at("state_decimation"));
}

URDF Configuration Example

<ros2_control name="{robot}_hardware" type="system">
    <hardware>
        <plugin>{vendor}/{Robot}HardwareInterface</plugin>
        <param name="network_interface">eth0</param>
        <param name="enable_monitor">true</param>
        <param name="command_writer_period_us">5000</param>  <!-- 200 Hz -->
        <param name="state_decimation">4</param>             <!-- 1000 Hz → 250 Hz -->
    </hardware>
    <!-- joints... -->
</ros2_control>

Common Configuration Combinations

Controller Rate command_writer_period_us state_decimation Effective State Rate
500 Hz 2000 (default) 1 (default) 1000 Hz
200 Hz 4000 – 5000 4 250 Hz
100 Hz 8000 – 10000 10 100 Hz

11. Safety Design

Startup Safety Mechanism

// Member variables
std::atomic<bool> first_state_received_{false};
std::atomic<bool> command_initialized_{false};

Three-layer protection:

Layer Location Protection
Layer 1 Command send function Do not send any command until first_state_received_ is true
Layer 2 State callback On the first state frame, initialize commands to current position with kp = kd = 0
Layer 3 write() On the first call, set posDes_ = current pos_ with kp = kd = 0

Safe initialization in the state callback:

void stateCallback(...) {
  // ... parse state ...

  // Safety: initialize on the first frame
  if (!first_state_received_.load()) {
    MotorCommand initial_cmd;
    for (int i = 0; i < NUM_MOTOR; ++i) {
      initial_cmd.q_target[i] = state.q[i];  // target = current position
      initial_cmd.kp[i]       = 0.0f;        // zero stiffness
      initial_cmd.kd[i]       = 0.0f;
      // ...
    }
    motor_command_buffer_.setData(initial_cmd);
    first_state_received_.store(true);
    RCLCPP_INFO(..., "First state received. Command initialized.");
  }
}

Safety guard in the command writer:

void commandWriter() {
  // Safety: do not send commands before any state has been received
  if (!first_state_received_.load()) {
    return;
  }
  // ... send command ...
}

State Decimation

Reduces CPU load from high-frequency state callbacks:

void stateCallback(...) {
  // ========== Safety-critical data — must be processed every frame (lightweight) ==========
  // Update mode_machine and other safety-critical fields...

  // ========== Decimation check ==========
  bool is_first_state = !first_state_received_.load();
  uint32_t frame_count = state_frame_counter_.fetch_add(1);
  if (!is_first_state && (frame_count % state_decimation_) != 0) {
    return;  // skip this frame to reduce CPU load
  }

  // ========== Full processing (decimated frames only) ==========
  // CRC verification, state parsing, IMU parsing, etc...
}

Decimation impact:

state_decimation Input Rate Effective Rate CPU Savings
1 1000 Hz 1000 Hz 0%
2 1000 Hz 500 Hz ~50%
4 1000 Hz 250 Hz ~75%
5 1000 Hz 200 Hz ~80%

12. Performance Monitoring

Monitor Mode (enabled by default)

Warnings are printed only when a threshold is exceeded — no log output during normal operation.

Threshold configuration:

struct MonitorThresholds {
  static constexpr uint64_t READ_WARN_US               = 100;   // read() latency threshold
  static constexpr uint64_t WRITE_WARN_US              = 100;   // write() latency threshold
  static constexpr uint64_t STATE_HANDLER_WARN_US      = 200;   // state callback latency threshold
  static constexpr uint64_t CMD_WRITER_WARN_US         = 200;   // command writer latency threshold
  static constexpr uint64_t CMD_INTERVAL_JITTER_WARN_US = 500;  // send interval jitter threshold
  static constexpr uint64_t CMD_INTERVAL_EXPECTED_US   = 2000;  // expected send period
};

Monitoring code template:

if (monitor_enabled_) {
  auto duration_us = std::chrono::duration_cast<std::chrono::microseconds>(
      std::chrono::steady_clock::now() - start_time).count();
  if (duration_us > MonitorThresholds::XXX_WARN_US) {
    RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000,
        "xxx() took %ld us (threshold: %lu us)", duration_us, MonitorThresholds::XXX_WARN_US);
  }
}

Jitter detection (for periodic send threads):

uint64_t current_time_us = /* current time */;
uint64_t last_time_us = cmd_writer_last_time_us_.exchange(current_time_us);
if (last_time_us > 0) {
  int64_t interval_us = current_time_us - last_time_us;
  // Use the configured period rather than a hard-coded value
  int64_t jitter_us = interval_us - static_cast<int64_t>(cmd_writer_period_us_);
  if (std::abs(jitter_us) > MonitorThresholds::CMD_INTERVAL_JITTER_WARN_US) {
    RCLCPP_WARN_THROTTLE(..., "Command interval jitter: %ld us (expected: %lu us)",
                         jitter_us, cmd_writer_period_us_);
  }
}

13. Real-Time Optimization

Zero-Allocation Design Principle

Avoid any heap allocation inside hot-path functions (read(), write(), callbacks):

Optimization Before After
DataBuffer make_shared on every call Atomic double buffer, pre-allocated
State / command parsing Temporary stack objects Pre-allocated member variables
SDK messages Local variables Reused member variables
ROS message vectors push_back (may realloc) resize once, then direct index access

Pre-allocated Message Objects

// Declare as member variables in the header
MotorState   motor_state_cache_;
ImuState     imu_state_cache_;
MotorCommand motor_cmd_cache_;
LowCmd_      low_cmd_cache_;                        // SDK message
sensor_msgs::msg::Joy joy_msg_cache_;               // ROS message

// Pre-allocate vectors in on_init()
joy_msg_cache_.axes.resize(5);
joy_msg_cache_.buttons.resize(8);

// Write directly into the pre-allocated cache in callbacks / write()
for (int i = 0; i < NUM_MOTOR; ++i) {
  motor_state_cache_.q[i] = ...;
}
motor_state_buffer_.setData(motor_state_cache_);

Reducing Processing Frequency

When the controller runs at a lower rate, there is no need to process state at the maximum SDK rate:

SDK state (1000 Hz) ──┬── mode_machine update  (every frame, lightweight)
                      │
                      └── full parsing  (decimated, e.g. 250 Hz)
                                │
                                ▼
                           DataBuffer
                                │
                                ▼
                       ros2_control (200 Hz)

14. Existing Implementations

Robot Vendor Communication Header Source
G1 Unitree DDS (unitree_sdk2) unitree/g1.hpp unitree/g1.cpp
X2 Agibot ROS2 Topic (aimdk_msgs) agibot/x2.hpp agibot/x2.cpp
PM01 EngineAI ROS2 Topic (interface_protocol) engineai/pm01.hpp engineai/pm01.cpp