ros2_control Guide 2026Hardware Interfaces · Controllers · Real Robot

sudo apt update
sudo apt install -y \
  ros-jazzy-ros2-control \
  ros-jazzy-ros2-controllers \
  ros-jazzy-controller-manager \
  ros-jazzy-joint-state-broadcaster \
  ros-jazzy-joint-trajectory-controller \
  ros-jazzy-diff-drive-controller \
  ros-jazzy-velocity-controllers \
  ros-jazzy-position-controllers

# Verify installation
ros2 pkg list | grep -E "ros2_control|controller_manager"

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

  <!-- ros2_control block — place BEFORE <gazebo> tags -->
  <ros2_control name="MyRobotSystem" type="system">

    <hardware>
      <!-- For simulated robot (Gazebo / Ignition) -->
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>

      <!-- For real hardware, replace with your custom plugin:
      <plugin>my_robot_hardware/MyRobotHardware</plugin>
      <param name="serial_port">/dev/ttyACM0</param>
      <param name="baud_rate">115200</param>
      -->
    </hardware>

    <!-- Revolute joint: position command, position+velocity state -->
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-3.14159</param>
        <param name="max">3.14159</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1.57</param>
        <param name="max">1.57</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

    <!-- Continuous joint (wheel): velocity command -->
    <joint name="left_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

    <joint name="right_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

  </ros2_control>

  <!-- Activate ros2_control plugin in Gazebo -->
  <gazebo>
    <plugin name="gazebo_ros2_control"
            filename="libgazebo_ros2_control.so">
      <robot_param>robot_description</robot_param>
      <robot_param_node>robot_state_publisher</robot_param_node>
      <parameters>$(find my_robot_bringup)/config/ros2_controllers.yaml</parameters>
    </plugin>
  </gazebo>

</robot>

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz — how often the hardware read/write cycle runs

    # Declare controllers to spawn
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

# JointStateBroadcaster has no extra params — it republishes all state interfaces

joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    # Tolerances
    goal_time:  0.5
    stopped_velocity_tolerance: 0.01
    state_publish_rate: 50.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false

diff_drive_controller:
  ros__parameters:
    left_wheel_names:  ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.3          # metres between wheel centres
    wheel_radius: 0.05             # metres
    use_stamped_vel: false
    # Velocity limits
    linear.x.max_velocity: 1.0
    linear.x.min_velocity: -1.0
    angular.z.max_velocity: 2.0
    angular.z.min_velocity: -2.0
    linear.x.max_acceleration: 1.0
    angular.z.max_acceleration: 1.5
    # Odometry
    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    pose_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    pkg = get_package_share_directory("my_robot_bringup")

    robot_description = Command([
        "xacro ",
        os.path.join(pkg, "urdf", "my_robot.urdf.xacro"),
    ])

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description}],
    )

    # controller_manager is started by the Gazebo plugin OR by:
    # ros2_control_node for real hardware
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            {"robot_description": robot_description},
            os.path.join(pkg, "config", "ros2_controllers.yaml"),
        ],
        output="both",
    )

    # Spawn joint_state_broadcaster first (always needed)
    spawn_jsb = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )

    # Spawn JTC after JSB is active
    spawn_jtc = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"],
    )

    # Spawn diff_drive after JSB is active
    spawn_diff = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diff_drive_controller", "--controller-manager", "/controller_manager"],
    )

    # Chain: start JTC and diff_drive AFTER JSB is active
    jsb_done = RegisterEventHandler(
        OnProcessExit(
            target_action=spawn_jsb,
            on_exit=[spawn_jtc, spawn_diff],
        )
    )

    return LaunchDescription([
        robot_state_publisher,
        ros2_control_node,
        spawn_jsb,
        jsb_done,
    ])

// my_robot_hardware/include/my_robot_hardware/my_robot_hardware.hpp
#pragma once

#include <vector>
#include <string>
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace my_robot_hardware {

class MyRobotHardware : public hardware_interface::SystemInterface {
public:
  // Called when hardware is loaded — parse params from URDF
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  // Register state interfaces (read-only: position, velocity, effort)
  std::vector<hardware_interface::StateInterface>
  export_state_interfaces() override;

  // Register command interfaces (write: position, velocity)
  std::vector<hardware_interface::CommandInterface>
  export_command_interfaces() override;

  // Called on lifecycle: configure → activate → deactivate → cleanup
  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State &) override;
  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State &) override;
  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State &) override;

  // Read sensor state from hardware (called at update_rate Hz)
  hardware_interface::return_type read(
    const rclcpp::Time &, const rclcpp::Duration &) override;

  // Write commands to hardware (called at update_rate Hz)
  hardware_interface::return_type write(
    const rclcpp::Time &, const rclcpp::Duration &) override;

private:
  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
  std::vector<double> hw_commands_;
  std::string serial_port_;
  int baud_rate_;
};

}  // namespace my_robot_hardware

// my_robot_hardware/src/my_robot_hardware.cpp
#include "my_robot_hardware/my_robot_hardware.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace my_robot_hardware {

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

  // Read params from URDF <param> tags
  serial_port_ = info_.hardware_parameters.at("serial_port");
  baud_rate_   = std::stoi(info_.hardware_parameters.at("baud_rate"));

  hw_positions_.resize(info_.joints.size(), 0.0);
  hw_velocities_.resize(info_.joints.size(), 0.0);
  hw_commands_.resize(info_.joints.size(), 0.0);

  return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface>
MyRobotHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    interfaces.emplace_back(
      info_.joints[i].name, "position", &hw_positions_[i]);
    interfaces.emplace_back(
      info_.joints[i].name, "velocity", &hw_velocities_[i]);
  }
  return interfaces;
}

std::vector<hardware_interface::CommandInterface>
MyRobotHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> interfaces;
  for (size_t i = 0; i < info_.joints.size(); ++i) {
    interfaces.emplace_back(
      info_.joints[i].name, "position", &hw_commands_[i]);
  }
  return interfaces;
}

hardware_interface::return_type MyRobotHardware::read(
  const rclcpp::Time &, const rclcpp::Duration &)
{
  // TODO: Read encoder positions from serial/CAN/EtherCAT
  // hw_positions_[i] = read_encoder(i);
  // hw_velocities_[i] = differentiate(hw_positions_[i]);
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type MyRobotHardware::write(
  const rclcpp::Time &, const rclcpp::Duration &)
{
  // TODO: Send hw_commands_[i] to motors via serial/CAN
  // send_position_command(i, hw_commands_[i]);
  return hardware_interface::return_type::OK;
}

}  // namespace my_robot_hardware

PLUGINLIB_EXPORT_CLASS(
  my_robot_hardware::MyRobotHardware,
  hardware_interface::SystemInterface)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from builtin_interfaces.msg import Duration


class JTCClient(Node):
    def __init__(self):
        super().__init__("jtc_client")
        self._client = ActionClient(
            self,
            FollowJointTrajectory,
            "/joint_trajectory_controller/follow_joint_trajectory"
        )

    def send_trajectory(self):
        self._client.wait_for_server()
        goal = FollowJointTrajectory.Goal()
        goal.trajectory.joint_names = ["joint1", "joint2"]

        # Point 1: reach at t=2s
        p1 = JointTrajectoryPoint()
        p1.positions = [0.5, -0.3]
        p1.velocities = [0.0, 0.0]
        p1.time_from_start = Duration(sec=2, nanosec=0)

        # Point 2: reach at t=4s
        p2 = JointTrajectoryPoint()
        p2.positions = [1.0, 0.5]
        p2.velocities = [0.0, 0.0]
        p2.time_from_start = Duration(sec=4, nanosec=0)

        # Point 3: return home at t=6s
        p3 = JointTrajectoryPoint()
        p3.positions = [0.0, 0.0]
        p3.velocities = [0.0, 0.0]
        p3.time_from_start = Duration(sec=6, nanosec=0)

        goal.trajectory.points = [p1, p2, p3]

        self.get_logger().info("Sending trajectory…")
        send_goal_future = self._client.send_goal_async(
            goal,
            feedback_callback=self._feedback_cb
        )
        send_goal_future.add_done_callback(self._goal_response_cb)

    def _goal_response_cb(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error("Trajectory rejected!")
            return
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self._result_cb)

    def _result_cb(self, future):
        self.get_logger().info("Trajectory complete!")

    def _feedback_cb(self, feedback_msg):
        pass  # actual_positions in feedback_msg.feedback


def main():
    rclpy.init()
    node = JTCClient()
    node.send_trajectory()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()