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;
#[derive(Error, Debug)]
pub enum SensorConstructionError {
#[error("Could not get correct sensor from sensor list. Type: {0}; Reason: {1}")]
CouldNotGetSensorFromList(Rs2Exception, String),
}
pub struct Sensor {
sensor_ptr: NonNull<sys::rs2_sensor>,
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 {
fn from(sensor_ptr: NonNull<sys::rs2_sensor>) -> Self {
Sensor {
sensor_ptr,
should_drop: false,
}
}
}
impl Sensor {
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)
}
}
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()))
}
}
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
}
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
}
}
}
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(())
}
}
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
}
}
}
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
}
}
}
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
}
}
}
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
}
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
}
}
}
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
}
}
}
}