realsense_rust/processing_blocks/
pointcloud.rs

1//! Processing block for generating point clouds from depth data
2//!
3//! This block accepts depth frames and outputs Points frames
4
5use crate::{
6    check_rs2_error,
7    frame::{DepthFrame, FrameEx, PointsFrame},
8    processing_blocks::errors::{ProcessFrameError, ProcessingBlockConstructionError},
9};
10use anyhow::Result;
11use realsense_sys as sys;
12use std::{convert::TryFrom, ptr::NonNull, task::Poll, time::Duration};
13
14/// Creates Point-Cloud processing block. This block accepts depth frames and outputs Points frames
15/// In addition, given non-depth frame, the block will align texture coordinate to the non-depth stream
16#[derive(Debug, Clone)]
17pub struct PointCloud {
18    /// The processing block for generating point clouds
19    processing_block: NonNull<sys::rs2_processing_block>,
20    /// The frame queue upon which the processing block will deposit point cloud frames
21    processing_queue: NonNull<sys::rs2_frame_queue>,
22}
23
24impl Drop for PointCloud {
25    fn drop(&mut self) {
26        unsafe {
27            sys::rs2_delete_frame_queue(self.processing_queue.as_ptr());
28            sys::rs2_delete_processing_block(self.processing_block.as_ptr());
29        }
30    }
31}
32
33impl PointCloud {
34    /// Create a new PointCloud processing block
35    pub fn new(processing_queue_size: i32) -> Result<Self, ProcessingBlockConstructionError> {
36        let (processing_block, processing_queue) = unsafe {
37            let mut err = std::ptr::null_mut::<sys::rs2_error>();
38
39            let ptr = sys::rs2_create_pointcloud(&mut err);
40            check_rs2_error!(
41                err,
42                ProcessingBlockConstructionError::CouldNotCreateProcessingBlock
43            )?;
44
45            let queue_ptr = sys::rs2_create_frame_queue(processing_queue_size, &mut err);
46            check_rs2_error!(
47                err,
48                ProcessingBlockConstructionError::CouldNotCreateProcessingQueue
49            )?;
50
51            sys::rs2_start_processing_queue(ptr, queue_ptr, &mut err);
52            check_rs2_error!(
53                err,
54                ProcessingBlockConstructionError::CouldNotStartProcessingQueue
55            )?;
56            (NonNull::new(ptr).unwrap(), NonNull::new(queue_ptr).unwrap())
57        };
58
59        Ok(Self {
60            processing_block,
61            processing_queue,
62        })
63    }
64
65    /// Process a depth frame to generate point cloud
66    pub fn queue(&mut self, frame: DepthFrame) -> Result<(), ProcessFrameError> {
67        unsafe {
68            let mut err = std::ptr::null_mut::<sys::rs2_error>();
69            sys::rs2_process_frame(
70                self.processing_block.as_ptr(),
71                frame.get_owned_raw().as_ptr(),
72                &mut err,
73            );
74            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
75            Ok(())
76        }
77    }
78
79    /// Wait to receive the point cloud results
80    pub fn wait(&mut self, timeout: Duration) -> Result<PointsFrame, ProcessFrameError> {
81        unsafe {
82            let mut err = std::ptr::null_mut::<sys::rs2_error>();
83            let timeout_millis = u32::try_from(timeout.as_millis()).unwrap_or(u32::MAX);
84
85            let points_frame =
86                sys::rs2_wait_for_frame(self.processing_queue.as_ptr(), timeout_millis, &mut err);
87            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
88            Ok(PointsFrame::try_from(NonNull::new(points_frame).unwrap()).unwrap())
89        }
90    }
91
92    /// Poll to receive the point cloud results
93    pub fn poll(&mut self) -> Result<Poll<PointsFrame>, ProcessFrameError> {
94        unsafe {
95            let mut err = std::ptr::null_mut::<sys::rs2_error>();
96            let mut frame = std::ptr::null_mut::<sys::rs2_frame>();
97            let is_ready =
98                sys::rs2_poll_for_frame(self.processing_queue.as_ptr(), &mut frame, &mut err);
99
100            // Check for errors
101            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
102
103            // Check for queue readiness
104            if is_ready == 0 {
105                Ok(Poll::Pending)
106            } else {
107                Ok(Poll::Ready(
108                    PointsFrame::try_from(NonNull::new(frame).unwrap()).unwrap(),
109                ))
110            }
111        }
112    }
113}
realsense_rust/processing_blocks/pointcloud.rs

realsense_rust/processing_blocks/
pointcloud.rs
