isaac_sim_bridge/imu/
mod.rs

1// SPDX-License-Identifier: MPL-2.0
2use std::sync::OnceLock;
3
4use crate::channel::{channel_singleton, Channel};
5use crate::ffi::ImuMeta;
6use crate::sensor::Sensor;
7
8/// Type-level marker for the IMU sensor channel. One sample per
9/// dispatch carries linear acceleration, angular velocity, and
10/// orientation packed into [`ImuMeta`] — no variable-sized payload.
11pub struct Imu;
12
13impl Sensor for Imu {
14    const NAME: &'static str = "imu";
15}
16
17pub type Callback = Box<dyn Fn(&str, &str, &ImuMeta) + Send + Sync + 'static>;
18
19#[unsafe(no_mangle)]
20pub extern "C" fn isaac_sim_bridge_channel_imu() -> *const Channel<Callback> {
21    static SLOT: OnceLock<Box<Channel<Callback>>> = OnceLock::new();
22    channel_singleton(&SLOT)
23}
24
25fn channel() -> &'static Channel<Callback> {
26    unsafe { &*isaac_sim_bridge_channel_imu() }
27}
28
29/// Register a callback to receive every IMU frame the bridge dispatches.
30/// The closure runs on the bridge thread; keep it bounded.
31pub fn register_imu_consumer<F>(cb: F)
32where
33    F: Fn(&str, &str, &ImuMeta) + Send + Sync + 'static,
34{
35    channel().register(Box::new(cb));
36}
37
38/// Fan out a single IMU frame to all registered consumers.
39pub fn dispatch_imu(source_id: &str, frame_id: &str, meta: &ImuMeta) {
40    channel().for_each(|cb| cb(source_id, frame_id, meta));
41}
42
43/// Number of currently registered IMU consumers.
44pub fn imu_consumer_count() -> usize {
45    channel().count()
46}
47
48/// Entry point called by the C++ bridge on each OmniGraph tick.
49pub fn forward_imu(source_id: &str, frame_id: &str, meta: &ImuMeta) {
50    log::debug!(
51        "[isaac-sim-rs] forward_imu: source='{}' frame='{}' lin_acc=[{:.3},{:.3},{:.3}] ang_vel=[{:.3},{:.3},{:.3}] q=[{:.3},{:.3},{:.3},{:.3}]",
52        source_id,
53        frame_id,
54        meta.lin_acc_x,
55        meta.lin_acc_y,
56        meta.lin_acc_z,
57        meta.ang_vel_x,
58        meta.ang_vel_y,
59        meta.ang_vel_z,
60        meta.orientation_w,
61        meta.orientation_x,
62        meta.orientation_y,
63        meta.orientation_z,
64    );
65    dispatch_imu(source_id, frame_id, meta);
66}
67
68#[cfg(test)]
69mod tests {
70    use super::*;
71    use std::sync::atomic::{AtomicUsize, Ordering};
72    use std::sync::Arc;
73
74    fn fake_meta() -> ImuMeta {
75        ImuMeta {
76            lin_acc_x: 0.1,
77            lin_acc_y: 0.2,
78            lin_acc_z: 9.81,
79            ang_vel_x: 0.0,
80            ang_vel_y: 0.0,
81            ang_vel_z: 0.5,
82            orientation_w: 1.0,
83            orientation_x: 0.0,
84            orientation_y: 0.0,
85            orientation_z: 0.0,
86            timestamp_ns: 0,
87        }
88    }
89
90    #[test]
91    fn registered_consumer_receives_dispatch_with_source() {
92        let count = Arc::new(AtomicUsize::new(0));
93        let count_clone = Arc::clone(&count);
94        let n_baseline = imu_consumer_count();
95
96        register_imu_consumer(move |src, frame, meta| {
97            assert_eq!(src, "/World/Carter/imu");
98            assert_eq!(frame, "sim_imu");
99            assert!((meta.lin_acc_z - 9.81).abs() < 1e-9);
100            assert_eq!(meta.orientation_w, 1.0);
101            count_clone.fetch_add(1, Ordering::SeqCst);
102        });
103
104        assert_eq!(imu_consumer_count(), n_baseline + 1);
105
106        forward_imu("/World/Carter/imu", "sim_imu", &fake_meta());
107
108        assert_eq!(count.load(Ordering::SeqCst), 1);
109    }
110}
isaac_sim_bridge/imu/mod.rs

isaac_sim_bridge/imu/
mod.rs
