realsense_rust/frame/
motion.rs

1//! Type for representing a RealSense Motion frame.
2//!
3//! Motion data for any Motion frame is held as a 3-vector. This data (retrieved
4//! through `motion()`) represents different things depending on the device recorded.
5//!
6//! See the docs for [MotionFrame::motion] for more.
7
8use super::prelude::{CouldNotGetFrameSensorError, FrameCategory, FrameConstructionError, FrameEx};
9use crate::{
10    check_rs2_error,
11    kind::{Rs2Extension, Rs2FrameMetadata, Rs2StreamKind, Rs2TimestampDomain},
12    sensor::Sensor,
13    stream_profile::StreamProfile,
14};
15use anyhow::Result;
16use num_traits::FromPrimitive;
17use realsense_sys as sys;
18use std::{
19    convert::{TryFrom, TryInto},
20    marker::PhantomData,
21    ptr::{self, NonNull},
22};
23
24/// A unit struct defining an Accel frame.
25#[derive(Debug)]
26pub struct Accel;
27/// A unit struct defining a Gyro frame.
28#[derive(Debug)]
29pub struct Gyro;
30
31/// Holds the raw data pointer and derived data from an RS2 Motion Frame.
32///
33/// All fields in this struct are initialized during struct creation (via `try_from`).
34/// Everything called from here during runtime should be valid as long as the
35/// Frame is in scope... like normal Rust.
36#[derive(Debug)]
37pub struct MotionFrame<Kind> {
38    /// The raw data pointer from the original rs2 frame.
39    frame_ptr: NonNull<sys::rs2_frame>,
40    /// The timestamp of the frame.
41    timestamp: f64,
42    /// The RealSense time domain from which the timestamp is derived.
43    timestamp_domain: Rs2TimestampDomain,
44    /// The frame number.
45    frame_number: u64,
46    /// The Stream Profile that created the frame.
47    frame_stream_profile: StreamProfile,
48    /// The motion data held in this Motion Frame. Motion data is represented as a
49    /// 3-vector, with different conventions depending on the device recorded.
50    /// See the [motion](MotionFrame::motion) function for more documentation.
51    motion: [f32; 3],
52    /// A boolean used during `Drop` calls. This allows for proper handling of the pointer
53    /// during ownership transfer.
54    should_drop: bool,
55    /// Holds the type metadata of this frame.
56    _phantom: PhantomData<Kind>,
57}
58
59/// A motion frame type holding the raw pointer and derived metadata for an RS2 Accel frame.
60pub type AccelFrame = MotionFrame<Accel>;
61/// A motion frame type holding the raw pointer and derived metadata for an RS2 Gyro frame.
62pub type GyroFrame = MotionFrame<Gyro>;
63
64impl FrameCategory for AccelFrame {
65    fn extension() -> Rs2Extension {
66        Rs2Extension::MotionFrame
67    }
68
69    fn kind() -> Rs2StreamKind {
70        Rs2StreamKind::Accel
71    }
72
73    fn has_correct_kind(&self) -> bool {
74        self.frame_stream_profile.kind() == Self::kind()
75    }
76}
77
78impl FrameCategory for GyroFrame {
79    fn extension() -> Rs2Extension {
80        Rs2Extension::MotionFrame
81    }
82
83    fn kind() -> Rs2StreamKind {
84        Rs2StreamKind::Gyro
85    }
86
87    fn has_correct_kind(&self) -> bool {
88        self.frame_stream_profile.kind() == Self::kind()
89    }
90}
91
92impl<K> Drop for MotionFrame<K> {
93    /// Drop the raw pointer stored with this struct whenever it goes out of scope.
94    fn drop(&mut self) {
95        unsafe {
96            if self.should_drop {
97                sys::rs2_release_frame(self.frame_ptr.as_ptr());
98            }
99        }
100    }
101}
102
103unsafe impl<K> Send for MotionFrame<K> {}
104
105impl<K> TryFrom<NonNull<sys::rs2_frame>> for MotionFrame<K> {
106    type Error = anyhow::Error;
107
108    /// Attempt to create an Image frame of extension K from the raw `rs2_frame`. All
109    /// members of the ImageFrame struct are validated and populated during this call.
110    ///
111    /// # Errors
112    ///
113    /// There are a number of errors that may occur if the data in the `rs2_frame` is not
114    /// valid, all of type [FrameConstructionError].
115    ///
116    /// - [CouldNotGetTimestamp](FrameConstructionError::CouldNotGetTimestamp)
117    /// - [CouldNotGetTimestampDomain](FrameConstructionError::CouldNotGetTimestampDomain)
118    /// - [CouldNotGetFrameStreamProfile](FrameConstructionError::CouldNotGetFrameStreamProfile)
119    /// - [CouldNotGetDataSize](FrameConstructionError::CouldNotGetDataSize)
120    /// - [CouldNotGetData](FrameConstructionError::CouldNotGetData)
121    ///
122    /// See [FrameConstructionError] documentation for more details.
123    ///
124    fn try_from(frame_ptr: NonNull<sys::rs2_frame>) -> Result<Self, Self::Error> {
125        unsafe {
126            let mut err = ptr::null_mut::<sys::rs2_error>();
127
128            let timestamp = sys::rs2_get_frame_timestamp(frame_ptr.as_ptr(), &mut err);
129            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestamp)?;
130
131            let timestamp_domain =
132                sys::rs2_get_frame_timestamp_domain(frame_ptr.as_ptr(), &mut err);
133            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestampDomain)?;
134
135            let frame_number = sys::rs2_get_frame_number(frame_ptr.as_ptr(), &mut err);
136            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameNumber)?;
137
138            let profile_ptr = sys::rs2_get_frame_stream_profile(frame_ptr.as_ptr(), &mut err);
139            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameStreamProfile)?;
140
141            let nonnull_profile_ptr =
142                NonNull::new(profile_ptr as *mut sys::rs2_stream_profile).unwrap();
143            let profile = StreamProfile::try_from(nonnull_profile_ptr)?;
144
145            let size = sys::rs2_get_frame_data_size(frame_ptr.as_ptr(), &mut err);
146            check_rs2_error!(err, FrameConstructionError::CouldNotGetDataSize)?;
147
148            let ptr = sys::rs2_get_frame_data(frame_ptr.as_ptr(), &mut err);
149            check_rs2_error!(err, FrameConstructionError::CouldNotGetData)?;
150
151            let data_as_ptr = ptr.as_ref().unwrap() as *const std::os::raw::c_void;
152            let data_size_in_f32s = (size as usize) / std::mem::size_of::<f32>();
153
154            let motion_raw =
155                std::slice::from_raw_parts(data_as_ptr.cast::<f32>(), data_size_in_f32s);
156
157            Ok(MotionFrame {
158                frame_ptr,
159                timestamp,
160                timestamp_domain: Rs2TimestampDomain::from_i32(timestamp_domain as i32).unwrap(),
161                frame_number,
162                frame_stream_profile: profile,
163                motion: [motion_raw[0], motion_raw[1], motion_raw[2]],
164                should_drop: true,
165                _phantom: PhantomData::<K> {},
166            })
167        }
168    }
169}
170
171impl<K> FrameEx for MotionFrame<K> {
172    fn stream_profile(&self) -> &StreamProfile {
173        &self.frame_stream_profile
174    }
175
176    fn sensor(&self) -> Result<Sensor> {
177        unsafe {
178            let mut err = std::ptr::null_mut::<sys::rs2_error>();
179            let sensor_ptr = sys::rs2_get_frame_sensor(self.frame_ptr.as_ptr(), &mut err);
180            check_rs2_error!(err, CouldNotGetFrameSensorError)?;
181
182            Ok(Sensor::from(NonNull::new(sensor_ptr).unwrap()))
183        }
184    }
185    fn timestamp(&self) -> f64 {
186        self.timestamp
187    }
188
189    fn timestamp_domain(&self) -> Rs2TimestampDomain {
190        self.timestamp_domain
191    }
192
193    fn frame_number(&self) -> u64 {
194        self.frame_number
195    }
196
197    fn metadata(&self, metadata_kind: Rs2FrameMetadata) -> Option<std::os::raw::c_longlong> {
198        if !self.supports_metadata(metadata_kind) {
199            return None;
200        }
201
202        unsafe {
203            let mut err = std::ptr::null_mut::<sys::rs2_error>();
204
205            let val = sys::rs2_get_frame_metadata(
206                self.frame_ptr.as_ptr(),
207                #[allow(clippy::useless_conversion)]
208                (metadata_kind as i32).try_into().unwrap(),
209                &mut err,
210            );
211            if err.as_ref().is_none() {
212                Some(val)
213            } else {
214                sys::rs2_free_error(err);
215                None
216            }
217        }
218    }
219
220    fn supports_metadata(&self, metadata_kind: Rs2FrameMetadata) -> bool {
221        unsafe {
222            let mut err = std::ptr::null_mut::<sys::rs2_error>();
223
224            let supports_metadata = sys::rs2_supports_frame_metadata(
225                self.frame_ptr.as_ptr(),
226                #[allow(clippy::useless_conversion)]
227                (metadata_kind as i32).try_into().unwrap(),
228                &mut err,
229            );
230
231            if err.as_ref().is_none() {
232                supports_metadata != 0
233            } else {
234                sys::rs2_free_error(err);
235                false
236            }
237        }
238    }
239
240    unsafe fn get_owned_raw(mut self) -> NonNull<sys::rs2_frame> {
241        self.should_drop = false;
242
243        self.frame_ptr
244    }
245}
246
247impl AccelFrame {
248    /// Returns a 3-item array representing the sensor motion recorded in the Accel frame.
249    ///
250    /// Accelerations are reported as [x, y, z] values, and are in units of m/s^2
251    ///
252    /// This function will return different data conventions entirely depending on the device
253    /// used to create the measurement.
254    ///
255    /// # Intel RealSense D435i
256    ///
257    /// - `motion[0]`: Positive x-axis points to the right.
258    /// - `motion[1]`: Positive y-axis points down.
259    /// - `motion[2]`: Positive z-axis points forward.
260    ///
261    /// # Intel RealSense T265
262    ///
263    /// - `motion[0]`: Positive X direction is towards right imager.
264    /// - `motion[1]`: Positive Y direction is upwards toward the top of the device.
265    /// - `motion[2]`: Positive Z direction is inwards toward the back of the device.
266    ///
267    /// Read more about the coordinate frames of RealSense motion in
268    /// [the RealSense docs](https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/)
269    ///
270    pub fn acceleration(&self) -> &[f32; 3] {
271        &self.motion
272    }
273}
274
275impl GyroFrame {
276    /// Returns a 3-item array representing the sensor motion recorded in the Gyro frame.
277    ///
278    /// Gyroscope measurements are reported as [x, y, z] values, and are in units of radians/s
279    ///
280    /// This function will return different data conventions entirely depending on the device
281    /// used to create the measurement.
282    ///
283    /// # Intel RealSense D435i
284    ///
285    /// - `motion[0]`: Positive x-axis points to the right.
286    /// - `motion[1]`: Positive y-axis points down.
287    /// - `motion[2]`: Positive z-axis points forward.
288    ///
289    /// # Intel RealSense T265
290    ///
291    /// - `motion[0]`: Positive X direction is towards right imager.
292    /// - `motion[1]`: Positive Y direction is upwards toward the top of the device.
293    /// - `motion[2]`: Positive Z direction is inwards toward the back of the device.
294    ///
295    /// Read more about the coordinate frames of RealSense motion in
296    /// [the RealSense docs](https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/)
297    ///
298    pub fn rotational_velocity(&self) -> &[f32; 3] {
299        &self.motion
300    }
301}
302
303#[cfg(test)]
304mod tests {
305    use super::*;
306
307    #[test]
308    fn frame_has_correct_kind() {
309        assert_eq!(AccelFrame::kind(), Rs2StreamKind::Accel);
310        assert_eq!(GyroFrame::kind(), Rs2StreamKind::Gyro);
311    }
312}
realsense_rust/frame/motion.rs

realsense_rust/frame/
motion.rs
