isaac_sim_bridge/camera/
info.rs

1// SPDX-License-Identifier: MPL-2.0
2use std::sync::OnceLock;
3
4use crate::channel::{channel_singleton, Channel};
5use crate::ffi::CameraInfoMeta;
6use crate::sensor::Sensor;
7
8/// Type-level marker for the camera-info (calibration metadata) channel.
9pub struct CameraInfo;
10
11impl Sensor for CameraInfo {
12    const NAME: &'static str = "camera_info";
13}
14
15/// One camera-info dispatch. Bundles the matrix and distortion slices
16/// alongside the small `CameraInfoMeta` so consumer callbacks don't
17/// have to thread eight separate parameters.
18#[allow(missing_docs)]
19pub struct CameraInfoFrame<'a> {
20    pub frame_id: &'a str,
21    pub distortion_model: &'a str,
22    pub projection_type: &'a str,
23    pub k: &'a [f64],
24    pub r: &'a [f64],
25    pub p: &'a [f64],
26    pub distortion: &'a [f32],
27    pub meta: &'a CameraInfoMeta,
28}
29
30pub type Callback = Box<dyn Fn(&str, &CameraInfoFrame<'_>) + Send + Sync + 'static>;
31
32#[unsafe(no_mangle)]
33pub extern "C" fn isaac_sim_bridge_channel_camera_info() -> *const Channel<Callback> {
34    static SLOT: OnceLock<Box<Channel<Callback>>> = OnceLock::new();
35    channel_singleton(&SLOT)
36}
37
38fn channel() -> &'static Channel<Callback> {
39    unsafe { &*isaac_sim_bridge_channel_camera_info() }
40}
41
42/// Register a callback to receive every camera-info frame the bridge dispatches.
43/// The closure runs on the bridge thread; keep it bounded.
44pub fn register_camera_info_consumer<F>(cb: F)
45where
46    F: Fn(&str, &CameraInfoFrame<'_>) + Send + Sync + 'static,
47{
48    channel().register(Box::new(cb));
49}
50
51/// Fan out a single camera-info frame to all registered consumers.
52pub fn dispatch_camera_info(source_id: &str, frame: &CameraInfoFrame<'_>) {
53    channel().for_each(|cb| cb(source_id, frame));
54}
55
56/// Number of currently registered camera-info consumers.
57pub fn camera_info_consumer_count() -> usize {
58    channel().count()
59}
60
61/// Entry point called by the C++ bridge on each OmniGraph tick. Bundles the
62/// matrix slices into a `CameraInfoFrame` and calls `dispatch_camera_info`.
63#[allow(clippy::too_many_arguments)]
64pub fn forward_camera_info(
65    source_id: &str,
66    frame_id: &str,
67    distortion_model: &str,
68    projection_type: &str,
69    k: &[f64],
70    r: &[f64],
71    p: &[f64],
72    distortion: &[f32],
73    meta: &CameraInfoMeta,
74) {
75    log::debug!(
76        "[isaac-sim-rs] forward_camera_info: source='{}' frame='{}' wxh={}x{} k={} r={} p={} d={} model='{}' proj='{}'",
77        source_id,
78        frame_id,
79        meta.width,
80        meta.height,
81        k.len(),
82        r.len(),
83        p.len(),
84        distortion.len(),
85        distortion_model,
86        projection_type,
87    );
88    let frame = CameraInfoFrame {
89        frame_id,
90        distortion_model,
91        projection_type,
92        k,
93        r,
94        p,
95        distortion,
96        meta,
97    };
98    dispatch_camera_info(source_id, &frame);
99}
100
101#[cfg(test)]
102mod tests {
103    use super::*;
104    use std::sync::atomic::{AtomicUsize, Ordering};
105    use std::sync::Arc;
106
107    fn fake_meta(w: i32, h: i32) -> CameraInfoMeta {
108        CameraInfoMeta {
109            width: w,
110            height: h,
111            timestamp_ns: 0,
112        }
113    }
114
115    #[test]
116    fn registered_consumer_receives_dispatch_with_source() {
117        let count = Arc::new(AtomicUsize::new(0));
118        let count_clone = Arc::clone(&count);
119        let n_baseline = camera_info_consumer_count();
120
121        register_camera_info_consumer(move |src, frame| {
122            assert_eq!(src, "/World/Camera");
123            assert_eq!(frame.frame_id, "sim_camera");
124            assert_eq!(frame.k.len(), 9);
125            assert_eq!(frame.p.len(), 12);
126            assert_eq!(frame.meta.width, 640);
127            assert_eq!(frame.meta.height, 480);
128            assert_eq!(frame.distortion_model, "plumb_bob");
129            count_clone.fetch_add(1, Ordering::SeqCst);
130        });
131
132        assert_eq!(camera_info_consumer_count(), n_baseline + 1);
133
134        let k = [500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0];
135        let r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0];
136        let p = [
137            500.0, 0.0, 320.0, 0.0, 0.0, 500.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0,
138        ];
139        let d = [0.0_f32, 0.0, 0.0, 0.0, 0.0];
140        let meta = fake_meta(640, 480);
141        forward_camera_info(
142            "/World/Camera",
143            "sim_camera",
144            "plumb_bob",
145            "pinhole",
146            &k,
147            &r,
148            &p,
149            &d,
150            &meta,
151        );
152
153        assert_eq!(count.load(Ordering::SeqCst), 1);
154    }
155}
isaac_sim_bridge/camera/info.rs

isaac_sim_bridge/camera/
info.rs
