isaac_sim_bridge/lidar/
pointcloud.rs

1// SPDX-License-Identifier: MPL-2.0
2use std::sync::OnceLock;
3
4use crate::channel::{channel_singleton, Channel};
5use crate::ffi::LidarPointCloudMeta;
6use crate::sensor::Sensor;
7
8/// Type-level marker for the 3D RTX LiDAR PointCloud sensor channel.
9pub struct LidarPointCloud;
10
11impl Sensor for LidarPointCloud {
12    const NAME: &'static str = "lidar_pointcloud";
13}
14
15pub type Callback = Box<dyn Fn(&str, &[f32], &LidarPointCloudMeta) + Send + Sync + 'static>;
16
17#[unsafe(no_mangle)]
18pub extern "C" fn isaac_sim_bridge_channel_lidar_pointcloud() -> *const Channel<Callback> {
19    static SLOT: OnceLock<Box<Channel<Callback>>> = OnceLock::new();
20    channel_singleton(&SLOT)
21}
22
23fn channel() -> &'static Channel<Callback> {
24    unsafe { &*isaac_sim_bridge_channel_lidar_pointcloud() }
25}
26
27/// Register a callback to receive every `LidarPointCloud` frame the bridge
28/// dispatches. The closure runs on the bridge thread; keep it bounded.
29pub fn register_lidar_pointcloud_consumer<F>(cb: F)
30where
31    F: Fn(&str, &[f32], &LidarPointCloudMeta) + Send + Sync + 'static,
32{
33    channel().register(Box::new(cb));
34}
35
36/// Fan out a single `LidarPointCloud` frame to all registered consumers.
37pub fn dispatch_lidar_pointcloud(source_id: &str, points: &[f32], meta: &LidarPointCloudMeta) {
38    channel().for_each(|cb| cb(source_id, points, meta));
39}
40
41/// Number of currently registered `LidarPointCloud` consumers.
42pub fn lidar_pointcloud_consumer_count() -> usize {
43    channel().count()
44}
45
46/// Entry point called by the C++ bridge on each OmniGraph tick.
47pub fn forward_lidar_pointcloud(source_id: &str, points: &[f32], meta: &LidarPointCloudMeta) {
48    log::debug!(
49        "[isaac-sim-rs] forward_lidar_pointcloud: source='{}' n={} (floats={}, width={}, height={})",
50        source_id,
51        meta.num_points,
52        points.len(),
53        meta.width,
54        meta.height
55    );
56    dispatch_lidar_pointcloud(source_id, points, meta);
57}
58
59#[cfg(test)]
60mod tests {
61    use super::*;
62    use std::sync::atomic::{AtomicUsize, Ordering};
63    use std::sync::Arc;
64
65    #[test]
66    fn registered_consumer_receives_dispatch_with_source() {
67        let count = Arc::new(AtomicUsize::new(0));
68        let count_clone = Arc::clone(&count);
69        let n_baseline = lidar_pointcloud_consumer_count();
70
71        register_lidar_pointcloud_consumer(move |src, points, meta| {
72            assert_eq!(src, "/World/Carter/Lidar3D");
73            assert_eq!(points.len(), 9);
74            assert_eq!(meta.num_points, 3);
75            count_clone.fetch_add(1, Ordering::SeqCst);
76        });
77
78        assert_eq!(lidar_pointcloud_consumer_count(), n_baseline + 1);
79
80        let points = [
81            1.0_f32, 0.0, 0.0, //
82            0.0, 1.0, 0.0, //
83            0.0, 0.0, 1.0,
84        ];
85        let meta = LidarPointCloudMeta {
86            num_points: 3,
87            width: 3,
88            height: 1,
89        };
90        dispatch_lidar_pointcloud("/World/Carter/Lidar3D", &points, &meta);
91
92        assert_eq!(count.load(Ordering::SeqCst), 1);
93    }
94}
isaac_sim_bridge/lidar/pointcloud.rs

isaac_sim_bridge/lidar/
pointcloud.rs
