ros2_control Introduction - EEET2610

Overview

ros2_control is a framework for real-time control of robots in ROS2. It provides hardware abstraction, allowing controllers to work with both real robots and simulators using the same interface.

Why ros2_control? Separates low-level hardware (ESP32 motors) from high-level control (velocity commands), enabling code reuse and standardization.

Architecture

Hardware Interface

Communicates with actual hardware (ESP32, motor drivers). Reads sensors, writes commands.

Controller Manager

Loads, configures, and manages controllers. Routes commands between controllers and hardware.

Controllers

Implement control algorithms (PID, diff-drive, Mecanum). Subscribe to cmd_vel, publish joint states.

Data Flow

┌─────────────────────┐
│   Navigation /      │
│   Teleop Node       │
└──────────┬──────────┘
           │ /cmd_vel (Twist)
           ▼
┌─────────────────────┐
│  Mecanum Drive      │
│    Controller       │
└──────────┬──────────┘
           │ Joint Commands
           ▼
┌─────────────────────┐
│  Controller         │
│    Manager          │
└──────────┬──────────┘
           │ Write/Read
           ▼
┌─────────────────────┐
│   Hardware          │
│   Interface (ESP32) │
└──────────┬──────────┘
           │ Serial/USB
           ▼
┌─────────────────────┐
│   Motor Drivers     │
│   (IBT-2 x4)        │
└─────────────────────┘

Key Concepts

Hardware Interface

Defines how software talks to hardware: State Interfaces (read from hardware):

Joint positions (encoder angles)
Joint velocities (wheel speeds)
Joint efforts (motor currents, optional)

Command Interfaces (write to hardware):

Joint velocities (target wheel speeds)
Joint efforts (target motor torques)

Controllers

Implement control logic: Common controllers:

joint_state_broadcaster - Publishes /joint_states topic
diff_drive_controller - 2-wheel differential drive
mecanum_drive_controller - 4-wheel Mecanum drive (we’ll use this!)
joint_trajectory_controller - Follows joint trajectories

Custom controllers:

You can write your own for specialized behaviors

Controller Manager

Central node that:

Loads controllers from YAML config
Activates/deactivates controllers
Manages controller lifecycles (configure → activate → deactivate → cleanup)

ros2_control vs Direct Control

Without ros2_control
With ros2_control

# Direct motor control (simple but not reusable)
import serial

ser = serial.Serial('/dev/ttyUSB0', 115200)

def send_velocity(vx, vy, omega):
    # Manually compute wheel velocities
    w1, w2, w3, w4 = inverse_kinematics(vx, vy, omega)

    # Send to ESP32
    command = f"q{w1:.2f},{w2:.2f},{w3:.2f},{w4:.2f}\n"
    ser.write(command.encode())

# Teleop node
send_velocity(0.5, 0.0, 0.3)

Problems:

Kinematics hardcoded in every node
Can’t switch between sim and real robot
No standardization
Difficult to add new controllers

URDF Integration

ros2_control configuration lives in URDF/Xacro:

<robot name="mecanum_robot">
  <!-- Links and joints as before -->

  <!-- ros2_control tag -->
  <ros2_control name="mecanum_system" type="system">

    <!-- Hardware interface plugin -->
    <hardware>
      <plugin>mecanum_hardware/MecanumSystemHardware</plugin>
      <param name="serial_port">/dev/ttyUSB0</param>
      <param name="baud_rate">115200</param>
    </hardware>

    <!-- Joint: Front-Left Wheel -->
    <joint name="wheel_FL_joint">
      <command_interface name="velocity"/>  <!-- We send velocity commands -->
      <state_interface name="position"/>    <!-- We read encoder position -->
      <state_interface name="velocity"/>    <!-- We read wheel velocity -->
    </joint>

    <!-- Repeat for FR, RL, RR -->
    <joint name="wheel_FR_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

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

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

  </ros2_control>
</robot>

Controller Configuration

File: config/mecanum_controller.yaml

controller_manager:
  ros__parameters:
    update_rate: 50  # Hz (20ms control loop)

    # List of controllers to load
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    mecanum_drive_controller:
      type: mecanum_drive_controller/MecanumDriveController

# Joint State Broadcaster config
joint_state_broadcaster:
  ros__parameters:
    # Publishes /joint_states for all joints

# Mecanum Drive Controller config
mecanum_drive_controller:
  ros__parameters:
    # Joint names (must match URDF)
    front_left_wheel_joint: wheel_FL_joint
    front_right_wheel_joint: wheel_FR_joint
    rear_left_wheel_joint: wheel_RL_joint
    rear_right_wheel_joint: wheel_RR_joint

    # Kinematics
    wheel_separation_x: 0.30  # Front-rear distance (m)
    wheel_separation_y: 0.35  # Left-right distance (m)
    wheel_radius: 0.05        # Wheel radius (m)

    # Command topic
    cmd_vel_topic: /cmd_vel

    # Odometry
    publish_odom: true
    odom_frame_id: odom
    base_frame_id: base_link
    publish_rate: 50.0

    # Velocity limits
    linear:
      x:
        max_velocity: 1.0      # m/s
        max_acceleration: 2.0  # m/s²
      y:
        max_velocity: 1.0
        max_acceleration: 2.0
    angular:
      z:
        max_velocity: 2.0      # rad/s
        max_acceleration: 4.0  # rad/s²

Launch File

File: launch/robot_control.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    # Paths
    pkg_share = FindPackageShare('mecanum_description').find('mecanum_description')
    urdf_file = PathJoinSubstitution([pkg_share, 'urdf', 'mecanum_robot.urdf.xacro'])
    controller_config = PathJoinSubstitution([pkg_share, 'config', 'mecanum_controller.yaml'])

    # Process xacro
    robot_description = Command(['xacro ', urdf_file])

    return LaunchDescription([
        # Robot State Publisher
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': robot_description}]
        ),

        # Controller Manager
        Node(
            package='controller_manager',
            executable='ros2_control_node',
            parameters=[
                {'robot_description': robot_description},
                controller_config
            ],
            output='screen'
        ),

        # Spawner: Joint State Broadcaster
        Node(
            package='controller_manager',
            executable='spawner',
            arguments=['joint_state_broadcaster'],
            output='screen'
        ),

        # Spawner: Mecanum Drive Controller
        Node(
            package='controller_manager',
            executable='spawner',
            arguments=['mecanum_drive_controller'],
            output='screen'
        ),
    ])

Controller Lifecycle

Controllers have managed lifecycles:

Unconfigured (initial state)
Inactive (configured but not running)
Active (running)
Finalized (shutdown)

Managing Controllers

# List all controllers
ros2 control list_controllers

# Example output:
# joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
# mecanum_drive_controller[mecanum_drive_controller/MecanumDriveController] active

# Load controller
ros2 control load_controller mecanum_drive_controller

# Configure controller
ros2 control set_controller_state mecanum_drive_controller configure

# Activate controller
ros2 control switch_controllers --activate mecanum_drive_controller

# Deactivate controller
ros2 control switch_controllers --deactivate mecanum_drive_controller

# Unload controller
ros2 control unload_controller mecanum_drive_controller

Spawner does it all: The spawner node in launch file automatically loads + configures + activates controllers. Manual commands useful for debugging only.

Hardware Interface Plugin

The hardware interface is a C++ plugin that talks to ESP32: Skeleton structure:

#include "hardware_interface/system_interface.hpp"

namespace mecanum_hardware {

class MecanumSystemHardware : public hardware_interface::SystemInterface {
public:
  // Lifecycle callbacks
  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;

  // State interfaces (read from hardware)
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  // Command interfaces (write to hardware)
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  // Read from hardware (called every control cycle)
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  // Write to hardware (called every control cycle)
  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  // Serial connection to ESP32
  int serial_fd_;

  // Joint states
  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
  std::vector<double> hw_commands_;
};

}  // namespace mecanum_hardware

Key methods:

read() - Read encoder positions/velocities from ESP32 via serial
write() - Send velocity commands to ESP32 via serial
export_state_interfaces() - Declare what states we provide (position, velocity)
export_command_interfaces() - Declare what commands we accept (velocity)

Package Dependencies

File: package.xml

<depend>hardware_interface</depend>
<depend>controller_interface</depend>
<depend>controller_manager</depend>
<depend>joint_state_broadcaster</depend>
<depend>mecanum_drive_controller</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>

Installation

# Install ros2_control packages
sudo apt install ros-jazzy-ros2-control ros-jazzy-ros2-controllers

# Install specific controllers
sudo apt install ros-jazzy-joint-state-broadcaster
sudo apt install ros-jazzy-mecanum-drive-controller

Mecanum controller availability: If mecanum_drive_controller not available as Debian package, you may need to build from source or use diff_drive_controller with custom kinematics.

Alternative: diff_drive_controller

If Mecanum controller unavailable, use differential drive controller (works for straight + rotation only, no strafing):

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["wheel_FL_joint", "wheel_RL_joint"]
    right_wheel_names: ["wheel_FR_joint", "wheel_RR_joint"]

    wheel_separation: 0.35
    wheel_radius: 0.05

    cmd_vel_topic: /cmd_vel

    publish_odom: true
    odom_frame_id: odom
    base_frame_id: base_link

Limitation: No lateral (Y-axis) movement. For full Mecanum capability, custom controller needed.

Testing Setup

Verify Controller Manager

# Launch robot control
ros2 launch mecanum_description robot_control.launch.py

# Check controllers loaded
ros2 control list_controllers

# Expected output:
# joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
# mecanum_drive_controller[mecanum_drive_controller/MecanumDriveController] active

Check Topics

# List topics
ros2 topic list

# Should include:
# /cmd_vel                      (Twist commands IN)
# /joint_states                 (Joint states OUT)
# /mecanum_drive_controller/odom (Odometry OUT)
# /tf                           (Transforms OUT)

Send Test Command

# Send velocity command
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" --once

# If hardware interface working:
# - ESP32 receives velocity command
# - Motors spin
# - Robot moves forward

Troubleshooting

Controller Manager fails to start

Error: Failed to load controller ...Solutions:

Check robot_description parameter published
Verify URDF has <ros2_control> tag
Check controller YAML syntax
Install missing controller packages

Hardware interface not found

Error: Could not load hardware interface plugin ...Solutions:

Build hardware interface package
Check plugin exported in CMakeLists.txt
Verify plugin name in URDF matches class name
Source workspace: source install/setup.bash

No command forwarded to hardware

Symptom: /cmd_vel messages sent but robot doesn’t moveDebug:

# Check controller active
ros2 control list_controllers

# Check command interfaces
ros2 control list_hardware_interfaces

# Monitor serial communication (ESP32 side)
# Add debug prints in hardware interface read/write

Joint states not published

Symptom: /joint_states topic emptySolutions:

Check joint_state_broadcaster active
Verify hardware interface read() updates positions/velocities
Check joint names in URDF match controller config

Benefits of ros2_control

Hardware Abstraction

Same controller code works with real robot and Gazebo simulator by swapping hardware interface.

Modular Design

Add/remove controllers without changing other code. Test new control algorithms easily.

Standardization

Controllers work with any robot using ros2_control. Reuse community-developed controllers.

Real-Time Capable

Designed for deterministic real-time control loops (important for smooth motion).

Next Steps

Hardware Interface

Implement hardware interface for ESP32 communication

Teleop Control

Control robot with keyboard using cmd_vel

Odometry Publishing

Publish robot pose from wheel encoders

ros2_control Demos

Official ros2_control examples and tutorials

References

[1] ros2_control Documentation: https://control.ros.org/ [2] ros2_controllers: https://control.ros.org/jazzy/doc/ros2_controllers/doc/controllers_index.html [3] Hardware Interface: https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/writing_new_hardware_component.html

WP1: MATLAB Simulation

WP2: Prototyping

WP3: Embedded Systems

WP4: ROS2 Setup

WP4: Robot Description

WP4: Control

WP4: Localization

WP4: Mapping

WP4: Navigation

WP4: Sensors

WP5: Integration & Presentation

​Overview

​Architecture

Hardware Interface

Controller Manager

Controllers

​Data Flow

​Key Concepts

​Hardware Interface

​Controllers

​Controller Manager

​ros2_control vs Direct Control

​URDF Integration

​Controller Configuration

​Launch File

​Controller Lifecycle

​Managing Controllers

​Hardware Interface Plugin

​Package Dependencies

​Installation

​Alternative: diff_drive_controller

​Testing Setup

​Verify Controller Manager

​Check Topics

​Send Test Command

​Troubleshooting

​Benefits of ros2_control

Hardware Abstraction

Modular Design

Standardization

Real-Time Capable

​Next Steps

Hardware Interface

Teleop Control

Odometry Publishing

ros2_control Demos

​References

Overview

Architecture

Data Flow

Key Concepts

Hardware Interface

Controllers

Controller Manager

ros2_control vs Direct Control

URDF Integration

Controller Configuration

Launch File

Controller Lifecycle

Managing Controllers

Hardware Interface Plugin

Package Dependencies

Installation

Alternative: diff_drive_controller

Testing Setup

Verify Controller Manager

Check Topics

Send Test Command

Troubleshooting

Benefits of ros2_control

Next Steps

References