multithreaded.cpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2026 William Leong

/**
 * multithreaded.cpp -- Robotics-style intra-process pipeline
 *
 * Demonstrates the typical robotics use case: multiple threads
 * communicate through named topics without sharing state directly.
 *
 * Pipeline:
 *   1. An IMU driver thread publishes orientation data at ~100 Hz.
 *   2. A state estimator thread subscribes to IMU data, fuses it
 *      into an estimated pose, and publishes the result.
 *   3. A controller thread subscribes to the pose estimate and
 *      publishes velocity commands.
 *   4. The main thread subscribes to cmd_vel and prints the commands
 *      (standing in for a motor driver).
 *
 * All threads share only the Bus instance -- no mutexes, condition
 * variables, or shared queues appear in user code.
 */

#include <atomic>
#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>

#include "lightmessagebus.h"

// ----- message types --------------------------------------------------------

struct ImuData
{
    double ax, ay, az; // accelerometer  [m/s^2]
    double gx, gy, gz; // gyroscope      [rad/s]
    int sequence;
};

struct PoseEstimate
{
    double x, y, theta; // 2-D pose
    int sequence;
};

struct CmdVel
{
    double linear;  // m/s
    double angular; // rad/s
};

// ----- main -----------------------------------------------------------------

int main()
{
    lightmessage::Bus bus;
    std::atomic<bool> running { true };

    // --- motor driver (main thread) -----------------------------------------
    // Subscribes to velocity commands -- in a real robot this would write
    // to the motor controller.
    auto cmdSub = bus.getSubscription<CmdVel>("motor_driver",
                                              "cmd_vel",
                                              [](const CmdVel& cmd) {
                                                  std::cout << "[motor]      linear=" << cmd.linear
                                                            << "  angular=" << cmd.angular << std::endl;
                                              });

    // --- controller thread --------------------------------------------------
    // Subscribes to pose estimates, computes and publishes velocity commands.
    auto ctrlSub = bus.getSubscription<PoseEstimate>("controller",
                                                     "pose",
                                                     [&bus](const PoseEstimate& pose)
                                                     {
                                                         // Simple proportional controller: drive toward a goal at (5,
                                                         // 0).
                                                         double dx = 5.0 - pose.x;
                                                         double dy = 0.0 - pose.y;
                                                         double dist = std::sqrt(dx * dx + dy * dy);
                                                         double angle = std::atan2(dy, dx) - pose.theta;

                                                         CmdVel cmd;
                                                         cmd.linear = std::min(dist, 1.0); // cap at 1 m/s
                                                         cmd.angular =
                                                             std::max(-1.0, std::min(angle, 1.0)); // cap at 1 rad/s
                                                         bus.publish("cmd_vel", cmd);
                                                     });

    // --- state estimator thread ---------------------------------------------
    // Subscribes to IMU data, publishes a (very simplified) pose estimate.
    auto estSub = bus.getSubscription<ImuData>("estimator",
                                               "imu/data",
                                               [&bus](const ImuData& imu)
                                               {
                                                   // Toy integration -- real code would use a Kalman filter.
                                                   static double x = 0, y = 0, theta = 0;
                                                   double dt = 0.01; // matches the 100 Hz publish rate

                                                   theta += imu.gz * dt;
                                                   x += imu.ax * dt * dt * 0.5;
                                                   y += imu.ay * dt * dt * 0.5;

                                                   PoseEstimate pose;
                                                   pose.x = x;
                                                   pose.y = y;
                                                   pose.theta = theta;
                                                   pose.sequence = imu.sequence;

                                                   if (imu.sequence % 10 == 0) // publish at 10 Hz
                                                       bus.publish("pose", pose);
                                               });

    // --- IMU driver thread --------------------------------------------------
    // Simulates a sensor sampling at ~100 Hz.
    std::thread imuDriver(
        [&bus, &running]()
        {
            int seq = 0;
            while (running)
            {
                ImuData imu;
                imu.ax = 0.1; // gentle forward acceleration
                imu.ay = 0.0;
                imu.az = 9.81;
                imu.gx = 0.0;
                imu.gy = 0.0;
                imu.gz = 0.01 * std::sin(seq * 0.01); // small yaw drift
                imu.sequence = seq++;

                bus.publish("imu/data", imu);
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
        });

    // Let the pipeline run, then shut down.
    std::this_thread::sleep_for(std::chrono::seconds(3));
    running = false;
    imuDriver.join();

    std::cout << "\nDone." << std::endl;
    return 0;
}