isaac_sim_bridge/odometry/
mod.rs

1// SPDX-License-Identifier: MPL-2.0
2use std::sync::OnceLock;
3
4use crate::channel::{channel_singleton, Channel};
5use crate::ffi::OdometryMeta;
6use crate::sensor::Sensor;
7
8/// Type-level marker for the chassis odometry channel. One sample per
9/// dispatch carries position, orientation, and body-frame velocities
10/// packed into [`OdometryMeta`] — no variable-sized payload.
11pub struct Odometry;
12
13impl Sensor for Odometry {
14    const NAME: &'static str = "odometry";
15}
16
17pub type Callback = Box<dyn Fn(&str, &str, &str, &OdometryMeta) + Send + Sync + 'static>;
18
19#[unsafe(no_mangle)]
20pub extern "C" fn isaac_sim_bridge_channel_odometry() -> *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_odometry() }
27}
28
29/// Register a callback to receive every chassis odometry frame the bridge dispatches.
30/// The closure runs on the bridge thread; keep it bounded.
31pub fn register_odometry_consumer<F>(cb: F)
32where
33    F: Fn(&str, &str, &str, &OdometryMeta) + Send + Sync + 'static,
34{
35    channel().register(Box::new(cb));
36}
37
38/// Fan out a single odometry frame to all registered consumers.
39pub fn dispatch_odometry(
40    source_id: &str,
41    chassis_frame_id: &str,
42    odom_frame_id: &str,
43    meta: &OdometryMeta,
44) {
45    channel().for_each(|cb| cb(source_id, chassis_frame_id, odom_frame_id, meta));
46}
47
48/// Number of currently registered odometry consumers.
49pub fn odometry_consumer_count() -> usize {
50    channel().count()
51}
52
53/// Entry point called by the C++ bridge on each OmniGraph tick.
54pub fn forward_odometry(
55    source_id: &str,
56    chassis_frame_id: &str,
57    odom_frame_id: &str,
58    meta: &OdometryMeta,
59) {
60    log::debug!(
61        "[isaac-sim-rs] forward_odometry: source='{}' chassis='{}' odom='{}' pos=[{:.3},{:.3},{:.3}] q=[{:.3},{:.3},{:.3},{:.3}] lin=[{:.3},{:.3},{:.3}] ang=[{:.3},{:.3},{:.3}]",
62        source_id,
63        chassis_frame_id,
64        odom_frame_id,
65        meta.position_x,
66        meta.position_y,
67        meta.position_z,
68        meta.orientation_w,
69        meta.orientation_x,
70        meta.orientation_y,
71        meta.orientation_z,
72        meta.lin_vel_x,
73        meta.lin_vel_y,
74        meta.lin_vel_z,
75        meta.ang_vel_x,
76        meta.ang_vel_y,
77        meta.ang_vel_z,
78    );
79    dispatch_odometry(source_id, chassis_frame_id, odom_frame_id, meta);
80}
81
82#[cfg(test)]
83mod tests {
84    use super::*;
85    use std::sync::atomic::{AtomicUsize, Ordering};
86    use std::sync::Arc;
87
88    fn fake_meta() -> OdometryMeta {
89        OdometryMeta {
90            position_x: 1.0,
91            position_y: 2.0,
92            position_z: 0.5,
93            orientation_w: 1.0,
94            orientation_x: 0.0,
95            orientation_y: 0.0,
96            orientation_z: 0.0,
97            lin_vel_x: 0.3,
98            lin_vel_y: 0.0,
99            lin_vel_z: 0.0,
100            ang_vel_x: 0.0,
101            ang_vel_y: 0.0,
102            ang_vel_z: 0.1,
103            timestamp_ns: 0,
104        }
105    }
106
107    #[test]
108    fn registered_consumer_receives_dispatch_with_source() {
109        let count = Arc::new(AtomicUsize::new(0));
110        let count_clone = Arc::clone(&count);
111        let n_baseline = odometry_consumer_count();
112
113        register_odometry_consumer(move |src, chassis, odom, meta| {
114            assert_eq!(src, "/World/Carter");
115            assert_eq!(chassis, "base_link");
116            assert_eq!(odom, "odom");
117            assert_eq!(meta.position_x, 1.0);
118            assert_eq!(meta.lin_vel_x, 0.3);
119            count_clone.fetch_add(1, Ordering::SeqCst);
120        });
121
122        assert_eq!(odometry_consumer_count(), n_baseline + 1);
123
124        forward_odometry("/World/Carter", "base_link", "odom", &fake_meta());
125
126        assert_eq!(count.load(Ordering::SeqCst), 1);
127    }
128}
isaac_sim_bridge/odometry/mod.rs

isaac_sim_bridge/odometry/
mod.rs
