vzense_rust/dcam560/
device.rs

1//! Basic routines to initialize or shut down a device and to set/get parameters.
2
3use std::{ffi::CStr, os::raw::c_char, thread::sleep, time::Duration};
4
5use sys::PsReturnStatus_PsRetOK as OK;
6use vzense_sys::dcam560 as sys;
7
8use crate::{
9    ColorFormat, ColorResolution, DataMode, DepthMeasuringRange, Resolution, cyan, red, yellow,
10};
11
12use super::SESSION_INDEX;
13
14/// The main interface to the camera.
15pub struct Device {
16    pub(super) handle: sys::PsDeviceHandle,
17    pub(super) frame_ready: sys::PsFrameReady,
18    pub(super) frame: sys::PsFrame,
19    pub(super) color_resolution: ColorResolution,
20    pub(super) color_is_mapped: bool,
21    pub(super) depth_is_mapped: bool,
22    pub(super) current_frame_is_depth: bool,
23    pub(super) min_depth_mm: u16,
24    pub(super) max_depth_mm: u16,
25}
26impl Device {
27    /// Initializes the sytem and returns a device if it finds one. Make sure a Vzense camera is connected. `scan_time` should be at least one second to find a device. Set `scan_time = Duration::MAX` to scan until a device was found (useful to wait for reconnection after the connection to a device was interrupted).
28    pub fn initialize(scan_time: Duration, verbose: bool) -> Result<Self, String> {
29        initialize(verbose)?;
30
31        let device_count = get_device_count(scan_time, verbose)?;
32
33        let mut device = Device::open_device_by_ip(get_ip(device_count)?)?;
34
35        if verbose {
36            let info = device.get_device_info(device_count)?;
37            println!(
38                "{}",
39                cyan!("model: {}, IP: {}, firmware: {}", info[0], info[1], info[2])
40            );
41        }
42
43        device.start_stream(verbose)?;
44
45        device.set_color_resolution(ColorResolution::Res640x480);
46
47        Ok(device)
48    }
49
50    /// Choosing the min/max depth in mm for the color mapping of the depth output. These values also bound the depths used in the `util::TochDetector` to reduce measuring artifacts.
51    pub fn set_depth_range(&mut self, min_depth_mm: u16, max_depth_mm: u16) -> Result<(), String> {
52        if min_depth_mm >= max_depth_mm {
53            return Err(red!("Input Error: min depth must be less than max depth"));
54        }
55        self.min_depth_mm = min_depth_mm;
56        self.max_depth_mm = max_depth_mm;
57        Ok(())
58    }
59
60    /// Get the current frame rate of the camera.
61    pub fn get_frame_rate(&self) -> Result<u8, String> {
62        let mut rate = 0;
63        let status = unsafe { sys::Ps2_GetTofFrameRate(self.handle, SESSION_INDEX, &mut rate) };
64        if status != OK {
65            return Err(red!("get frame rate failed with status {}", status));
66        }
67        Ok(rate)
68    }
69
70    /// Set the ToF frame rate. Different devices have different maximum values. Please refer to the device specification.
71    pub fn set_frame_rate(&self, rate: u8) -> Result<(), String> {
72        let status = unsafe { sys::Ps2_SetTofFrameRate(self.handle, SESSION_INDEX, rate) };
73        if status != OK {
74            return Err(red!("set frame rate failed with status {}", status));
75        }
76        Ok(())
77    }
78
79    /// Get frame info like frame type, pixel format, width, height, etc.
80    pub fn get_frame_info(&self) -> String {
81        format!("{:?}", self.frame)
82    }
83
84    /// Checks if the number of pixels in `frame` equals `pixel_count`.
85    pub fn check_pixel_count(&self, pixel_count: usize) {
86        let w = self.frame.width as usize;
87        let h = self.frame.height as usize;
88        if w * h != pixel_count {
89            println!("{}", red!("!!! pixel count is not equal to {} * {}", w, h))
90        }
91    }
92
93    /// Set the color frame format to either RGB or BGR.
94    pub fn set_color_format(&self, format: ColorFormat) {
95        unsafe {
96            match format {
97                ColorFormat::Rgb => sys::Ps2_SetColorPixelFormat(
98                    self.handle,
99                    SESSION_INDEX,
100                    sys::PsPixelFormat_PsPixelFormatRGB888,
101                ),
102                ColorFormat::Bgr => sys::Ps2_SetColorPixelFormat(
103                    self.handle,
104                    SESSION_INDEX,
105                    sys::PsPixelFormat_PsPixelFormatBGR888,
106                ),
107            };
108        }
109    }
110
111    /// Enable or disable the mapping of the color image to depth camera space.
112    pub fn map_color_to_depth(&mut self, is_enabled: bool) {
113        if self.color_resolution != ColorResolution::Res640x480 {
114            self.set_color_resolution(ColorResolution::Res640x480);
115            self.color_resolution = ColorResolution::Res640x480;
116        }
117        unsafe {
118            // should actually be `Ps2_SetMapperEnabledRGBToDepth` but the names seem to be mixed up
119            sys::Ps2_SetMapperEnabledDepthToRGB(
120                self.handle,
121                SESSION_INDEX,
122                if is_enabled { 1 } else { 0 },
123            );
124        }
125        self.color_is_mapped = is_enabled;
126    }
127
128    /// Enable or disable the mapping of the depth image to color camera space.
129    pub fn map_depth_to_color(&mut self, is_enabled: bool) {
130        if self.color_resolution != ColorResolution::Res640x480 {
131            self.set_color_resolution(ColorResolution::Res640x480);
132            self.color_resolution = ColorResolution::Res640x480;
133        }
134        unsafe {
135            // should actually be `Ps2_SetMapperEnabledDepthToRGB` but the names seem to be mixed up
136            sys::Ps2_SetMapperEnabledRGBToDepth(
137                self.handle,
138                SESSION_INDEX,
139                if is_enabled { 1 } else { 0 },
140            );
141        }
142        self.depth_is_mapped = is_enabled;
143    }
144
145    /// Sets the resolution of the color frame. Three resolutions are currently available: 640x480, 800x600, and 1600x1200.
146    pub fn set_color_resolution(&mut self, resolution: ColorResolution) -> Resolution {
147        if self.color_is_mapped || self.depth_is_mapped {
148            println!(
149                "{}",
150                yellow!(
151                    "setting of color resolution is ignored because color frame is mapped to depth or vice versa"
152                )
153            );
154        } else {
155            let res = match resolution {
156                ColorResolution::Res640x480 => sys::PsResolution_PsRGB_Resolution_640_480,
157                ColorResolution::Res800x600 => sys::PsResolution_PsRGB_Resolution_800_600,
158                ColorResolution::Res1600x1200 => sys::PsResolution_PsRGB_Resolution_1600_1200,
159            };
160            unsafe {
161                sys::Ps2_SetRGBResolution(self.handle, SESSION_INDEX, res);
162            }
163            self.color_resolution = resolution;
164        }
165        self.get_color_resolution()
166    }
167
168    /// Returns the resolution of the color frame.
169    pub fn get_color_resolution(&self) -> Resolution {
170        let mut resolution_type = 0;
171        unsafe {
172            sys::Ps2_GetRGBResolution(self.handle, SESSION_INDEX, &mut resolution_type);
173        }
174        match resolution_type {
175            2 => Resolution::new(640, 480),
176            5 => Resolution::new(800, 600),
177            4 => Resolution::new(1600, 1200),
178            _ => panic!("{}", red!("unknown rgb resolution")),
179        }
180    }
181
182    /// Sets the depth range mode.
183    pub fn set_depth_measuring_range(&self, depth_range: DepthMeasuringRange) {
184        let depth_range = match depth_range {
185            DepthMeasuringRange::Near => sys::PsDepthRange_PsNearRange,
186            DepthMeasuringRange::Mid => sys::PsDepthRange_PsMidRange,
187            DepthMeasuringRange::Far => sys::PsDepthRange_PsFarRange,
188        };
189        unsafe {
190            sys::Ps2_SetDepthRange(self.handle, SESSION_INDEX, depth_range);
191        }
192    }
193
194    /// Returns the current depth measuring range `(min, max)` of the camera in mm.
195    pub fn get_depth_measuring_range(&self) -> (u16, u16) {
196        let mut depth_range = sys::PsDepthRange::default();
197
198        unsafe {
199            sys::Ps2_GetDepthRange(self.handle, SESSION_INDEX, &mut depth_range);
200        }
201
202        let mut mr = sys::PsMeasuringRange::default();
203
204        unsafe {
205            sys::Ps2_GetMeasuringRange(self.handle, SESSION_INDEX, depth_range, &mut mr);
206        }
207
208        match depth_range {
209            sys::PsDepthRange_PsNearRange => (mr.effectDepthMinNear, mr.effectDepthMaxNear),
210            sys::PsDepthRange_PsMidRange => (mr.effectDepthMinMid, mr.effectDepthMaxMid),
211            sys::PsDepthRange_PsFarRange => (mr.effectDepthMinFar, mr.effectDepthMaxFar),
212            _ => panic!("{}", red!("unknown measuring range")),
213        }
214    }
215
216    /// Set the wait time for the call to `read_next_frame` in ms.
217    pub fn set_wait_time(&self, time: u16) {
218        unsafe {
219            sys::Ps2_SetWaitTimeOfReadNextFrame(self.handle, SESSION_INDEX, time);
220        }
221    }
222
223    /// Set data mode
224    pub fn set_data_mode(&self, mode: DataMode) {
225        let mode = match mode {
226            DataMode::DepthAndRGB => sys::PsDataMode_PsDepthAndRGB_30,
227            DataMode::IRAndRGB => sys::PsDataMode_PsIRAndRGB_30,
228            DataMode::DepthAndIRAndRGB => sys::PsDataMode_PsDepthAndIRAndRGB_30,
229        };
230
231        unsafe {
232            sys::Ps2_SetDataMode(self.handle, SESSION_INDEX, mode);
233        }
234    }
235
236    /// Current data mode.
237    pub fn get_data_mode(&self) -> Result<DataMode, String> {
238        let mut data_mode = sys::PsDataMode::default();
239        let status = unsafe { sys::Ps2_GetDataMode(self.handle, SESSION_INDEX, &mut data_mode) };
240        if status != OK {
241            return Err(red!("get data mode failed with status {}", status));
242        }
243        match data_mode {
244            sys::PsDataMode_PsDepthAndRGB_30 => Ok(DataMode::DepthAndRGB),
245            sys::PsDataMode_PsIRAndRGB_30 => Ok(DataMode::IRAndRGB),
246            sys::PsDataMode_PsDepthAndIRAndRGB_30 => Ok(DataMode::DepthAndIRAndRGB),
247            _ => Err(red!("unknown data mode")),
248        }
249    }
250
251    /// Stops the stream, closes the device, and clears all resources.
252    pub fn shut_down(&mut self, verbose: bool) {
253        unsafe {
254            sys::Ps2_StopStream(self.handle, SESSION_INDEX);
255            sys::Ps2_CloseDevice(&mut self.handle);
256
257            let status = sys::Ps2_Shutdown();
258            if status != OK {
259                println!("{}", red!("shut down failed with status: {}", status));
260            } else if verbose {
261                println!("shut down device successfully");
262            }
263        }
264    }
265
266    /// Returns device info as an array of Strings: \[model, IP, firmware, serial number\]
267    pub fn get_device_info(&self, device_count: u32) -> Result<[String; 4], String> {
268        if device_count == 0 {
269            return Err(red!("no device to get info for"));
270        }
271
272        let mut device_info = sys::PsDeviceInfo::default();
273
274        unsafe { sys::Ps2_GetDeviceListInfo(&mut device_info, device_count) };
275
276        let ip = device_info.ip.as_ptr();
277        let uri = device_info.uri.as_ptr(); // model_name:serial_number
278        let serial = device_info.alias.as_ptr(); // serial number
279
280        let firmware = self
281            .get_firmware_version()
282            .expect(&red!("Cannot get firmware version"));
283
284        Ok([
285            unsafe { CStr::from_ptr(uri) }
286                .to_str()
287                .unwrap()
288                .split(":")
289                .collect::<Vec<&str>>()[0]
290                .to_string(),
291            unsafe { CStr::from_ptr(ip) }.to_string_lossy().into_owned(),
292            firmware,
293            unsafe { CStr::from_ptr(serial) }
294                .to_string_lossy()
295                .into_owned(),
296        ])
297    }
298
299    // private functions_______________________________________________________
300
301    fn get_firmware_version(&self) -> Result<String, String> {
302        let mut buffer = [0; 64];
303        match get_firmware_version(self.handle, &mut buffer) {
304            OK => Ok(CStr::from_bytes_until_nul(&buffer)
305                .unwrap()
306                .to_string_lossy()
307                .into_owned()),
308            error_code => Err(format!("{}", error_code)),
309        }
310    }
311
312    fn open_device_by_ip(ip: *const c_char) -> Result<Self, String> {
313        let mut handle = 0 as sys::PsDeviceHandle;
314        let status = unsafe { sys::Ps2_OpenDeviceByIP(ip, &mut handle) };
315        if status != OK {
316            return Err(red!("open device failed with status {}", status));
317        }
318        if !handle.is_null() {
319            Ok(Device {
320                handle,
321                frame_ready: sys::PsFrameReady::default(),
322                frame: sys::PsFrame::default(),
323                color_resolution: ColorResolution::Res640x480,
324                color_is_mapped: false,
325                depth_is_mapped: false,
326                current_frame_is_depth: false,
327                min_depth_mm: 500,  // default value
328                max_depth_mm: 1000, // default value
329            })
330        } else {
331            Err(red!("device ptr is null"))
332        }
333    }
334
335    fn start_stream(&self, verbose: bool) -> Result<(), String> {
336        let status = unsafe { sys::Ps2_StartStream(self.handle, SESSION_INDEX) };
337        if status != OK {
338            return Err(red!("start stream failed with status {}", status));
339        }
340        if verbose {
341            println!("stream started")
342        }
343        Ok(())
344    }
345
346    #[deprecated(since = "0.3.0", note = "Use initialize(scan_time, verbose) instead.")]
347    pub fn init() -> Result<Self, String> {
348        Err("Deprecated, use initialize(scan_time, verbose) instead".to_string())
349    }
350}
351
352/// `Data` trait to allow use of `Device` in `util::TouchDetector`.
353impl crate::util::touch_detector::Data for Device {
354    fn get_frame_p_frame_data(&self) -> *mut u8 {
355        self.frame.pFrameData
356    }
357    fn get_frame_data_len(&self) -> usize {
358        self.frame.dataLen as usize
359    }
360    fn get_min_depth_mm(&self) -> u16 {
361        self.min_depth_mm
362    }
363    fn get_max_depth_mm(&self) -> u16 {
364        self.max_depth_mm
365    }
366    fn current_frame_is_depth(&self) -> bool {
367        self.current_frame_is_depth
368    }
369}
370
371fn get_firmware_version(handle: sys::PsDeviceHandle, buffer: &mut [u8]) -> sys::PsReturnStatus {
372    let len = buffer.len().try_into().unwrap();
373    let ptr: *mut c_char = buffer.as_mut_ptr().cast();
374    unsafe { sys::Ps2_GetFirmwareVersionNumber(handle, SESSION_INDEX, ptr, len) }
375}
376
377fn initialize(verbose: bool) -> Result<(), String> {
378    if verbose {
379        println!("initializing...");
380    }
381    let status = unsafe { sys::Ps2_Initialize() };
382
383    // status -101 is reinitialization
384    if status == -101 {
385        if verbose {
386            println!("reinitializing...");
387        }
388    } else if status != OK {
389        return Err(red!("initialization failed with status {}", status));
390    }
391    Ok(())
392}
393
394/// Tries to find devices every 200 ms for duration `scan_time`.
395fn get_device_count(scan_time: Duration, verbose: bool) -> Result<u32, String> {
396    if scan_time < Duration::from_secs(1) {
397        println!(
398            "{}",
399            yellow!("vzense-rust warning: scan time might be too short to detect a device")
400        );
401    }
402    let sleep_interval = Duration::from_millis(200);
403    let try_count = scan_time.div_duration_f64(sleep_interval).ceil() as u64;
404
405    let mut device_count = 0;
406    let mut times_tried = 0;
407    let mut status;
408    if verbose {
409        println!("searching for device...");
410    }
411    loop {
412        status = unsafe { sys::Ps2_GetDeviceCount(&mut device_count) };
413
414        if status != OK {
415            return Err(red!("get device count failed with status {}", status));
416        } else {
417            if device_count > 0 {
418                if verbose {
419                    println!("{}", cyan!("device found"));
420                }
421                break;
422            }
423            times_tried += 1;
424            if times_tried >= try_count {
425                return Err(red!("no device found"));
426            }
427            sleep(sleep_interval);
428        }
429    }
430    Ok(device_count)
431}
432
433fn get_ip(device_count: u32) -> Result<*const c_char, String> {
434    let mut device_info = sys::PsDeviceInfo::default();
435    unsafe {
436        let status = sys::Ps2_GetDeviceListInfo(&mut device_info, device_count);
437        if status != OK {
438            return Err(red!("get device list info failed with status {}", status));
439        }
440    }
441    Ok(device_info.ip.as_ptr())
442}
vzense_rust/dcam560/device.rs

vzense_rust/dcam560/
device.rs
