We consider a minimal robot stack with four nodes:

• sensor_node: publishes a scalar measurement \( y_k \) at rate \( f_s \).
• controller_node: subscribes to \( y_k \), computes command \( u_k \), publishes cmd.
• motor_node: subscribes to cmd, logs or simulates actuation.
• tf_node: broadcasts a simple static frame tree (Lesson 4 usage only).

The application uses only ROS ideas already introduced: nodes, topics, services, packages/workspaces, launch, TF usage.

flowchart TD
  S["sensor_node: publish y_k on /sensor"] --> C["controller_node: subscribe /sensor, publish u_k on /cmd"]
  C --> M["motor_node: subscribe /cmd, apply/log"]
  T["tf_node: broadcast frames world->base->sensor"] -.-> S
  T -.-> C
  L["launch file"] --> S
  L --> C
  L --> M
  L --> T
        

Even in a “software-first” walkthrough, we need a small mathematical model of what flows through topics. Let samples arrive at discrete times \( t_k = kT_s \) where \( T_s = 1/f_s \) . The sensor publishes a measurement sequence:

\[ y_k = y(t_k) + n_k,\quad k=0,1,2,\dots \]

Here \( n_k \) models sensor noise. The controller computes a control command as a causal map: \( u_k = \pi(\{y_i\}_{i=0}^k) \). In minimal form we take a proportional controller:

\[ u_k = K_p\,(r_k - y_k) \]

where \( r_k \) is a reference signal (a unit step in our demo). Students know linear control, so we interpret the whole ROS pipeline as a sampled-data loop with delay.

Suppose the publish-subscribe path introduces a random latency \( d_k \ge 0 \) seconds from sensor publication to controller callback. The controller thus acts on delayed data:

\[ \tilde{y}_k = y(t_k - d_k) \]

A common stability heuristic from sampled-data control is that the total effective delay should remain below a fraction of the sampling period. A conservative sufficient condition for stable proportional control of a first-order plant is:

\[ \sup_k d_k \lt \frac{T_s}{2}. \]

Proof sketch (control-side intuition). Consider a stable continuous-time first-order plant \( \dot{x} = -ax + bu \) with \( a>0 \). Under zero-order-hold sampling and delay \( d \), the discrete-time closed-loop pole approximates \( z \approx e^{-a(T_s-d)}(1-bK_p d) \). If \( d\lt T_s/2 \), then \( T_s-d \gt T_s/2 \) preserves decay in \( e^{-a(T_s-d)} \), and for sufficiently small \( K_p \) the term \( (1-bK_p d) \) stays within \( (-1,1) \), hence \( |z|<1 \). This does not require any robot kinematics; it just links ROS timing to sampled-data stability the students already know.

Create a workspace (Lesson 3) and four packages. The minimal tree is:

ros_ws/
  src/
    sensor_pkg/
    controller_pkg/
    motor_pkg/
    tf_pkg/
  install/
  build/
  log/

Each package contains a single node executable and a launch file in a separate “bringup” package or inside controller_pkg.

Python nodes are fastest for prototyping. We use rclpy and standard messages std_msgs/msg/Float64.

4.1 sensor_node (publisher)

# sensor_pkg/sensor_pkg/sensor_node.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
import math, random

class SensorNode(Node):
    def __init__(self):
        super().__init__('sensor_node')
        self.pub = self.create_publisher(Float64, '/sensor', 10)
        self.Ts = 0.02  # 50 Hz
        self.k = 0
        self.timer = self.create_timer(self.Ts, self.tick)

    def tick(self):
        # simple scalar signal y_k = sin(0.5 t) + noise
        t = self.k * self.Ts
        y = math.sin(0.5 * t) + random.gauss(0.0, 0.01)
        msg = Float64()
        msg.data = y
        self.pub.publish(msg)
        self.k += 1

def main():
    rclpy.init()
    node = SensorNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

4.2 controller_node (subscriber + publisher)

# controller_pkg/controller_pkg/controller_node.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64

class ControllerNode(Node):
    def __init__(self):
        super().__init__('controller_node')
        self.sub = self.create_subscription(Float64, '/sensor', self.cb, 10)
        self.pub = self.create_publisher(Float64, '/cmd', 10)

        self.Kp = 1.5
        self.r = 1.0  # step reference

    def cb(self, msg):
        y = msg.data
        u = self.Kp * (self.r - y)
        out = Float64()
        out.data = u
        self.pub.publish(out)

def main():
    rclpy.init()
    node = ControllerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

4.3 motor_node (subscriber)

# motor_pkg/motor_pkg/motor_node.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64

class MotorNode(Node):
    def __init__(self):
        super().__init__('motor_node')
        self.sub = self.create_subscription(Float64, '/cmd', self.cb, 10)

    def cb(self, msg):
        self.get_logger().info(f"Actuating with u={msg.data:.3f}")

def main():
    rclpy.init()
    node = MotorNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Library note. In robotics Python stacks, ROS2 nodes coexist with numpy for linear algebra, scipy for filters, and (later courses) opencv-python or pybullet. Here we only need standard math and ROS2 types.

C++ is preferred for real-time-ish loops and embedded targets. We mirror the same architecture using rclcpp.

5.1 controller_node.cpp

// controller_pkg/src/controller_node.cpp
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"

class ControllerNode : public rclcpp::Node {
public:
  ControllerNode() : Node("controller_node"), Kp_(1.5), r_(1.0) {
    sub_ = this->create_subscription<std_msgs::msg::Float64>(
      "/sensor", 10,
      std::bind(&ControllerNode::cb, this, std::placeholders::_1)
    );
    pub_ = this->create_publisher<std_msgs::msg::Float64>("/cmd", 10);
  }

private:
  void cb(const std_msgs::msg::Float64::SharedPtr msg) {
    double y = msg->data;
    double u = Kp_ * (r_ - y);
    std_msgs::msg::Float64 out;
    out.data = u;
    pub_->publish(out);
  }

  double Kp_, r_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr sub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_;
};

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

Library note. C++ robotics code frequently uses Eigen (linear algebra), tf2 (transforms), and pluginlib (runtime composition). We only need rclcpp + std_msgs here.

Java ROS2 nodes use rcljava. The API is similar: create node, publisher/subscriber, callback.

// controller_pkg/src/main/java/ControllerNode.java
import org.ros2.rcljava.RCLJava;
import org.ros2.rcljava.node.Node;
import org.ros2.rcljava.publisher.Publisher;
import org.ros2.rcljava.subscription.Subscription;
import std_msgs.msg.Float64;

public class ControllerNode {
  public static void main(String[] args) {
    RCLJava.rclJavaInit();
    Node node = RCLJava.createNode("controller_node");

    final double Kp = 1.5;
    final double r = 1.0;

    Publisher<Float64> pub = node.<Float64>createPublisher(
        Float64.class, "/cmd");

    Subscription<Float64> sub = node.<Float64>createSubscription(
        Float64.class, "/sensor",
        msg -> {
          double y = msg.getData();
          Float64 out = new Float64();
          out.setData(Kp * (r - y));
          pub.publish(out);
        });

    RCLJava.spin(node);
    node.dispose();
    RCLJava.shutdown();
  }
}

Library note. Java robotics ecosystems often integrate with ROS2 plus JVM tools for distributed systems; later courses may use Java for HRI clients or Android-based robots.

MATLAB supports ROS2 through Robotics System Toolbox. A minimal controller subscribing to /sensor and publishing /cmd:

% controller_node.m
node = ros2node("/matlab_controller");
sub  = ros2subscriber(node, "/sensor", "std_msgs/Float64");
pub  = ros2publisher(node, "/cmd", "std_msgs/Float64");

Kp = 1.5; r = 1.0;

rate = ros2rate(node, 50);  % 50 Hz loop
while true
    msg = receive(sub, 0.1);      % timeout 0.1 s
    y = msg.data;
    u = Kp * (r - y);

    out = ros2message(pub);
    out.data = u;
    send(pub, out);

    waitfor(rate);
end

In Simulink, the same can be done with blocks: ROS2 Subscribe → Gain + Sum → ROS2 Publish. This is conceptually identical to the discrete-time equation in Section 2.

Following Lesson 4, we provide a static transform tree to label data. We do not derive kinematics; we only broadcast fixed offsets.

# tf_pkg/tf_pkg/tf_node.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

class TFNode(Node):
    def __init__(self):
        super().__init__('tf_node')
        self.broadcaster = StaticTransformBroadcaster(self)

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "world"
        t.child_frame_id = "base"
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0
        t.transform.rotation.w = 1.0  # identity quaternion

        self.broadcaster.sendTransform(t)

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

Any consumer can now interpret measurements as belonging to sensor frame attached to base in world.

A launch file coordinates node startup and parameters (Lesson 3). Minimal Python launch:

# controller_pkg/launch/minimal_robot.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(package='sensor_pkg', executable='sensor_node', name='sensor_node'),
        Node(package='controller_pkg', executable='controller_node', name='controller_node'),
        Node(package='motor_pkg', executable='motor_node', name='motor_node'),
        Node(package='tf_pkg', executable='tf_node', name='tf_node'),
    ])

Internally, ROS2 uses an executor that schedules callbacks. If callbacks take computation time \( c_k \) and arrive with period \( T_s \), a simple no-drop condition is:

\[ \sup_k c_k \lt T_s. \]

This is the same utilization idea from real-time control. If violated, queues grow and latency \( d_k \) increases, potentially breaking stability heuristics from Section 2.

flowchart TD
  A["Timer or message arrives"] --> B["Executor enqueues callback"]
  B --> C["Callback runs for c_k seconds"]
  C --> D["Publish output u_k"]
  D --> E["Next message arrives after T_s"]
  E --> A
  C -->|if c_k >= T_s| Q["Queue grows -> latency d_k increases"]
        

Minimal checks (no new concepts):

• ros2 topic list: verify /sensor and /cmd exist.
• ros2 topic echo /sensor: inspect numeric stream.
• ros2 node list: verify all nodes launched.
• ros2 run tf2_tools view_frames: confirm frame tree (usage only).

A simple quantitative sanity check is that the controller reduces error in expectation. With zero-mean noise \( E[n_k]=0 \) and constant reference \( r \):

\[ E[u_k] = K_p\,(r - E[y_k]) = K_p\,(r - E[y(t_k)]). \]

If \( E[y(t_k)] \) approaches \( r \), then \( E[u_k] \to 0 \), matching the physical intuition of a stabilizing feedback loop.

Problem 1 (Latency Bound): A sensor publishes at \( f_s = 40 \) Hz. The controller callback time is constant \( c = 8 \) ms. Assuming a single-threaded executor and no other load, verify whether the no-drop condition \( c \lt T_s \) holds.

Solution:

The sampling period is \( T_s = 1/f_s = 1/40 = 0.025 \) s = \( 25 \) ms. Since \( c = 8 \) ms and \( 8 \lt 25 \), the condition holds, so callbacks can be serviced without queue growth.

Problem 2 (Effect of Delay on Discrete Pole): Consider the first-order plant \( \dot{x}=-ax+bu \), sampled with period \( T_s \) and constant delay \( d \). Using the approximation from Section 2, \( z \approx e^{-a(T_s-d)}(1-bK_p d) \), show that increasing \( d \) moves \( |z| \) toward instability.

Solution:

Differentiate \( |z| \) w.r.t. \( d \). Over small delays with \( 1-bK_p d > 0 \):

\[ |z| \approx e^{-a(T_s-d)}(1-bK_p d). \]

Taking log: \( \ln|z| = -a(T_s-d) + \ln(1-bK_p d) \). Then

\[ \frac{\partial}{\partial d}\ln|z| = a - \frac{bK_p}{1-bK_p d}. \]

For stabilizing gains, \( bK_p \) is positive. Near \( d=0 \), the derivative becomes \( a - bK_p \), which is typically negative only for small gains. As \( d \) increases, the second term grows in magnitude, making the derivative less negative or positive, hence \( \ln|z| \) increases and \( |z| \) moves toward 1.

Problem 3 (Message Expectation): If measurement noise \( n_k \) is i.i.d. with \( E[n_k]=0 \) and variance \( \sigma_n^2 \), compute \( E[u_k] \) and \( \operatorname{Var}(u_k) \) for the proportional controller \( u_k = K_p(r-y_k) \).

Solution:

With \( y_k = y(t_k)+n_k \),

\[ u_k = K_p(r - y(t_k) - n_k). \]

Thus \( E[u_k] = K_p(r - y(t_k)) \). Since \( n_k \) is independent,

\[ \operatorname{Var}(u_k) = K_p^2 \operatorname{Var}(n_k) = K_p^2 \sigma_n^2. \]

Problem 4 (QoS Thought Experiment): Suppose /sensor is published at 100 Hz but the controller is only 50 Hz. With a queue depth of 10, estimate how long until the queue overflows if every message is stored (no drops) and callbacks take negligible time.

Solution:

Net arrival rate is \( 100-50 = 50 \) messages/s. Time to fill a queue of 10 is \( 10/50 = 0.2 \) s. After ~0.2 s, older messages must drop (or latency grows), motivating suitable QoS choices in ROS2.

We built a minimal but complete ROS2 robot application by composing nodes, topics, launch, and TF usage. Mathematically, we modeled the topic stream as a sampled-data loop with latency, showing how ROS timing connects to stability concepts from linear control. The same architecture was implemented in Python, C++, Java, and MATLAB/Simulink using the canonical ROS2 libraries. Next lesson: engineering practices for making such stacks reproducible and robust.