Telemetry Logger (C++)

A low-priority observer node that subscribes to all robot state topics and logs summary telemetry at a configurable rate. Useful for debugging, monitoring, and post-run analysis without affecting real-time performance.

Problem

You need to see what your robot is doing: where it is, what it is commanding, what its sensors read. But logging at full sensor rate (100-200 Hz) floods the terminal, wastes CPU on string formatting, and can cause real-time nodes to miss deadlines if the logger takes too long. You need a lightweight telemetry tap that runs at the lowest priority and logs at a human-readable rate.

How It Works

The telemetry logger runs at the lowest execution priority (order(100)) so it never delays real-time nodes. It subscribes to key topics (odometry, velocity commands, IMU) and samples them at a configurable interval.

Rate-limited logging: Instead of logging every tick, the node counts ticks and only formats output every Nth tick. At 100 Hz with log_every_n = 100, you get 1 Hz logging. This approach is cheaper than running the node at 1 Hz because:

  1. The node still drains subscriber buffers every tick (prevents stale data buildup)
  2. No separate clock or timer needed
  3. The log interval is always an integer multiple of the tick rate

Why horus::log::info() instead of printf? The horus log system is non-blocking and thread-safe. Output appears in horus log CLI, can be filtered by tag, and is captured by the blackbox for post-mortem replay. printf blocks on stdout and can cause deadline misses in nodes that share the same terminal.

When To Use

  • Debugging any multi-node robot system
  • Monitoring robot state during integration testing
  • Recording summary telemetry for post-run analysis

Prerequisites

Complete Code

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

class TelemetryLogger : public horus::Node {
public:
    TelemetryLogger(int log_every_n = 100)
        : Node("telemetry"), log_every_n_(log_every_n)
    {
        // Subscribe to all topics we want to monitor
        odom_sub_  = subscribe<horus::msg::Odometry>("odom");
        cmd_sub_   = subscribe<horus::msg::CmdVel>("cmd_vel");
        imu_sub_   = subscribe<horus::msg::Imu>("imu.data");
        estop_sub_ = subscribe<horus::msg::EmergencyStop>("emergency.stop");
    }

    void tick() override {
        // ── Always drain subscriber buffers ─────────────────────────
        // Even when not logging, drain to prevent stale data buildup.
        // This ensures the next log entry shows the *latest* data,
        // not data that has been sitting in the buffer for seconds.
        auto odom  = odom_sub_->recv();
        auto cmd   = cmd_sub_->recv();
        auto imu   = imu_sub_->recv();
        auto estop = estop_sub_->recv();

        // Cache latest values for logging
        if (odom) {
            last_x_     = odom->get()->pose.x;
            last_y_     = odom->get()->pose.y;
            last_theta_ = odom->get()->pose.theta;
            odom_alive_ = true;
        }
        if (cmd) {
            last_linear_  = cmd->get()->linear;
            last_angular_ = cmd->get()->angular;
            cmd_alive_ = true;
        }
        if (imu) {
            last_accel_z_ = imu->get()->linear_acceleration[2];
            last_gyro_z_  = imu->get()->angular_velocity[2];
            imu_alive_ = true;
        }
        if (estop) {
            estop_engaged_ = (estop->get()->engaged != 0);
        }

        // ── Rate-limited logging ────────────────────────────────────
        tick_count_++;
        if (tick_count_ % log_every_n_ != 0) return;

        // ── Format and publish summary ──────────────────────────────
        char buf[256];
        std::snprintf(buf, sizeof(buf),
            "pos=(%.2f, %.2f) hdg=%.1f deg  "
            "cmd=(%.2f m/s, %.2f rad/s)  "
            "az=%.2f m/s^2  gz=%.2f rad/s  "
            "estop=%s",
            last_x_, last_y_, last_theta_ * 180.0 / M_PI,
            last_linear_, last_angular_,
            last_accel_z_, last_gyro_z_,
            estop_engaged_ ? "ACTIVE" : "clear");
        horus::log::info("telemetry", buf);

        // ── Health check: warn on missing topics ────────────────────
        if (!odom_alive_) horus::log::warn("telemetry", "No odom data received");
        if (!cmd_alive_)  horus::log::warn("telemetry", "No cmd_vel data received");
        if (!imu_alive_)  horus::log::warn("telemetry", "No IMU data received");

        // Record to blackbox at lower rate (every 10th log = 0.1 Hz)
        if (tick_count_ % (log_every_n_ * 10) == 0) {
            char bb[128];
            std::snprintf(bb, sizeof(bb),
                "pos=(%.2f,%.2f) cmd=%.2f estop=%d",
                last_x_, last_y_, last_linear_,
                estop_engaged_ ? 1 : 0);
            horus::blackbox::record("telemetry", bb);
        }
    }

    void enter_safe_state() override {
        // Logger has no actuators, but record the event for post-mortem
        horus::blackbox::record("telemetry",
            "System entered safe state — final snapshot: "
            "see preceding log entries for last known state");
        horus::log::warn("telemetry", "Safe state activated by scheduler");
    }

private:
    horus::Subscriber<horus::msg::Odometry>*      odom_sub_;
    horus::Subscriber<horus::msg::CmdVel>*        cmd_sub_;
    horus::Subscriber<horus::msg::Imu>*           imu_sub_;
    horus::Subscriber<horus::msg::EmergencyStop>* estop_sub_;

    // Configurable log interval
    int log_every_n_;

    // Cached latest values
    double last_x_ = 0, last_y_ = 0, last_theta_ = 0;
    float last_linear_ = 0, last_angular_ = 0;
    double last_accel_z_ = 0, last_gyro_z_ = 0;
    bool estop_engaged_ = false;

    // Health tracking
    bool odom_alive_ = false;
    bool cmd_alive_  = false;
    bool imu_alive_  = false;

    int tick_count_ = 0;
};

int main() {
    horus::Scheduler sched;
    sched.tick_rate(100_hz).name("telemetry_demo");

    // Log every 100 ticks = 1 Hz output at 100 Hz tick rate
    TelemetryLogger logger(100);
    sched.add(logger)
        .order(100)                    // lowest priority — never delay real-time nodes
        .on_miss(horus::Miss::Skip)    // skip if overrun — logging is expendable
        .build();

    sched.spin();
}

Common Errors

SymptomCauseFix
Log output floods terminallog_every_n too smallIncrease to 100+ (1 Hz at 100 Hz tick)
Logged data is staleNot draining subscribers every tickCall recv() every tick, not just on log ticks
Real-time nodes miss deadlinesLogger at too high priority or printf blocking stdoutSet order(100) and use horus::log::info()
Topic shows "no data" foreverTopic name mismatch between publisher and subscriberVerify topic names match exactly (dots, not slashes)
Blackbox entries too largeLogging too much detail in blackboxKeep blackbox entries short; detailed logging goes to horus::log
Memory grows over timeSubscriber buffers not drainedEnsure recv() is called every tick for all subscribers

Design Decisions

ChoiceRationale
order(100) — lowest priorityLogging must never delay safety, control, or sensor nodes
Miss::Skip policyLogging is expendable — skipping a log entry is always acceptable
Drain every tick, log every NthFresh data on every log entry; no stale buffer accumulation
horus::log::info() over printfNon-blocking, thread-safe, filterable, captured by blackbox
Cache values between log entriesAvoid repeated null checks; always have something to log
Health check warningsSurfaces broken topics early — one of the most useful debugging aids
Blackbox recording at 0.1 HzLightweight persistent record for post-mortem analysis without overhead

Variations

CSV file logging — write to a file for offline analysis:

void tick() override {
    // ... drain subscribers and cache values ...
    if (tick_count_ % log_every_n_ != 0) return;

    // Append to CSV file
    FILE* f = std::fopen("/tmp/telemetry.csv", "a");
    if (f) {
        std::fprintf(f, "%d,%.4f,%.4f,%.4f,%.4f,%.4f\n",
            tick_count_, last_x_, last_y_, last_theta_,
            last_linear_, last_angular_);
        std::fclose(f);
    }
}

Per-topic rate monitoring — track publish rates to detect stalled sensors:

int odom_count_ = 0, cmd_count_ = 0, imu_count_ = 0;

void tick() override {
    if (odom_sub_->recv()) odom_count_++;
    if (cmd_sub_->recv())  cmd_count_++;
    if (imu_sub_->recv())  imu_count_++;

    if (tick_count_ % 100 == 0) {
        char buf[128];
        std::snprintf(buf, sizeof(buf),
            "rates: odom=%d Hz  cmd=%d Hz  imu=%d Hz",
            odom_count_, cmd_count_, imu_count_);
        horus::log::info("telemetry", buf);
        odom_count_ = cmd_count_ = imu_count_ = 0;
    }
}

Conditional detail logging — verbose output only when anomalies detected:

if (estop_engaged_) {
    // Log extra detail during E-stop events
    horus::log::warn("telemetry", "E-stop active — dumping full state");
    // ... log every subscriber value in detail ...
} else {
    // Normal summary only
    horus::log::info("telemetry", summary_buf);
}

Key Takeaways

  • The telemetry logger runs at order(100) with Miss::Skip — it must never interfere with real-time nodes
  • Drain all subscriber buffers every tick, even when not logging, to prevent stale data
  • Use horus::log::info() instead of printf — it is non-blocking and captured by the blackbox
  • Health check warnings ("no odom data received") are one of the most useful debugging features
  • Rate-limited logging (every Nth tick) is simpler and cheaper than running a separate low-rate node