Tutorial 3: Full Robot System (C++)

Name: HORUS
Availability: InStock

Build a complete mobile robot with 6 nodes, coordinate transforms, runtime parameters, safety monitoring, and telemetry — all in one scheduler.

What You'll Build

┌────────────┐   ┌────────────┐   ┌────────────┐
│  LiDAR     │──→│ Controller │──→│  Motors    │
│  (10 Hz)   │   │  (100 Hz)  │   │  (100 Hz)  │
└────────────┘   └────────────┘   └────────────┘
                      ↑  ↑
┌────────────┐        │  │       ┌────────────┐
│  IMU       │────────┘  └──────│  Safety    │
│  (200 Hz)  │                   │  (50 Hz)   │
└────────────┘                   └────────────┘
                                 ┌────────────┐
                                 │  Telemetry │
                                 │  (1 Hz)    │
                                 └────────────┘

What You'll Learn

6-node pipeline with different rates and execution orders
Coordinate transforms (lidar → base_link → world)
Runtime parameters for live tuning
Safety node with emergency stop
Telemetry logging at 1 Hz
Multi-rate execution in a single scheduler

Prerequisites

Completed Tutorial 1 and Tutorial 2

Step 1: Create the Project

horus new full_robot --lang cpp
cd full_robot

Step 2: Write the System

Replace src/main.cpp:

#include <horus/horus.hpp>
#include <algorithm>
#include <cmath>
#include <cstdio>
#include <atomic>
using namespace horus::literals;

// ── Shared safety flag ──────────────────────────────────────────────────
static std::atomic<bool> estop_active{false};

// ── 1. LiDAR Driver (10 Hz) ────────────────────────────────────────────

class LidarDriver : public horus::Node {
public:
    LidarDriver() : Node("lidar_driver") {
        scan_pub_ = advertise<horus::msg::LaserScan>("lidar.scan");
    }

    void tick() override {
        if (++tick_ % 20 != 0) return;  // 10 Hz from 200 Hz scheduler

        auto scan = scan_pub_->loan();
        // Simulate: wall at 1.5m with gap at 90°
        for (int i = 0; i < 360; i++) {
            if (i > 85 && i < 95) {
                scan->ranges[i] = 5.0f;  // gap (doorway)
            } else {
                float dist = 1.5f + 0.2f * std::sin(i * 0.05f);
                scan->ranges[i] = dist;
            }
        }
        scan->angle_min = 0.0f;
        scan->angle_max = 6.28318f;
        scan_pub_->publish(std::move(scan));
    }

private:
    horus::Publisher<horus::msg::LaserScan>* scan_pub_;
    int tick_ = 0;
};

// ── 2. IMU Driver (200 Hz — every tick) ─────────────────────────────────

class ImuDriver : public horus::Node {
public:
    ImuDriver() : Node("imu_driver") {
        imu_pub_ = advertise<horus::msg::Imu>("imu.data");
    }

    void tick() override {
        horus::msg::Imu imu{};
        imu.linear_acceleration[2] = 9.81;  // gravity
        imu.angular_velocity[2] = 0.01;     // slight yaw drift
        imu.orientation[3] = 1.0;           // identity quaternion w
        imu_pub_->send(imu);
    }

private:
    horus::Publisher<horus::msg::Imu>* imu_pub_;
};

// ── 3. Controller (100 Hz) ──────────────────────────────────────────────

class Controller : public horus::Node {
public:
    Controller(horus::Params& params) : Node("controller"), params_(params) {
        scan_sub_ = subscribe<horus::msg::LaserScan>("lidar.scan");
        imu_sub_  = subscribe<horus::msg::Imu>("imu.data");
        cmd_pub_  = advertise<horus::msg::CmdVel>("cmd_vel");
    }

    void tick() override {
        if (++tick_ % 2 != 0) return;  // 100 Hz from 200 Hz
        if (estop_active.load()) return;

        double max_speed = params_.get<double>("max_speed", 0.3);
        double safe_dist = params_.get<double>("safe_distance", 0.5);

        // Read lidar (latest scan)
        float min_range = 999.0f;
        int min_idx = 0;
        if (auto scan = scan_sub_->recv()) {
            for (int i = 0; i < 360; i++) {
                if (scan->get()->ranges[i] > 0.01f && scan->get()->ranges[i] < min_range) {
                    min_range = scan->get()->ranges[i];
                    min_idx = i;
                }
            }
        }

        // Read IMU (compensate yaw drift)
        double yaw_rate = 0;
        if (auto imu = imu_sub_->recv()) {
            yaw_rate = imu->get()->angular_velocity[2];
        }

        // Simple obstacle avoidance
        horus::msg::CmdVel cmd{};
        if (min_range < static_cast<float>(safe_dist)) {
            cmd.linear = 0.0f;
            cmd.angular = (min_idx < 180) ? -0.5f : 0.5f;
        } else {
            cmd.linear = static_cast<float>(max_speed);
            cmd.angular = static_cast<float>(-yaw_rate * 0.5);  // drift compensation
        }
        cmd_pub_->send(cmd);
    }

    void enter_safe_state() override {
        horus::msg::CmdVel stop{};
        cmd_pub_->send(stop);
    }

private:
    horus::Subscriber<horus::msg::LaserScan>* scan_sub_;
    horus::Subscriber<horus::msg::Imu>*       imu_sub_;
    horus::Publisher<horus::msg::CmdVel>*     cmd_pub_;
    horus::Params& params_;
    int tick_ = 0;
};

// ── 4. Motor Driver (100 Hz) ────────────────────────────────────────────

class MotorDriver : public horus::Node {
public:
    MotorDriver() : Node("motor_driver") {
        cmd_sub_ = subscribe<horus::msg::CmdVel>("cmd_vel");
        odom_pub_ = advertise<horus::msg::Odometry>("odom");
    }

    void tick() override {
        if (++tick_ % 2 != 0) return;  // 100 Hz
        auto cmd = cmd_sub_->recv();
        if (!cmd) return;

        double v = cmd->get()->linear;
        double w = cmd->get()->angular;
        double dt = 0.01;

        x_ += v * std::cos(theta_) * dt;
        y_ += v * std::sin(theta_) * dt;
        theta_ += w * dt;

        horus::msg::Odometry odom{};
        odom.pose.x = x_;
        odom.pose.y = y_;
        odom.pose.theta = theta_;
        odom_pub_->send(odom);
    }

    void enter_safe_state() override {
        horus::log::warn("motor", "Safe state — motors stopped");
    }

private:
    horus::Subscriber<horus::msg::CmdVel>*  cmd_sub_;
    horus::Publisher<horus::msg::Odometry>* odom_pub_;
    double x_ = 0, y_ = 0, theta_ = 0;
    int tick_ = 0;
};

// ── 5. Safety Monitor (50 Hz) ───────────────────────────────────────────

class SafetyMonitor : public horus::Node {
public:
    SafetyMonitor() : Node("safety_monitor") {
        scan_sub_  = subscribe<horus::msg::LaserScan>("lidar.scan");
        estop_pub_ = advertise<horus::msg::EmergencyStop>("emergency.stop");
    }

    void tick() override {
        if (++tick_ % 4 != 0) return;  // 50 Hz

        auto scan = scan_sub_->recv();
        if (!scan) return;

        bool danger = false;
        for (int i = 0; i < 360; i++) {
            if (scan->get()->ranges[i] > 0.01f && scan->get()->ranges[i] < 0.2f) {
                danger = true;
                break;
            }
        }

        if (danger && !estop_active.load()) {
            estop_active.store(true);
            horus::msg::EmergencyStop estop{};
            estop.engaged = 1;
            estop_pub_->send(estop);
            horus::log::error("safety", "EMERGENCY STOP — object < 20cm");
            horus::blackbox::record("safety", "E-stop: object < 20cm");
        }

        if (!danger && estop_active.load()) {
            estop_active.store(false);
            horus::msg::EmergencyStop clear{};
            clear.engaged = 0;
            estop_pub_->send(clear);
            horus::log::info("safety", "E-stop cleared");
        }
    }

private:
    horus::Subscriber<horus::msg::LaserScan>*   scan_sub_;
    horus::Publisher<horus::msg::EmergencyStop>* estop_pub_;
    int tick_ = 0;
};

// ── 6. Telemetry Logger (1 Hz) ──────────────────────────────────────────

class Telemetry : public horus::Node {
public:
    Telemetry() : Node("telemetry") {
        odom_sub_ = subscribe<horus::msg::Odometry>("odom");
        cmd_sub_  = subscribe<horus::msg::CmdVel>("cmd_vel");
    }

    void tick() override {
        if (++tick_ % 200 != 0) return;  // 1 Hz

        auto odom = odom_sub_->recv();
        auto cmd  = cmd_sub_->recv();

        char buf[128];
        std::snprintf(buf, sizeof(buf), "pos=(%.2f, %.2f) heading=%.1f° cmd=(%.2f, %.2f)",
            odom ? odom->get()->pose.x : 0.0,
            odom ? odom->get()->pose.y : 0.0,
            odom ? odom->get()->pose.theta * 57.3 : 0.0,
            cmd ? cmd->get()->linear : 0.0f,
            cmd ? cmd->get()->angular : 0.0f);
        horus::log::info("telemetry", buf);
    }

private:
    horus::Subscriber<horus::msg::Odometry>* odom_sub_;
    horus::Subscriber<horus::msg::CmdVel>*   cmd_sub_;
    int tick_ = 0;
};

// ── Main ────────────────────────────────────────────────────────────────

int main() {
    // Coordinate transforms
    horus::TransformFrame tf;
    tf.register_frame("world");
    tf.register_frame("base_link", "world");
    tf.register_frame("lidar", "base_link");
    tf.update("lidar", {0.2, 0.0, 0.3}, {0, 0, 0, 1}, 0);

    // Runtime parameters
    horus::Params params;
    params.set("max_speed", 0.3);
    params.set("safe_distance", 0.5);

    // Scheduler
    horus::Scheduler sched;
    sched.tick_rate(200_hz).name("full_robot");

    // Add nodes in execution order
    ImuDriver imu;
    sched.add(imu).order(0).build();                                      // 200 Hz

    LidarDriver lidar;
    sched.add(lidar).order(1).build();                                    // 10 Hz

    SafetyMonitor safety;
    sched.add(safety).order(2).budget(2_ms).on_miss(horus::Miss::Stop).build();  // 50 Hz, critical

    Controller ctrl(params);
    sched.add(ctrl).order(10).budget(5_ms).on_miss(horus::Miss::Skip).build();   // 100 Hz

    MotorDriver motors;
    sched.add(motors).order(20).on_miss(horus::Miss::SafeMode).build();          // 100 Hz

    Telemetry telemetry;
    sched.add(telemetry).order(100).build();                              // 1 Hz

    auto nodes = sched.node_list();
    std::printf("Robot ready: %zu nodes\n", nodes.size());
    for (auto& n : nodes) std::printf("  - %s\n", n.c_str());

    sched.spin();
}

Step 3: Build and Run

horus build && horus run

Step 4: Monitor Everything

horus topic list          # 6+ active topics
horus node list           # 6 running nodes
horus topic echo odom     # watch robot position
horus topic echo cmd_vel  # watch velocity commands
horus log                 # see telemetry + safety events

Key Architecture Decisions

Node	Rate	Order	Miss Policy	Why
IMU	200 Hz	0	default	Fastest sensor, runs every tick
LiDAR	10 Hz	1	default	Mechanical limit of sensor
Safety	50 Hz	2	Stop	If safety can't run, entire system must stop
Controller	100 Hz	10	Skip	Skipping one tick is better than lag
Motors	100 Hz	20	SafeMode	Overrun → stop motors immediately
Telemetry	1 Hz	100	default	Non-critical, best effort

Next Steps

Cross-Language Interop — mix C++, Rust, Python in one system
Guide: Hardware Integration — connect real sensors
Guide: Real-Time C++ — SCHED_FIFO, CPU pinning