isaac_sim_bridge/lidar/
flatscan.rs

1// SPDX-License-Identifier: MPL-2.0
2use std::sync::OnceLock;
3
4use crate::channel::{channel_singleton, Channel};
5use crate::ffi::LidarFlatScanMeta;
6use crate::sensor::Sensor;
7
8/// Type-level marker for the 2D RTX LiDAR FlatScan sensor channel.
9pub struct LidarFlatScan;
10
11impl Sensor for LidarFlatScan {
12    const NAME: &'static str = "lidar_flatscan";
13}
14
15pub type Callback = Box<dyn Fn(&str, &[f32], &[u8], &LidarFlatScanMeta) + Send + Sync + 'static>;
16
17/// Process-wide singleton handle to the FlatScan channel.
18///
19/// `#[no_mangle] pub extern "C"` makes the symbol visible to the dynamic
20/// linker with default visibility. When the bridge crate is compiled
21/// into multiple cdylibs (the bridge plugin's cdylib + any adapter
22/// cdylib loaded with RTLD_GLOBAL), every cdylib's call to this getter
23/// resolves to the first-loaded copy under RTLD_GLOBAL semantics, so
24/// register and dispatch from any cdylib hit the same heap registry.
25#[unsafe(no_mangle)]
26pub extern "C" fn isaac_sim_bridge_channel_lidar_flatscan() -> *const Channel<Callback> {
27    static SLOT: OnceLock<Box<Channel<Callback>>> = OnceLock::new();
28    channel_singleton(&SLOT)
29}
30
31fn channel() -> &'static Channel<Callback> {
32    unsafe { &*isaac_sim_bridge_channel_lidar_flatscan() }
33}
34
35/// Register a callback to receive every `LidarFlatScan` frame the bridge
36/// dispatches. The closure runs on the bridge thread; keep it bounded.
37/// Registrations accumulate for the lifetime of the process — there is no
38/// unregister; drop the adapter runner to stop receiving frames.
39pub fn register_lidar_flatscan_consumer<F>(cb: F)
40where
41    F: Fn(&str, &[f32], &[u8], &LidarFlatScanMeta) + Send + Sync + 'static,
42{
43    channel().register(Box::new(cb));
44}
45
46/// Fan out a single `LidarFlatScan` frame to all registered consumers.
47/// Called by `forward_lidar_flatscan` after logging; also callable directly
48/// from tests or synthetic sources that bypass the C++ bridge path.
49pub fn dispatch_lidar_flatscan(
50    source_id: &str,
51    scan: &[f32],
52    intensities: &[u8],
53    meta: &LidarFlatScanMeta,
54) {
55    channel().for_each(|cb| cb(source_id, scan, intensities, meta));
56}
57
58/// Number of currently registered `LidarFlatScan` consumers.
59pub fn lidar_flatscan_consumer_count() -> usize {
60    channel().count()
61}
62
63/// Entry point called by the C++ bridge on each OmniGraph tick. Logs
64/// a debug summary then calls `dispatch_lidar_flatscan` to fan out.
65pub fn forward_lidar_flatscan(
66    source_id: &str,
67    scan: &[f32],
68    intensities: &[u8],
69    meta: &LidarFlatScanMeta,
70) {
71    let depth_min = scan.iter().copied().fold(f32::INFINITY, f32::min);
72    let depth_max = scan.iter().copied().fold(f32::NEG_INFINITY, f32::max);
73    log::debug!(
74        "[isaac-sim-rs] forward_lidar_flatscan: source='{}' scan_n={}, intensity_n={}, fov={:.1}°, observed_depth=[{:.3},{:.3}]m",
75        source_id,
76        scan.len(),
77        intensities.len(),
78        meta.horizontal_fov,
79        depth_min,
80        depth_max
81    );
82
83    dispatch_lidar_flatscan(source_id, scan, intensities, meta);
84}
85
86#[cfg(test)]
87mod tests {
88    use super::*;
89    use std::sync::atomic::{AtomicUsize, Ordering};
90    use std::sync::Arc;
91
92    fn fake_meta() -> LidarFlatScanMeta {
93        LidarFlatScanMeta {
94            horizontal_fov: 270.0,
95            horizontal_resolution: 0.25,
96            azimuth_min: -135.0,
97            azimuth_max: 135.0,
98            depth_min: 0.1,
99            depth_max: 30.0,
100            num_rows: 1,
101            num_cols: 4,
102            rotation_rate: 10.0,
103        }
104    }
105
106    #[test]
107    fn registered_consumer_receives_dispatch_with_source() {
108        let count = Arc::new(AtomicUsize::new(0));
109        let count_clone = Arc::clone(&count);
110        let n_baseline = lidar_flatscan_consumer_count();
111
112        register_lidar_flatscan_consumer(move |src, scan, _intens, meta| {
113            assert_eq!(src, "/World/Lidar2D");
114            assert_eq!(scan.len(), 4);
115            assert!((meta.horizontal_fov - 270.0).abs() < 1e-6);
116            count_clone.fetch_add(1, Ordering::SeqCst);
117        });
118
119        assert_eq!(lidar_flatscan_consumer_count(), n_baseline + 1);
120
121        let scan = [0.5_f32, 1.2, 2.7, 3.0];
122        let intensities = [10_u8, 50, 200, 100];
123        dispatch_lidar_flatscan("/World/Lidar2D", &scan, &intensities, &fake_meta());
124
125        assert_eq!(count.load(Ordering::SeqCst), 1);
126    }
127}
isaac_sim_bridge/lidar/flatscan.rs

isaac_sim_bridge/lidar/
flatscan.rs
