clankers-cli 0.1.6

Command-line interface for clankeRS
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

use clankers::prelude::*;

/// How long to wait for a camera frame before giving up. Bounding the wait
/// turns "publisher never started" from a silent hang into a diagnostic.
const FRAME_TIMEOUT: Duration = Duration::from_secs(10);

#[clankers::node]
async fn main(ctx: RobotContext) -> RobotResult<()> {
    let node = RobotNode::new(ctx.node_name().as_str()).await?;
    let mut camera = node
        .subscribe::<ImageMsg>("/camera/image_raw", QosProfile::sensor_data())
        .await?;
    let detections_pub = node
        .publish::<DetectionArray>("/detections", QosProfile::default())
        .await?;

    let model_cfg = ctx.model_config("detector")?;
    let model_path = ctx.resolve_path(&model_cfg.path);

    let mut model = ModelBuilder::from_config(&model_cfg, model_path.clone())?.build()?;

    let input_name = model.engine().input_specs()[0].name.clone();

    tracing::info!("clankeRS node: {}", ctx.node_name());
    tracing::info!("Loaded model {}", model_path.display());

    loop {
        let frame = match tokio::time::timeout(FRAME_TIMEOUT, camera.next()).await {
            Ok(Some(frame)) => frame,
            Ok(None) => break,
            Err(_) => {
                tracing::warn!(
                    "no frames on /camera/image_raw for {FRAME_TIMEOUT:?}; is a publisher \
                     running? feed this node with `clankers replay <log.mcap>`"
                );
                break;
            }
        };

        let tensor = ImageTensor::from_ros_msg(&frame)?
            .resize(224, 224)?
            .normalize_imagenet()?
            .to_nchw()?;

        let shape = tensor.nchw_shape();
        let view = tensor.as_nchw_view(&shape)?;
        let outputs = model.run_named([(input_name.as_str(), view)])?;
        let detections = output_to_detections(
            &outputs
                .first()
                .ok_or_else(|| RobotError::Model("model produced no outputs".into()))?
                .to_f32_vec()?,
        );

        if let Some(stats) = model.stats() {
            tracing::debug!(
                latency_ms = stats.latency_ms(),
                clankers_copies = stats.clankers_copies,
                "inference"
            );
        }

        detections_pub
            .publish(DetectionArray {
                stamp_nanos: Timestamp::now().as_nanos(),
                frame_id: frame.frame_id.clone(),
                detections,
            })
            .await?;
    }

    Ok(())
}

fn output_to_detections(output: &[f32]) -> Vec<Detection> {
    output
        .iter()
        .enumerate()
        .filter(|(_, &score)| score > 0.1)
        .map(|(class_id, &score)| Detection {
            class_id: class_id as u32,
            class_name: format!("class_{class_id}"),
            score,
            x: 0.0,
            y: 0.0,
            width: 1.0,
            height: 1.0,
        })
        .collect()
}