realsense-rust 1.1.0

High-level RealSense library in Rust
Documentation
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
//! Type for abstracting over the concept of a Sensor
//!
//! The words device and sensor are overloaded in this industry, and librealsense2 makes no
//! attempts to fix that. While the words device and sensor are often used colloquially, here
//! `Device` refers to a whole Realsense package, such as a D435i, L515, etc. `Sensor` refers to
//! "sub-devices"; rather, the individual sensing components that one can stream from. A sensor may
//! be an IMU, which has accelerometer and gyroscope streams, or a camera of some kind, which has
//! streams related to different types of images.
//!
//! The hierarchy is effectively:
//!
//! [`Device`] |-> [`Sensor`] |-> [`StreamProfile`]

use crate::{
    check_rs2_error,
    device::{Device, DeviceConstructionError},
    kind::{
        OptionSetError, Rs2CameraInfo, Rs2Exception, Rs2Extension, Rs2Option, Rs2OptionRange,
        SENSOR_EXTENSIONS,
    },
    stream_profile::StreamProfile,
};
use anyhow::Result;
use realsense_sys as sys;
use std::{
    convert::{From, TryInto},
    ffi::CStr,
    mem::MaybeUninit,
    ptr::NonNull,
};
use thiserror::Error;

/// Type describing errors that can occur when trying to construct a sensor.
///
/// Follows the standard pattern of errors where the enum variant describes what the low-level code
/// was attempting to do while the string carried alongside describes the underlying error message
/// from any C++ exceptions that occur.
#[derive(Error, Debug)]
pub enum SensorConstructionError {
    /// Could not get the correct sensor from the sensor list.
    #[error("Could not get correct sensor from sensor list. Type: {0}; Reason: {1}")]
    CouldNotGetSensorFromList(Rs2Exception, String),
}

/// Type for holding sensor-related data.
///
/// A sensor in librealsense2 corresponds to a physical component on the unit in some way, shape,
/// or form. These may or may not correspond to multiple streams. e.g. an IMU on the device may
/// correspond to accelerometer and gyroscope streams, or an IR camera sensor on the device may
/// correspond to depth & video streams.
///
/// Sensors are constructed one of two ways:
///
/// 1. From the device's [sensor list](crate::device::Device::sensors)
/// 2. By getting the sensor that [corresponds to a given frame](crate::frame::FrameEx::sensor)
pub struct Sensor {
    /// The underlying non-null sensor pointer.
    ///
    /// This should not be deleted unless the sensor was constructed via `rs2_create_sensor`
    sensor_ptr: NonNull<sys::rs2_sensor>,
    /// Boolean used for telling us if we should drop the sensor pointer or not.
    should_drop: bool,
}

impl Drop for Sensor {
    fn drop(&mut self) {
        unsafe {
            if self.should_drop {
                sys::rs2_delete_sensor(self.sensor_ptr.as_ptr());
            }
        }
    }
}

unsafe impl Send for Sensor {}

impl std::convert::From<NonNull<sys::rs2_sensor>> for Sensor {
    /// Attempt to construct a Sensor from a non-null pointer to `rs2_sensor`.
    fn from(sensor_ptr: NonNull<sys::rs2_sensor>) -> Self {
        Sensor {
            sensor_ptr,
            should_drop: false,
        }
    }
}

impl Sensor {
    /// Create a sensor from a sensor list and an index
    ///
    /// Unlike when you directly acquire a `*mut rs2_sensor` from an API in librealsense2, such as
    /// when calling `rs2_get_frame_sensor`, you have to drop this pointer at the end (because you
    /// now own it). When calling `try_from` we don't want to drop in the default case, since our
    /// `*mut rs2_sensor` may not be owned by us, but by the device / frame / etc.
    ///
    /// The main difference then is that this API defaults to using `rs2_create_sensor` vs. a call
    /// to get a sensor from somewhere else.
    ///
    /// This can fail for similar reasons to `try_from`, and is likewise only valid if `index` is
    /// less than the length of `sensor_list` (see `rs2_get_sensors_count` for how to get that
    /// length).
    ///
    /// Guaranteeing the lifetime / semantics of the sensor is difficult, so this should probably
    /// not be used outside of this crate. See `crate::device::Device` for where this is used.
    ///
    /// # Errors
    ///
    /// Returns [`SensorConstructionError::CouldNotGetSensorFromList`] if the index is invalid or
    /// if the sensor list is invalid in some way.
    pub(crate) fn try_create(
        sensor_list: &NonNull<sys::rs2_sensor_list>,
        index: i32,
    ) -> Result<Self, SensorConstructionError> {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();

            let sensor_ptr = sys::rs2_create_sensor(sensor_list.as_ptr(), index, &mut err);
            check_rs2_error!(err, SensorConstructionError::CouldNotGetSensorFromList)?;

            let nonnull_ptr = NonNull::new(sensor_ptr).unwrap();
            let mut sensor = Sensor::from(nonnull_ptr);
            sensor.should_drop = true;
            Ok(sensor)
        }
    }

    /// Get the parent device that this sensor corresponds to.
    ///
    /// Returns the device that this sensor corresponds to iff that device is still connected and
    /// the sensor is still valid. Otherwise returns an error.
    ///
    /// # Errors
    ///
    /// Returns [`DeviceConstructionError::CouldNotCreateDeviceFromSensor`] if the device cannot be
    /// obtained due to the physical device being disconnected or the internal sensor pointer
    /// becoming invalid.
    pub fn device(&self) -> Result<Device, DeviceConstructionError> {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let device_ptr = sys::rs2_create_device_from_sensor(self.sensor_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, DeviceConstructionError::CouldNotCreateDeviceFromSensor)?;

            Ok(Device::from(NonNull::new(device_ptr).unwrap()))
        }
    }

    /// Get sensor extension.
    pub fn extension(&self) -> Rs2Extension {
        let ext = SENSOR_EXTENSIONS
            .iter()
            .find(|ext| unsafe {
                let mut err = std::ptr::null_mut::<sys::rs2_error>();
                let is_extendable = sys::rs2_is_sensor_extendable_to(
                    self.sensor_ptr.as_ptr(),
                    #[allow(clippy::useless_conversion)]
                    (**ext as i32).try_into().unwrap(),
                    &mut err,
                );

                if err.as_ref().is_none() {
                    is_extendable != 0
                } else {
                    sys::rs2_free_error(err);
                    false
                }
            })
            .unwrap();
        *ext
    }

    /// Get the value associated with the provided Rs2Option for the sensor.
    ///
    /// Returns An `f32` value corresponding to that option within the librealsense2 library, or None
    /// if the option is not supported.
    pub fn get_option(&self, option: Rs2Option) -> Option<f32> {
        if !self.supports_option(option) {
            return None;
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let val = sys::rs2_get_option(
                self.sensor_ptr.as_ptr().cast::<sys::rs2_options>(),
                #[allow(clippy::useless_conversion)]
                (option as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                Some(val)
            } else {
                sys::rs2_free_error(err);
                None
            }
        }
    }

    /// Sets the `value` associated with the provided `option` for the sensor.
    ///
    /// Returns null tuple if the option can be successfully set on the sensor, otherwise an error.
    ///
    /// # Errors
    ///
    /// Returns [`OptionSetError::OptionNotSupported`] if the option is not supported on this
    /// sensor.
    ///
    /// Returns [`OptionSetError::OptionIsReadOnly`] if the option is supported but cannot be set
    /// on this sensor.
    ///
    /// Returns [`OptionSetError::CouldNotSetOption`] if the option is supported and not read-only,
    /// but could not be set for another reason (invalid value, internal exception, etc.).
    pub fn set_option(&mut self, option: Rs2Option, value: f32) -> Result<(), OptionSetError> {
        if !self.supports_option(option) {
            return Err(OptionSetError::OptionNotSupported);
        }

        if self.is_option_read_only(option) {
            return Err(OptionSetError::OptionIsReadOnly);
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            sys::rs2_set_option(
                self.sensor_ptr.as_ptr().cast::<sys::rs2_options>(),
                #[allow(clippy::useless_conversion)]
                (option as i32).try_into().unwrap(),
                value,
                &mut err,
            );
            check_rs2_error!(err, OptionSetError::CouldNotSetOption)?;

            Ok(())
        }
    }

    /// Gets the range for a given option.
    ///
    /// Returns some option range if the sensor supports the option, else `None`.
    pub fn get_option_range(&self, option: Rs2Option) -> Option<Rs2OptionRange> {
        if !self.supports_option(option) {
            return None;
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();

            let mut min = MaybeUninit::uninit();
            let mut max = MaybeUninit::uninit();
            let mut step = MaybeUninit::uninit();
            let mut default = MaybeUninit::uninit();

            sys::rs2_get_option_range(
                self.sensor_ptr.as_ptr().cast::<sys::rs2_options>(),
                #[allow(clippy::useless_conversion)]
                (option as i32).try_into().unwrap(),
                min.as_mut_ptr(),
                max.as_mut_ptr(),
                step.as_mut_ptr(),
                default.as_mut_ptr(),
                &mut err,
            );

            if err.as_ref().is_none() {
                Some(Rs2OptionRange {
                    min: min.assume_init(),
                    max: max.assume_init(),
                    step: step.assume_init(),
                    default: default.assume_init(),
                })
            } else {
                sys::rs2_free_error(err);
                None
            }
        }
    }

    /// Predicate for determining if this sensor supports a given option
    ///
    /// Returns true iff the option is supported by this sensor.
    pub fn supports_option(&self, option: Rs2Option) -> bool {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let val = sys::rs2_supports_option(
                self.sensor_ptr.as_ptr().cast::<sys::rs2_options>(),
                #[allow(clippy::useless_conversion)]
                (option as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                val != 0
            } else {
                sys::rs2_free_error(err);
                false
            }
        }
    }

    /// Predicate for determining if the provided option is immutable or not.
    ///
    /// Returns true if the option is supported and can be mutated, otherwise false.
    pub fn is_option_read_only(&self, option: Rs2Option) -> bool {
        if !self.supports_option(option) {
            return false;
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let val = sys::rs2_is_option_read_only(
                self.sensor_ptr.as_ptr().cast::<sys::rs2_options>(),
                #[allow(clippy::useless_conversion)]
                (option as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                val != 0
            } else {
                sys::rs2_free_error(err);
                false
            }
        }
    }

    /// Get a list of stream profiles associated with this sensor
    ///
    /// Returns a vector containing all the stream profiles associated with the sensor. The vector
    /// will have a length of zero if an error occurs while getting the stream profiles.
    pub fn stream_profiles(&self) -> Vec<StreamProfile> {
        let mut profiles = Vec::new();
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let profiles_ptr = sys::rs2_get_stream_profiles(self.sensor_ptr.as_ptr(), &mut err);
            if err.as_ref().is_some() {
                sys::rs2_free_error(err);
                return profiles;
            }

            let nonnull_profiles_ptr =
                NonNull::new(profiles_ptr as *mut sys::rs2_stream_profile_list).unwrap();

            let len = sys::rs2_get_stream_profiles_count(nonnull_profiles_ptr.as_ptr(), &mut err);

            if err.as_ref().is_some() {
                sys::rs2_free_error(err);
                sys::rs2_delete_stream_profiles_list(nonnull_profiles_ptr.as_ptr());
                return profiles;
            }

            for i in 0..len {
                match StreamProfile::try_create(&nonnull_profiles_ptr, i) {
                    Ok(s) => {
                        profiles.push(s);
                    }
                    Err(_) => {
                        continue;
                    }
                }
            }
            sys::rs2_delete_stream_profiles_list(nonnull_profiles_ptr.as_ptr());
        }
        profiles
    }

    // fn recommended_processing_blocks(&self) -> Vec<ProcessingBlock>{}

    /// Gets the value associated with the provided camera info key from the sensor.
    ///
    /// Returns some value corresponding to the camera info requested if this sensor supports that
    /// camera info, else `None`.
    pub fn info(&self, camera_info: Rs2CameraInfo) -> Option<&CStr> {
        if !self.supports_info(camera_info) {
            return None;
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();

            let val = sys::rs2_get_sensor_info(
                self.sensor_ptr.as_ptr(),
                #[allow(clippy::useless_conversion)]
                (camera_info as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                Some(CStr::from_ptr(val))
            } else {
                sys::rs2_free_error(err);
                None
            }
        }
    }

    /// Predicate method for determining if the sensor supports a certain kind of camera info.
    ///
    /// Returns true iff the sensor has a value associated with the `camera_info` key.
    pub fn supports_info(&self, camera_info: Rs2CameraInfo) -> bool {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let supports_info = sys::rs2_supports_sensor_info(
                self.sensor_ptr.as_ptr(),
                #[allow(clippy::useless_conversion)]
                (camera_info as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                supports_info != 0
            } else {
                sys::rs2_free_error(err);
                false
            }
        }
    }
}