[package]
name = "ros2-bridge"
version = "0.1.0"
description = "Bridge horus topics to ROS2"

use horus::prelude::*;
use serde::{Deserialize, Serialize};

/// Lightweight bridge payload — serialized for cross-framework transport
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct BridgePacket {
    topic_id: u32,       // which topic this came from
    timestamp_ns: u64,
    linear: f32,         // for cmd_vel
    angular: f32,
}

// ── Bridge Node ─────────────────────────────────────────────

struct BridgeOutNode {
    cmd_sub: Topic<CmdVel>,
    imu_sub: Topic<Imu>,
    bridge_pub: Topic<BridgePacket>,
    tick_count: u64,
}

impl BridgeOutNode {
    fn new() -> Result<Self> {
        Ok(Self {
            cmd_sub: Topic::new("cmd_vel")?,
            imu_sub: Topic::new("imu.raw")?,
            bridge_pub: Topic::new("bridge.out")?,
            tick_count: 0,
        })
    }
}

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

    fn tick(&mut self) {
        self.tick_count += 1;

        // IMPORTANT: always recv() every tick to drain buffers
        if let Some(cmd) = self.cmd_sub.recv() {
            self.bridge_pub.send(BridgePacket {
                topic_id: 1, // cmd_vel
                timestamp_ns: self.tick_count * 20_000_000, // 50Hz
                linear: cmd.linear,
                angular: cmd.angular,
            });
        }

        // Drain IMU even if we don't bridge every reading
        if let Some(_imu) = self.imu_sub.recv() {
            // Bridge IMU at lower rate if needed
        }
    }
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new();

    // Execution order: bridge reads horus topics and publishes bridge packets
    scheduler.add(BridgeOutNode::new()?)
        .order(90)              // runs after all horus nodes
        .rate(50_u64.hz())      // 50Hz bridge rate — RViz doesn't need 1kHz
        .build()?;

    scheduler.run()
}

#!/usr/bin/env python3
"""
ROS2 node that reads horus bridge packets and republishes as ROS2 messages.
Run this in a separate process with ROS2 sourced.
"""
import horus
# import rclpy
# from geometry_msgs.msg import Twist

# ros_pub = create_publisher(Twist, '/cmd_vel', 10)

def bridge_in_tick(node):
    # IMPORTANT: always call recv() every tick
    packet = node.recv("bridge.out")
    if packet is None:
        return

    # Convert horus BridgePacket to ROS2 Twist
    # twist = Twist()
    # twist.linear.x = packet.linear
    # twist.angular.z = packet.angular
    # ros_pub.publish(twist)
    pass

if __name__ == "__main__":
    bridge_in = horus.Node(name="BridgeIn", tick=bridge_in_tick,
                           subs=["bridge.out"], rate=50)
    horus.run(bridge_in)

  Note: 'BridgeOut' is real-time (from .rate()). Budget 16.0ms, deadline 19.0ms were derived, and sustained misses will reduce its rate and then isolate it. Set .budget()/.deadline() to choose them, or an execution class (.compute(), .async_io()) to opt out.
Added RT node 'BridgeOut' with priority 90 at 50.0Hz
Initialized node 'BridgeOut'
Dependency graph (ready-dispatch mode): 1 steps from .order() tiers (no topic metadata yet)
Starting RT executor with 1 RT nodes on dedicated thread
^C
Ctrl+C received! Shutting down HORUS scheduler...
Shutdown node 'BridgeOut' successfully
Scheduler shutdown complete