This document provides the AI agent with coding conventions and templates for the robot_hardware_interface package.
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
| 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 |
All robots share the following components to avoid code duplication.
#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#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)#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>;#include "robot_hardware_interface/common/monitor.hpp"
// Provides MonitorThresholds constants:
// - READ_WARN_US, WRITE_WARN_US, STATE_HANDLER_WARN_US, etc.#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}#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)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}
[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
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>- Use the SDK's
ChannelPublisher/ChannelSubscriber - Use
CreateRecurrentThreadExto create the periodic send thread - Handle CRC computation and verification
- Use
rclcpp::Publisher/rclcpp::Subscription - Start
executor_->spin()inon_activateand stop it inon_deactivate - Recommended QoS:
Best Effort+Volatile(lowest latency)
| 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 |
// 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"));
}<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>| 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 |
// 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 ...
}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% |
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_);
}
}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 |
// 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_);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)
| 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 |