ROS2 Architecture for Real-Time Robot Control

Week 8 — Control Systems Implementation
Day 4 of 7 | Difficulty: ⭐⭐⭐

---

Why ROS2?

ROS1 was revolutionary but had a critical flaw: no real-time guarantees. ROS2 fixes this with:

• DDS middleware: Data Distribution Service for reliable, low-latency communication
• Real-time support: Compatible with RT-PREEMPT Linux kernels
• Lifecycle nodes: Managed startup/shutdown with state machines
• Security: Encryption, authentication, access control
• Multi-robot: Native support for distributed systems

For control systems, ROS2 is the industry standard.

---

ROS2 Architecture for Control

Node: The Unit of Computation

Every component is a node:

• sensor_driver node — reads encoders, IMU, cameras
• controller node — runs PID/LQR, publishes torque commands
• planner node — generates trajectories
• motor_driver node — receives torques, sends to hardware

Nodes communicate via topics (pub/sub) or services (request/response).

Topics: The Data Bus

Topic	Message Type	Publisher	Subscriber
/joint_states	sensor_msgs/JointState	Sensor driver	Controller, RViz
/joint_commands	std_msgs/Float64MultiArray	Controller	Motor driver
/trajectory	trajectory_msgs/JointTrajectory	Planner	Controller
/diagnostics	diagnostic_msgs/DiagnosticArray	All nodes	Logger

Control Loop in ROS2

[Sensor Node] ──/joint_states──▶ [Controller Node]
                                      │
                                      ▼
                              [LQR/PID computes u]
                                      │
                                      ▼
[Motor Node] ◀──/joint_commands──────┘

Period: The controller node typically runs at 100–1000 Hz using a rclcpp::WallTimer.

---

Writing a Controller Node (C++ Skeleton)

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>

class JointController : public rclcpp::Node {
public:
    JointController() : Node("joint_controller") {
        // Subscribe to joint states
        state_sub_ = create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 10,
            std::bind(&JointController::state_callback, this, std::placeholders::_1));
        
        // Publish joint commands
        cmd_pub_ = create_publisher<std_msgs::msg::Float64MultiArray>(
            "/joint_commands", 10);
        
        // 1 kHz control loop
        timer_ = create_wall_timer(
            std::chrono::milliseconds(1),
            std::bind(&JointController::control_loop, this));
    }

private:
    void state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) {
        current_position_ = msg->position;
        current_velocity_ = msg->velocity;
    }
    
    void control_loop() {
        // Compute control law (PID/LQR)
        auto cmd = compute_control(current_position_, current_velocity_, target_position_);
        cmd_pub_->publish(cmd);
    }
    
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr cmd_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    std::vector<double> current_position_, current_velocity_, target_position_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<JointController>());
    rclcpp::shutdown();
    return 0;
}

---

Launch Files: Orchestrating the Stack

A launch.py file starts all nodes in order:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(package='my_robot', executable='sensor_driver', name='sensor'),
        Node(package='my_robot', executable='controller', name='controller'),
        Node(package='my_robot', executable='motor_driver', name='motor'),
    ])

Run with: ros2 launch my_robot control_stack.launch.py

---

Real-Time Considerations

1. DDS Tuning

Use Fast DDS or Cyclone DDS with:

• Deadline QoS: ensures messages arrive within $T_s$
• Reliability QoS: retransmits lost packets
• History QoS: keeps last $N$ samples

2. Thread Priority

Set controller thread to SCHED_FIFO with high priority:

struct sched_param param;
param.sched_priority = 90;
pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

3. Kernel

Use RT-PREEMPT kernel patch for hard real-time. Standard Ubuntu is soft real-time only.

4. Avoid Allocations in the Loop

No new, malloc, or std::vector resizing in the control callback. Pre-allocate all buffers at startup.

---

Key Takeaway

ROS2 transforms a robot from a monolithic program into a modular, distributed system. Each component (sensing, control, actuation) is a replaceable node. The control loop is just a node that subscribes to sensors and publishes commands — at 1 kHz, if you tune DDS and the kernel right.

---

Next: Day 5 — Sensor Fusion for State Estimation