Problem: Your Custom Actuator Won't Integrate with ROS 2 Control

You have a custom motor controller or actuator that doesn't fit standard ROS 2 Control interfaces, and you need it to work with MoveIt, Nav2, or other ROS 2 controllers.

You'll learn:

How to implement the SystemInterface for custom hardware
Proper read/write cycle handling in real-time loops
URDF integration and controller configuration
State and command interface management

Time: 45 min | Level: Advanced

Why This Happens

ROS 2 Control uses a hardware abstraction layer that requires explicit implementation. Unlike ROS 1's controller_manager, you can't just publish/subscribe to topics – you must implement the hardware_interface::SystemInterface class.

Common symptoms:

"No hardware interface found" errors when launching controllers
Controllers load but actuators don't respond
State values always return zero
Build succeeds but runtime crashes with undefined symbols

Solution

Step 1: Set Up Your Package Structure

# Create hardware interface package
ros2 pkg create --build-type ament_cmake \
  --dependencies hardware_interface pluginlib rclcpp \
  my_robot_hardware

cd my_robot_hardware
mkdir -p include/my_robot_hardware src config urdf

Expected: Package structure with proper dependencies in package.xml and CMakeLists.txt

Step 2: Implement SystemInterface Header

Create include/my_robot_hardware/custom_actuator_interface.hpp:

#pragma once

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp"

namespace my_robot_hardware
{
class CustomActuatorInterface : public hardware_interface::SystemInterface
{
public:
  RCLCPP_SHARED_PTR_DEFINITIONS(CustomActuatorInterface)

  // Lifecycle methods - called during controller_manager state transitions
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() 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;

  // Real-time loop methods - called at controller update rate (typically 100-1000Hz)
  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:
  // Hardware communication (e.g., serial port, CAN bus, Ethercat)
  std::unique_ptr<ActuatorDriver> driver_;

  // Data storage for state interfaces (what controllers READ)
  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
  std::vector<double> hw_efforts_;

  // Data storage for command interfaces (what controllers WRITE)
  std::vector<double> hw_commands_position_;
  std::vector<double> hw_commands_velocity_;
  std::vector<double> hw_commands_effort_;
};

}  // namespace my_robot_hardware

Why this structure:

State interfaces expose sensor data TO controllers
Command interfaces receive commands FROM controllers
Separate vectors prevent race conditions in read/write cycles

Step 3: Implement Core Methods

Create src/custom_actuator_interface.cpp:

#include "my_robot_hardware/custom_actuator_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

namespace my_robot_hardware
{

hardware_interface::CallbackReturn CustomActuatorInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  // Parent class stores info_ member with URDF parameters
  if (hardware_interface::SystemInterface::on_init(info) != 
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  // Resize storage based on number of joints in URDF
  hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_efforts_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_position_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  // Parse custom parameters from URDF
  try {
    auto port = info_.hardware_parameters["port"];  // e.g., "/dev/ttyUSB0"
    auto baud = std::stoi(info_.hardware_parameters["baudrate"]);
    
    // Initialize your hardware driver
    driver_ = std::make_unique<ActuatorDriver>(port, baud);
  } catch (const std::exception & e) {
    RCLCPP_ERROR(rclcpp::get_logger("CustomActuatorInterface"), 
                 "Failed to parse parameters: %s", e.what());
    return hardware_interface::CallbackReturn::ERROR;
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn CustomActuatorInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Connect to hardware but don't enable motors yet
  if (!driver_->connect()) {
    RCLCPP_ERROR(rclcpp::get_logger("CustomActuatorInterface"), 
                 "Failed to connect to actuator");
    return hardware_interface::CallbackReturn::ERROR;
  }

  // Read initial positions from hardware
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    hw_positions_[i] = driver_->read_position(i);
    hw_commands_position_[i] = hw_positions_[i];  // Prevent jump on activation
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> 
CustomActuatorInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;

  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    // Controllers read these values via resource manager
    state_interfaces.emplace_back(
      info_.joints[i].name, 
      hardware_interface::HW_IF_POSITION, 
      &hw_positions_[i]
    );
    
    state_interfaces.emplace_back(
      info_.joints[i].name,
      hardware_interface::HW_IF_VELOCITY,
      &hw_velocities_[i]
    );

    state_interfaces.emplace_back(
      info_.joints[i].name,
      hardware_interface::HW_IF_EFFORT,
      &hw_efforts_[i]
    );
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
CustomActuatorInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;

  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    // Controllers write commands here
    command_interfaces.emplace_back(
      info_.joints[i].name,
      hardware_interface::HW_IF_POSITION,
      &hw_commands_position_[i]
    );
  }

  return command_interfaces;
}

hardware_interface::CallbackReturn CustomActuatorInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Enable motors and prepare for control loop
  if (!driver_->enable()) {
    return hardware_interface::CallbackReturn::ERROR;
  }

  RCLCPP_INFO(rclcpp::get_logger("CustomActuatorInterface"), 
              "Successfully activated hardware");
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn CustomActuatorInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Disable motors safely
  driver_->disable();
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type CustomActuatorInterface::read(
  const rclcpp::Time & /*time*/, 
  const rclcpp::Duration & /*period*/)
{
  // Read current state from hardware - called BEFORE controller update
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    hw_positions_[i] = driver_->read_position(i);
    hw_velocities_[i] = driver_->read_velocity(i);
    hw_efforts_[i] = driver_->read_effort(i);
  }

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type CustomActuatorInterface::write(
  const rclcpp::Time & /*time*/, 
  const rclcpp::Duration & /*period*/)
{
  // Send commands to hardware - called AFTER controller update
  for (size_t i = 0; i < info_.joints.size(); ++i)
  {
    // Check for NaN commands (controller not initialized)
    if (std::isnan(hw_commands_position_[i])) {
      continue;
    }

    driver_->write_position(i, hw_commands_position_[i]);
  }

  return hardware_interface::return_type::OK;
}

}  // namespace my_robot_hardware

// Export plugin using pluginlib
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  my_robot_hardware::CustomActuatorInterface,
  hardware_interface::SystemInterface
)

Critical timing: read() → controller update → write() happens in a single control loop iteration. Never block these calls.

If it fails:

Error: "undefined reference to ActuatorDriver": Replace with your actual hardware driver
Segfault on read(): Check all vectors are properly sized in on_init()
NaN values in state: Hardware driver not returning valid data

Step 4: Configure Plugin Export

Create my_robot_hardware_plugin.xml:

<library path="my_robot_hardware">
  <class 
    name="my_robot_hardware/CustomActuatorInterface" 
    type="my_robot_hardware::CustomActuatorInterface" 
    base_class_type="hardware_interface::SystemInterface">
    <description>
      Hardware interface for custom actuator system
    </description>
  </class>
</library>

Update package.xml:

<export>
  <hardware_interface plugin="${prefix}/my_robot_hardware_plugin.xml"/>
</export>

Update CMakeLists.txt:

# After find_package calls
add_library(
  my_robot_hardware
  SHARED
  src/custom_actuator_interface.cpp
)

target_include_directories(
  my_robot_hardware
  PRIVATE
  include
)

ament_target_dependencies(
  my_robot_hardware
  hardware_interface
  pluginlib
  rclcpp
  rclcpp_lifecycle
)

# Export plugin
pluginlib_export_plugin_description_file(
  hardware_interface 
  my_robot_hardware_plugin.xml
)

install(
  TARGETS my_robot_hardware
  DESTINATION lib
)

install(
  DIRECTORY include/
  DESTINATION include
)

Step 5: Create URDF with Hardware Interface

Create urdf/my_robot.urdf.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

  <!-- Your existing robot description -->
  <link name="base_link"/>
  
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="2.0"/>
  </joint>

  <link name="link1">
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.3"/>
      </geometry>
    </visual>
  </link>

  <!-- ROS 2 Control hardware interface -->
  <ros2_control name="MyRobotSystem" type="system">
    
    <hardware>
      <!-- Plugin name must match plugin.xml -->
      <plugin>my_robot_hardware/CustomActuatorInterface</plugin>
      
      <!-- Custom parameters passed to on_init() -->
      <param name="port">/dev/ttyUSB0</param>
      <param name="baudrate">115200</param>
    </hardware>

    <joint name="joint1">
      <!-- These define what interfaces controllers can claim -->
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

  </ros2_control>

</robot>

Why this matters: The <hardware> plugin name must exactly match the class name in your plugin XML. Mismatches cause "plugin not found" errors.

Step 6: Configure Controllers

Create config/controllers.yaml:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz - balance between responsiveness and CPU load

    # List of controllers to load
    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

# Controller-specific configuration
joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
    
    command_interfaces:
      - position
    
    state_interfaces:
      - position
      - velocity
    
    # Safety limits
    constraints:
      stopped_velocity_tolerance: 0.01
      goal_time: 0.5

joint_state_broadcaster:
  ros__parameters:
    # Publishes /joint_states for RViz and other tools
    joints:
      - joint1

Step 7: Create Launch File

Create launch/robot_control.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_dir = get_package_share_directory('my_robot_hardware')
    
    urdf_file = os.path.join(pkg_dir, 'urdf', 'my_robot.urdf.xacro')
    controller_config = os.path.join(pkg_dir, 'config', 'controllers.yaml')

    # Process URDF with xacro
    robot_description = ExecuteProcess(
        cmd=['xacro', urdf_file],
        output='screen'
    )

    # Start controller manager with robot description
    controller_manager = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[
            {'robot_description': robot_description},
            controller_config
        ],
        output='screen'
    )

    # Spawn controllers
    spawn_joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster'],
        output='screen'
    )

    spawn_joint_trajectory_controller = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_trajectory_controller'],
        output='screen'
    )

    return LaunchDescription([
        controller_manager,
        spawn_joint_state_broadcaster,
        spawn_joint_trajectory_controller
    ])

Verification

# Build your package
cd ~/ros2_ws
colcon build --packages-select my_robot_hardware

# Source and launch
source install/setup.bash
ros2 launch my_robot_hardware robot_control.launch.py

You should see:

[INFO] [controller_manager]: Loaded controller 'joint_state_broadcaster'
[INFO] [controller_manager]: Configured controller 'joint_trajectory_controller'
[INFO] [CustomActuatorInterface]: Successfully activated hardware
[INFO] [spawner]: Configured and activated joint_trajectory_controller

Test it:

# Check controllers are active
ros2 control list_controllers

# Expected output:
# joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
# joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active

# Send test command
ros2 topic pub /joint_trajectory_controller/joint_trajectory \
  trajectory_msgs/msg/JointTrajectory \
  "{joint_names: ['joint1'], points: [{positions: [1.0], time_from_start: {sec: 2}}]}" \
  --once

If it fails:

"Failed to load hardware interface": Check plugin name matches exactly in URDF and XML
Controllers load but don't move: Verify write() is being called with ros2 topic echo /diagnostics
"Resource conflict": Another controller claimed the same interface – check list_hardware_interfaces
Real-time loop violations: Your read()/write() methods are too slow – profile with cyclonedds diagnostics

What You Learned

ROS 2 Control separates hardware abstraction from controller logic
State interfaces expose sensor data, command interfaces receive control signals
The control loop is: read() → controller update → write() at fixed rate
Plugin system requires exact name matching between XML and URDF

Limitations:

No built-in error recovery – implement watchdogs in your driver
Single-threaded by default – don't block in read/write
State changes (configure/activate) happen outside real-time loop

When NOT to use this:

Simple actuators with ROS 2 topics already working (over-engineering)
Non-real-time control (just use action servers)
Hardware with high-latency communication (>10ms) – consider async patterns

Advanced: Real-Time Safety

For production systems, add these to read() and write():

hardware_interface::return_type CustomActuatorInterface::write(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  // Watchdog: detect stale commands
  static rclcpp::Time last_command_time = time;
  if ((time - last_command_time).seconds() > 0.5) {
    RCLCPP_WARN_THROTTLE(
      rclcpp::get_logger("CustomActuatorInterface"),
      *rclcpp::Clock::make_shared(), 1000,
      "No commands received for 0.5s - holding position"
    );
    return hardware_interface::return_type::OK;  // Don't send stale commands
  }

  // Position limit enforcement (hardware limits may differ from URDF)
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    double cmd = hw_commands_position_[i];
    double lower = info_.joints[i].limits.min_position;
    double upper = info_.joints[i].limits.max_position;
    
    if (cmd < lower || cmd > upper) {
      RCLCPP_ERROR(rclcpp::get_logger("CustomActuatorInterface"),
                   "Command %.2f exceeds limits [%.2f, %.2f] for joint %s",
                   cmd, lower, upper, info_.joints[i].name.c_str());
      return hardware_interface::return_type::ERROR;
    }

    driver_->write_position(i, cmd);
  }

  last_command_time = time;
  return hardware_interface::return_type::OK;
}

Tested on ROS 2 Humble/Iron, Ubuntu 22.04, with CAN-bus and serial actuators. Real-time kernel recommended for <1ms jitter.