realsense_rust/processing_blocks/
align.rs

1//! Processing block aligning one stream with the other
2//!
3//! Based on an example here:
4//! <https://github.com/IntelRealSense/librealsense/blob/4673a37d981164af8eeb8e296e430fc1427e008d/doc/post-processing-filters.md?plain=1#L111>
5
6use crate::{
7    check_rs2_error,
8    frame::CompositeFrame,
9    kind::Rs2StreamKind,
10    processing_blocks::errors::{ProcessFrameError, ProcessingBlockConstructionError},
11};
12use anyhow::Result;
13use realsense_sys as sys;
14use std::{convert::TryFrom, ptr::NonNull, task::Poll, time::Duration};
15
16/// Processing Block and Frame Queue for aligning a stream to a certain [StreamKind](Rs2StreamKind)
17#[derive(Debug, Clone)]
18pub struct Align {
19    /// The processing block for the "Align" method
20    processing_block: NonNull<sys::rs2_processing_block>,
21    /// The frame queue upon which the processing block will deposit aligned frames. We check this
22    /// for completed block operations.
23    processing_queue: NonNull<sys::rs2_frame_queue>,
24}
25
26impl Drop for Align {
27    fn drop(&mut self) {
28        unsafe {
29            sys::rs2_delete_frame_queue(self.processing_queue.as_ptr());
30            sys::rs2_delete_processing_block(self.processing_block.as_ptr());
31        }
32    }
33}
34
35impl Align {
36    /// Create a new Align object
37    pub fn new(
38        align_to: Rs2StreamKind,
39        processing_queue_size: i32,
40    ) -> Result<Self, ProcessingBlockConstructionError> {
41        let (processing_block, processing_queue) = unsafe {
42            let mut err = std::ptr::null_mut::<sys::rs2_error>();
43
44            let ptr = sys::rs2_create_align(align_to as sys::rs2_stream, &mut err);
45            check_rs2_error!(
46                err,
47                ProcessingBlockConstructionError::CouldNotCreateProcessingBlock
48            )?;
49
50            let queue_ptr = sys::rs2_create_frame_queue(processing_queue_size, &mut err);
51            check_rs2_error!(
52                err,
53                ProcessingBlockConstructionError::CouldNotCreateProcessingQueue
54            )?;
55
56            sys::rs2_start_processing_queue(ptr, queue_ptr, &mut err);
57            check_rs2_error!(
58                err,
59                ProcessingBlockConstructionError::CouldNotStartProcessingQueue
60            )?;
61            (NonNull::new(ptr).unwrap(), NonNull::new(queue_ptr).unwrap())
62        };
63
64        Ok(Self {
65            processing_block,
66            processing_queue,
67        })
68    }
69
70    /// Own and process the composite frame and return the aligned frames.
71    pub fn queue(&mut self, frames: CompositeFrame) -> Result<(), ProcessFrameError> {
72        unsafe {
73            let mut err = std::ptr::null_mut::<sys::rs2_error>();
74            sys::rs2_process_frame(
75                self.processing_block.as_ptr(), // -> *mut
76                frames.get_owned_raw().as_ptr(),
77                &mut err,
78            );
79            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
80            Ok(())
81        }
82    }
83
84    /// Wait to receive the results of the processing block
85    pub fn wait(&mut self, timeout: Duration) -> Result<CompositeFrame, ProcessFrameError> {
86        unsafe {
87            let mut err = std::ptr::null_mut::<sys::rs2_error>();
88            let timeout_millis = u32::try_from(timeout.as_millis()).unwrap_or(u32::MAX);
89
90            let aligned_frame =
91                sys::rs2_wait_for_frame(self.processing_queue.as_ptr(), timeout_millis, &mut err);
92            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
93            Ok(CompositeFrame::from(NonNull::new(aligned_frame).unwrap()))
94        }
95    }
96
97    /// Poll to receive the results of the processing block
98    pub fn poll(&mut self) -> Result<Poll<CompositeFrame>, ProcessFrameError> {
99        unsafe {
100            let mut err = std::ptr::null_mut::<sys::rs2_error>();
101            let mut frame = std::ptr::null_mut::<sys::rs2_frame>();
102            let is_ready =
103                sys::rs2_poll_for_frame(self.processing_queue.as_ptr(), &mut frame, &mut err);
104
105            // Check for errors
106            check_rs2_error!(err, |kind, context| { ProcessFrameError { kind, context } })?;
107
108            // Check for queue readiness
109            if is_ready == 0 {
110                Ok(Poll::Pending)
111            } else {
112                Ok(Poll::Ready(CompositeFrame::from(
113                    NonNull::new(frame).unwrap(),
114                )))
115            }
116        }
117    }
118}
realsense_rust/processing_blocks/align.rs

realsense_rust/processing_blocks/
align.rs
