Tutorial 3: Full Robot Integration

Looking for the Python version? See Tutorial 3: Full Robot System (Python).

Prerequisites

What You'll Build

A robot system with 4 nodes running at different rates:

ImuSensor (100 Hz) ──→ "imu.data" ──→ StateEstimator (100 Hz)
                                              │
Commander (10 Hz)  ──→ "motor.cmd" ──→ MotorController (50 Hz)
                                              │
                                        "motor.state" ──→ StateEstimator
                                              │
                                         "robot.pose" (output)

You'll learn to compose multiple node types, schedule them at different rates, and monitor the system with horus monitor.

Time estimate: ~20 minutes

Step 1: Create the Project

horus new robot-integration -r
cd robot-integration

You should see horus.toml, src/main.rs, and .horus/ in the project directory.

Step 2: Define Shared Types

Replace src/main.rs with the shared data types. We reuse the types from Tutorials 1 and 2, plus a robot pose:

use horus::prelude::*;

#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct ImuData {
    accel_x: f32, accel_y: f32, accel_z: f32,
    gyro_roll: f32, gyro_pitch: f32, gyro_yaw: f32,
    timestamp: f64,
}

#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct MotorCommand {
    velocity: f32,    // rad/s
    max_torque: f32,  // N*m
}

#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct MotorState {
    position: f32,    // radians
    velocity: f32,    // rad/s
    torque: f32,      // N*m
    timestamp: f64,
}

#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct RobotPose {
    x: f32,           // meters
    y: f32,           // meters
    heading: f32,     // radians
    timestamp: f64,
}

Step 3: The IMU Sensor (from Tutorial 1)

struct ImuSensor {
    publisher: Topic<ImuData>,
    tick_count: u64,
}

impl ImuSensor {
    fn new() -> Result<Self> {
        Ok(Self { publisher: Topic::new("imu.data")?, tick_count: 0 })
    }
}

impl Node for ImuSensor {
    fn name(&self) -> &str { "ImuSensor" }

    fn tick(&mut self) {
        let t = self.tick_count as f64 * 0.01;
        self.publisher.send(ImuData {
            accel_x: 0.0, accel_y: 0.0, accel_z: 9.81,
            gyro_roll: 0.0, gyro_pitch: 0.0,
            gyro_yaw: 0.1 * (t * 0.5).sin() as f32,
            timestamp: t,
        });
        self.tick_count += 1;
    }

    fn shutdown(&mut self) -> Result<()> {
        eprintln!("ImuSensor shutting down after {} ticks", self.tick_count);
        Ok(())
    }
}

Step 4: The Motor Controller (from Tutorial 2)

struct MotorController {
    commands: Topic<MotorCommand>,
    state_pub: Topic<MotorState>,
    position: f32,
    velocity: f32,
}

impl MotorController {
    fn new() -> Result<Self> {
        Ok(Self {
            commands: Topic::new("motor.cmd")?,
            state_pub: Topic::new("motor.state")?,
            position: 0.0, velocity: 0.0,
        })
    }
}

impl Node for MotorController {
    fn name(&self) -> &str { "MotorController" }

    fn tick(&mut self) {
        let dt = 0.02; // 50 Hz
        if let Some(cmd) = self.commands.recv() {
            let error = cmd.velocity - self.velocity;
            self.velocity += error.clamp(-cmd.max_torque, cmd.max_torque) * dt;
        }
        self.position += self.velocity * dt;
        self.state_pub.send(MotorState {
            position: self.position, velocity: self.velocity,
            torque: 0.0, timestamp: 0.0,
        });
    }

    // SAFETY: always zero velocity on shutdown to prevent runaway
    fn shutdown(&mut self) -> Result<()> {
        self.velocity = 0.0;
        self.state_pub.send(MotorState {
            position: self.position, velocity: 0.0, torque: 0.0, timestamp: 0.0,
        });
        eprintln!("Motor stopped at position {:.2} rad", self.position);
        Ok(())
    }
}

Step 5: The State Estimator (NEW)

This node fuses IMU and motor data to estimate the robot's 2D pose. It subscribes to TWO topics and publishes ONE — showing how nodes compose:

struct StateEstimator {
    imu_sub: Topic<ImuData>,
    motor_sub: Topic<MotorState>,
    pose_pub: Topic<RobotPose>,
    pose: RobotPose,
}

impl StateEstimator {
    fn new() -> Result<Self> {
        Ok(Self {
            imu_sub: Topic::new("imu.data")?,
            motor_sub: Topic::new("motor.state")?,
            pose_pub: Topic::new("robot.pose")?,
            pose: RobotPose::default(),
        })
    }
}

impl Node for StateEstimator {
    fn name(&self) -> &str { "StateEstimator" }

    fn tick(&mut self) {
        let dt = 0.01; // 100 Hz

        // Read IMU for heading changes
        if let Some(imu) = self.imu_sub.recv() {
            self.pose.heading += imu.gyro_yaw * dt as f32;
            self.pose.timestamp = imu.timestamp;
        }

        // Read motor state for forward motion
        if let Some(motor) = self.motor_sub.recv() {
            let speed = motor.velocity * 0.1; // scale to m/s
            self.pose.x += speed * self.pose.heading.cos() * dt as f32;
            self.pose.y += speed * self.pose.heading.sin() * dt as f32;
        }

        self.pose_pub.send(self.pose);
    }

    fn shutdown(&mut self) -> Result<()> {
        eprintln!(
            "StateEstimator final pose: ({:.2}, {:.2}) heading={:.2} rad",
            self.pose.x, self.pose.y, self.pose.heading
        );
        Ok(())
    }
}

In production you'd use an Extended Kalman Filter (EKF) instead of simple integration. This simplified estimator demonstrates multi-topic composition.

Step 6: The Commander

struct Commander {
    publisher: Topic<MotorCommand>,
    tick_count: u64,
}

impl Commander {
    fn new() -> Result<Self> {
        Ok(Self { publisher: Topic::new("motor.cmd")?, tick_count: 0 })
    }
}

impl Node for Commander {
    fn name(&self) -> &str { "Commander" }

    fn tick(&mut self) {
        let t = self.tick_count as f64 * 0.1; // 10 Hz
        self.publisher.send(MotorCommand {
            velocity: 1.0 * (t * 0.3).sin() as f32,
            max_torque: 10.0,
        });
        self.tick_count += 1;
    }

    fn shutdown(&mut self) -> Result<()> {
        self.publisher.send(MotorCommand { velocity: 0.0, max_torque: 0.0 });
        eprintln!("Commander shutting down, sent zero-velocity command.");
        Ok(())
    }
}

Step 7: Wire the Complete System

Multi-rate scheduling — each node runs at the rate appropriate for its responsibility:

fn main() -> Result<()> {
    eprintln!("Robot integration demo starting...\n");
    eprintln!("Open another terminal and run: horus monitor\n");

    let mut scheduler = Scheduler::new();

    // Order: sensors (0) → commander (1) → controller (2) → estimator (3)
    // Producers publish before consumers read.

    scheduler.add(ImuSensor::new()?)
        .order(0)
        .rate(100_u64.hz())  // sensor: high frequency for accuracy
        .build()?;

    scheduler.add(Commander::new()?)
        .order(1)
        .rate(10_u64.hz())   // commands: change slowly
        .build()?;

    scheduler.add(MotorController::new()?)
        .order(2)
        .rate(50_u64.hz())   // actuator: moderate update rate
        .build()?;

    scheduler.add(StateEstimator::new()?)
        .order(3)
        .rate(100_u64.hz())  // estimator: matches fastest sensor
        .build()?;

    scheduler.run()
}

Step 8: Run and Monitor

horus run

You should see:

Robot integration demo starting...

Open another terminal and run: horus monitor

The system runs silently — the estimator publishes pose but nothing prints it. Open a second terminal to see the live pose data:

horus topic echo robot.pose

You should see pose updates streaming at 100 Hz. Open a third terminal for the monitor dashboard:

horus monitor

You should see all 4 nodes with their tick rates, all topics with message counts, and system health metrics. Press Ctrl+C in the first terminal to stop — you should see shutdown messages from all nodes.

Key Takeaways

  • Multi-node composition — 4 nodes working together through shared topics
  • Multi-rate scheduling — .rate(100_u64.hz()), .rate(50_u64.hz()), .rate(10_u64.hz()) on different nodes
  • Data fusion — a single node subscribes to multiple topics and publishes a fused result
  • Execution order — sensors (0) before controllers (2) before estimators (3)
  • System monitoring — horus monitor and horus topic echo for live inspection

Next Steps

See Also