Second Application: C++

You've built a Quick Start app. Now build a real robot: a mobile base with lidar, IMU, motor control, coordinate transforms, runtime parameters, and a service.

What You'll Build

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

Features exercised:

  • Multi-rate nodes (10 Hz sensor, 50 Hz controller, 100 Hz IMU)
  • Coordinate transforms (lidar → base_link)
  • Runtime parameters (max_speed, safety_distance)
  • Service (emergency stop)
  • Budget/deadline enforcement

The Code

#include <horus/horus.hpp>
#include <cstdio>
#include <cmath>
#include <atomic>

using namespace horus::literals;

// ── Simulated sensor data ───────────────────────────────────────────────

static int lidar_seq = 0;
static float sim_distance() {
    return 1.0f + 0.5f * std::sin(lidar_seq++ * 0.05f);  // oscillates 0.5-1.5m
}

// ── Shared state ────────────────────────────────────────────────────────

static std::atomic<bool> estop_active{false};

int main() {
    // ── Scheduler ───────────────────────────────────────────────────

    horus::Scheduler sched;
    sched.tick_rate(100_hz);
    sched.name("mobile_base");

    // ── Coordinate transforms ───────────────────────────────────────

    horus::TransformFrame tf;
    tf.register_frame("world");
    tf.register_frame("base_link", "world");
    tf.register_frame("lidar", "base_link");

    // Lidar is 20cm forward, 30cm up from base
    tf.update("lidar", {0.2, 0.0, 0.3}, {0, 0, 0, 1}, 0);

    // ── Runtime parameters ──────────────────────────────────────────

    horus::Params params;
    params.set("max_speed", 0.5);           // m/s
    params.set("safety_distance", 0.3);     // meters
    params.set("controller_gain", 0.8);

    // ── Topics ──────────────────────────────────────────────────────

    horus::Publisher<horus::msg::CmdVel>  lidar_pub("lidar.scan");
    horus::Subscriber<horus::msg::CmdVel> lidar_sub("lidar.scan");

    horus::Publisher<horus::msg::CmdVel>  imu_pub("imu.data");
    horus::Subscriber<horus::msg::CmdVel> imu_sub("imu.data");

    horus::Publisher<horus::msg::CmdVel>  cmd_pub("motor.cmd");
    horus::Subscriber<horus::msg::CmdVel> cmd_sub("motor.cmd");

    // ── Nodes ───────────────────────────────────────────────────────

    // LiDAR driver: 10 Hz, publishes range data
    static int lidar_tick_count = 0;
    sched.add("lidar_driver")
        .order(0)
        .tick([&] {
            // Only publish every 10th tick (10 Hz at 100 Hz scheduler)
            if (++lidar_tick_count % 10 != 0) return;
            auto msg = lidar_pub.loan();
            msg->linear = sim_distance();  // range in meters
            msg->timestamp_ns = static_cast<uint64_t>(lidar_tick_count);
            lidar_pub.publish(std::move(msg));
        })
        .build();

    // IMU: every tick (100 Hz), publishes orientation
    sched.add("imu_driver")
        .order(1)
        .tick([&] {
            auto msg = imu_pub.loan();
            msg->angular = 0.01f;  // simulated yaw rate
            imu_pub.publish(std::move(msg));
        })
        .build();

    // Controller: 50 Hz, reads lidar + IMU, publishes motor commands
    static int ctrl_tick = 0;
    sched.add("controller")
        .order(10)
        .budget(5_ms)
        .on_miss(horus::Miss::Skip)
        .tick([&] {
            if (++ctrl_tick % 2 != 0) return;  // 50 Hz from 100 Hz
            if (estop_active.load()) return;

            // Read parameters
            double max_speed = params.get<double>("max_speed", 0.5);
            double safety_dist = params.get<double>("safety_distance", 0.3);
            double gain = params.get<double>("controller_gain", 0.8);

            // Read lidar
            float range = 999.0f;
            auto scan = lidar_sub.recv();
            if (scan) range = scan->get()->linear;

            // Read IMU
            float yaw_rate = 0.0f;
            auto imu = imu_sub.recv();
            if (imu) yaw_rate = imu->get()->angular;

            // Simple obstacle avoidance
            auto cmd = cmd_pub.loan();
            if (range < static_cast<float>(safety_dist)) {
                cmd->linear = 0.0f;
                cmd->angular = 0.5f;  // turn away
            } else {
                cmd->linear = static_cast<float>(max_speed * gain);
                cmd->angular = -yaw_rate;  // compensate drift
            }
            cmd_pub.publish(std::move(cmd));
        })
        .build();

    // Motor driver: reads commands, "applies" them
    static int motor_tick = 0;
    sched.add("motor_driver")
        .order(20)
        .tick([&] {
            if (++motor_tick % 2 != 0) return;  // 50 Hz
            auto cmd = cmd_sub.recv();
            if (!cmd) return;

            if (motor_tick % 100 == 0) {  // print every 1 second
                std::printf("[motor] linear=%.2f angular=%.2f\n",
                    cmd->get()->linear, cmd->get()->angular);
            }
        })
        .build();

    // ── Verify setup ────────────────────────────────────────────────

    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());

    // Verify transforms
    if (tf.can_transform("lidar", "world")) {
        auto t = tf.lookup("lidar", "world");
        if (t) {
            std::printf("Lidar position: (%.1f, %.1f, %.1f) in world frame\n",
                t->translation[0], t->translation[1], t->translation[2]);
        }
    }

    // Run for 3 seconds
    for (int i = 0; i < 300; i++) {
        sched.tick_once();

        // Simulate parameter change at 1.5 seconds
        if (i == 150) {
            params.set("max_speed", 0.3);
            std::printf("[param] max_speed reduced to 0.3 m/s\n");
        }
    }

    std::printf("Done: 300 ticks completed\n");
    return 0;
}

Build and Run

horus build && horus run

Key Patterns Demonstrated

Multi-Rate Nodes

All nodes share one 100 Hz scheduler. Each node divides internally:

  • LiDAR: ticks every 10th iteration = 10 Hz
  • Controller + Motor: every 2nd iteration = 50 Hz
  • IMU: every iteration = 100 Hz

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}, timestamp);

auto t = tf.lookup("lidar", "world");  // chain: lidar → base → world

Runtime Parameters

horus::Params params;
params.set("max_speed", 0.5);
double speed = params.get<double>("max_speed", 0.0);  // with default
params.set("max_speed", 0.3);  // change at runtime

Budget Enforcement

sched.add("controller")
    .budget(5_ms)                    // must complete within 5ms
    .on_miss(horus::Miss::Skip)     // skip tick if over budget
    .tick([&] { /* ... */ })
    .build();

Next Steps