clankers-cli 0.1.6

use clankers::prelude::*;

/// Frames the built-in producer emits before the node exits.
const FRAMES: u32 = 10;

/// How long to wait for a frame before giving up, so the node can never hang.
const FRAME_TIMEOUT: Duration = Duration::from_secs(2);

#[clankers::node]
async fn main(ctx: RobotContext) -> RobotResult<()> {
    let node = RobotNode::new(ctx.node_name().as_str()).await?;

    // Subscribe before anything publishes: the bus delivers only to receivers
    // that already exist, so a subscriber created later misses earlier frames.
    let mut frames = node
        .subscribe::<ImageMsg>("/frames", QosProfile::sensor_data())
        .await?;
    let detections = node
        .publish::<DetectionArray>("/detections", QosProfile::default())
        .await?;

    // Stand-in for a camera driver, so `clankers run` does something on its own.
    // Delete this once a real publisher — or `clankers replay` — feeds /frames.
    let frames_pub = node
        .publish::<ImageMsg>("/frames", QosProfile::sensor_data())
        .await?;
    tokio::spawn(async move {
        for _ in 0..FRAMES {
            let frame = ImageMsg::new(2, 2, vec![0u8; 2 * 2 * 3]);
            if frames_pub.publish(frame).await.is_err() {
                break;
            }
            tokio::time::sleep(Duration::from_millis(100)).await;
        }
    });

    tracing::info!(frames = FRAMES, "reading /frames, publishing to /detections");

    let mut count = 0u32;
    while count < FRAMES {
        // Never await a sensor unbounded: a silent hang is the least
        // debuggable failure a robot node can have.
        match tokio::time::timeout(FRAME_TIMEOUT, frames.next()).await {
            Ok(Some(frame)) => {
                // Carry the frame's stamp through, so a detection can always be
                // traced back to the image it came from.
                detections
                    .publish(DetectionArray {
                        stamp_nanos: frame.stamp_nanos,
                        frame_id: frame.frame_id.clone(),
                        detections: vec![Detection {
                            class_id: 0,
                            class_name: "hello".to_string(),
                            score: 1.0,
                            x: 0.0,
                            y: 0.0,
                            width: frame.width as f32,
                            height: frame.height as f32,
                        }],
                    })
                    .await?;
                count += 1;
                tracing::info!(count, "published detections");
            }
            Ok(None) => break,
            Err(_) => {
                tracing::warn!("no frame on /frames for {FRAME_TIMEOUT:?}; stopping");
                break;
            }
        }
    }

    tracing::info!(frames = count, "node finished");
    Ok(())
}