vzense_rust/scepter/
device.rs

1//! Basic routines to initialize or shut down a device and to set/get parameters.
2
3use std::os::raw::c_char;
4use std::{ffi::CStr, time::Duration};
5use sys::ScStatus_SC_OK as OK;
6
7use vzense_sys::scepter as sys;
8
9use crate::{ColorFormat, ColorResolution, Resolution, cyan, red, yellow};
10
11use super::get_message;
12
13/// The main interface to the camera.
14pub struct Device {
15    pub(super) handle: sys::ScDeviceHandle,
16    pub(super) frame_ready: sys::ScFrameReady,
17    pub(super) frame: sys::ScFrame,
18    pub(super) color_resolution: ColorResolution,
19    pub(super) color_is_mapped: bool,
20    pub(super) depth_is_mapped: bool,
21    pub(super) current_frame_is_depth: bool,
22    pub(super) min_depth_mm: u16,
23    pub(super) max_depth_mm: u16,
24}
25impl Device {
26    /// 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).
27    pub fn initialize(scan_time: Duration, verbose: bool) -> Result<Self, String> {
28        initialize(verbose)?;
29
30        let device_count = get_device_count(scan_time, verbose)?;
31
32        let mut device = Device::open_device_by_ip(get_ip(device_count)?)?;
33
34        if verbose {
35            let info = device.get_device_info(device_count)?;
36            println!(
37                "{}",
38                cyan!("model: {}, IP: {}, firmware: {}", info[0], info[1], info[2])
39            );
40        }
41
42        device.start_stream(verbose)?;
43
44        device.set_color_resolution(ColorResolution::Res640x480);
45
46        Ok(device)
47    }
48
49    /// 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.
50    pub fn set_depth_range(&mut self, min_depth_mm: u16, max_depth_mm: u16) -> Result<(), String> {
51        if min_depth_mm >= max_depth_mm {
52            return Err(red!("Input Error: min depth must be less than max depth"));
53        }
54        self.min_depth_mm = min_depth_mm;
55        self.max_depth_mm = max_depth_mm;
56        Ok(())
57    }
58
59    /// Get the current frame rate of the camera.
60    pub fn get_frame_rate(&self) -> Result<u8, String> {
61        let mut rate = 0;
62        let status = unsafe { sys::scGetFrameRate(self.handle, &mut rate) };
63        if status != OK {
64            return Err(red!(
65                "get frame rate failed with status {}",
66                get_message(status)
67            ));
68        }
69        Ok(rate as u8)
70    }
71
72    /// Set the ToF frame rate. The interface takes a long time, about 500 ms. Different devices have different maximum values. Please refer to the device specification.
73    pub fn set_frame_rate(&self, rate: u8) -> Result<(), String> {
74        let status = unsafe { sys::scSetFrameRate(self.handle, rate as i32) };
75        if status != OK {
76            if status == -105 {
77                println!(
78                    "{}",
79                    yellow!("Frame rate is probably set above the maximum possible value.")
80                );
81            }
82            return Err(red!(
83                "set frame rate failed with status {} {}",
84                get_message(status),
85                status
86            ));
87        }
88        Ok(())
89    }
90
91    /// Get frame info like frame type, pixel format, width, height, etc.
92    pub fn get_frame_info(&self) -> String {
93        format!("{:?}", self.frame)
94    }
95
96    /// Checks if the number of pixels in `frame` equals `pixel_count`.
97    pub fn check_pixel_count(&self, pixel_count: usize) {
98        let w = self.frame.width as usize;
99        let h = self.frame.height as usize;
100        if w * h != pixel_count {
101            println!("{}", red!("!!! pixel count is not equal to {} * {}", w, h))
102        }
103    }
104
105    /// Set the color frame format to either RGB or BGR.
106    pub fn set_color_format(&self, format: ColorFormat) {
107        unsafe {
108            match format {
109                ColorFormat::Rgb => sys::scSetColorPixelFormat(
110                    self.handle,
111                    sys::ScPixelFormat_SC_PIXEL_FORMAT_RGB_888,
112                ),
113                ColorFormat::Bgr => sys::scSetColorPixelFormat(
114                    self.handle,
115                    sys::ScPixelFormat_SC_PIXEL_FORMAT_BGR_888,
116                ),
117            };
118        }
119    }
120
121    /// Enable or disable the mapping of the color image to depth camera space.
122    pub fn map_color_to_depth(&mut self, is_enabled: bool) {
123        if self.color_resolution != ColorResolution::Res640x480 {
124            self.set_color_resolution(ColorResolution::Res640x480);
125            self.color_resolution = ColorResolution::Res640x480;
126        }
127        unsafe {
128            sys::scSetTransformColorImgToDepthSensorEnabled(
129                self.handle,
130                if is_enabled { 1 } else { 0 },
131            );
132        }
133        self.color_is_mapped = is_enabled;
134    }
135
136    /// Enable or disable the mapping of the depth image to color camera space.
137    pub fn map_depth_to_color(&mut self, is_enabled: bool) {
138        if self.color_resolution != ColorResolution::Res640x480 {
139            self.set_color_resolution(ColorResolution::Res640x480);
140            self.color_resolution = ColorResolution::Res640x480;
141        }
142        unsafe {
143            sys::scSetTransformDepthImgToColorSensorEnabled(
144                self.handle,
145                if is_enabled { 1 } else { 0 },
146            );
147        }
148        self.depth_is_mapped = is_enabled;
149    }
150
151    /// Sets the resolution of the color frame and also returns it. Three resolutions are currently available: 640x480, 800x600, and 1600x1200.
152    pub fn set_color_resolution(&mut self, resolution: ColorResolution) -> Resolution {
153        if self.color_is_mapped || self.depth_is_mapped {
154            println!(
155                "{}",
156                yellow!(
157                    "setting of color resolution is ignored because color frame is mapped to depth or vice versa"
158                )
159            );
160        } else {
161            unsafe {
162                match resolution {
163                    ColorResolution::Res640x480 => sys::scSetColorResolution(self.handle, 640, 480),
164                    ColorResolution::Res800x600 => sys::scSetColorResolution(self.handle, 800, 600),
165                    ColorResolution::Res1600x1200 => {
166                        sys::scSetColorResolution(self.handle, 1600, 1200)
167                    }
168                };
169            }
170            self.color_resolution = resolution;
171        }
172        self.get_color_resolution()
173    }
174
175    /// Returns the resolution of the color frame.
176    pub fn get_color_resolution(&self) -> Resolution {
177        let mut w = 0;
178        let mut h = 0;
179        unsafe {
180            sys::scGetColorResolution(self.handle, &mut w, &mut h);
181        }
182        Resolution::new(w as u32, h as u32)
183    }
184
185    /// Current work mode.
186    pub fn get_work_mode(&self) -> Result<u32, String> {
187        let mut work_mode = sys::ScWorkMode::default();
188        let status = unsafe { sys::scGetWorkMode(self.handle, &mut work_mode) };
189        if status != OK {
190            return Err(red!(
191                "get work mode failed with status {}",
192                get_message(status)
193            ));
194        }
195        Ok(work_mode)
196    }
197
198    /// Stops the stream, closes the device, and clears all resources.
199    pub fn shut_down(&mut self, verbose: bool) {
200        unsafe {
201            sys::scStopStream(self.handle);
202            sys::scCloseDevice(&mut self.handle);
203
204            let status = sys::scShutdown();
205            if status != OK {
206                println!(
207                    "{}",
208                    red!("shut down failed with status: {}", get_message(status))
209                );
210            } else if verbose {
211                println!("shut down device successfully");
212            }
213        }
214    }
215
216    /// Returns device info as an array of Strings: \[model, IP, firmware, serial number\]
217    pub fn get_device_info(&self, device_count: u32) -> Result<[String; 4], String> {
218        if device_count == 0 {
219            return Err(red!("no device to get info for"));
220        }
221
222        let mut device_info = sys::ScDeviceInfo::default();
223
224        unsafe { sys::scGetDeviceInfoList(device_count, &mut device_info) };
225
226        let ip = device_info.ip.as_ptr();
227        let model = device_info.productName.as_ptr();
228        let serial = device_info.serialNumber.as_ptr();
229
230        let firmware = self
231            .get_firmware_version()
232            .expect(&red!("cannot get firmware version"));
233
234        Ok([
235            unsafe { CStr::from_ptr(model) }
236                .to_string_lossy()
237                .into_owned(),
238            unsafe { CStr::from_ptr(ip) }.to_string_lossy().into_owned(),
239            firmware,
240            unsafe { CStr::from_ptr(serial) }
241                .to_string_lossy()
242                .into_owned(),
243        ])
244    }
245
246    // private functions_______________________________________________________
247
248    fn get_firmware_version(&self) -> Result<String, String> {
249        let mut buffer = [0; 64];
250        match get_firmware_version(self.handle, &mut buffer) {
251            OK => Ok(CStr::from_bytes_until_nul(&buffer)
252                .unwrap()
253                .to_string_lossy()
254                .into_owned()),
255            error_code => Err(format!("{}", error_code)),
256        }
257    }
258
259    fn open_device_by_ip(ip: *const c_char) -> Result<Self, String> {
260        let mut handle = 0 as sys::ScDeviceHandle;
261        let status = unsafe { sys::scOpenDeviceByIP(ip, &mut handle) };
262        if status != OK {
263            return Err(red!(
264                "open device failed with status {}",
265                get_message(status)
266            ));
267        }
268        if !handle.is_null() {
269            Ok(Device {
270                handle,
271                frame_ready: sys::ScFrameReady::default(),
272                frame: sys::ScFrame::default(),
273                color_resolution: ColorResolution::Res640x480,
274                color_is_mapped: false,
275                depth_is_mapped: false,
276                current_frame_is_depth: false,
277                min_depth_mm: 500,  // default value
278                max_depth_mm: 1000, // default value
279            })
280        } else {
281            Err(red!("device ptr is null"))
282        }
283    }
284
285    fn start_stream(&self, verbose: bool) -> Result<(), String> {
286        let status = unsafe { sys::scStartStream(self.handle) };
287        if status != OK {
288            return Err(red!(
289                "start stream failed with status {}",
290                get_message(status)
291            ));
292        }
293        if verbose {
294            println!("stream started")
295        }
296        Ok(())
297    }
298
299    /// Returns the current depth measuring range `(min, max)` of the camera in mm. **Note**: At least the min value seems to have no practical meaning. For the NYX650 the returned min value is 1 mm which makes no sense, while the max value is 4700 mm. In the specs the depth range for the NYX650 is given as min: 300 mm, max: 4500 mm.
300    #[deprecated]
301    pub fn get_depth_measuring_range(&self) -> (u16, u16) {
302        unsafe {
303            let mut min = 0;
304            let mut max = 0;
305            sys::scGetDepthRangeValue(self.handle, &mut min, &mut max);
306            (min as u16, max as u16)
307        }
308    }
309
310    #[deprecated(since = "0.3.0", note = "Use initialize(scan_time, verbose) instead.")]
311    pub fn init() -> Result<Self, String> {
312        Err("Deprecated, use initialize(scan_time, verbose) instead".to_string())
313    }
314}
315
316/// `Data` trait to allow use of `Device` in `util::TouchDetector`.
317impl crate::util::touch_detector::Data for Device {
318    fn get_frame_p_frame_data(&self) -> *mut u8 {
319        self.frame.pFrameData
320    }
321    fn get_frame_data_len(&self) -> usize {
322        self.frame.dataLen as usize
323    }
324    fn get_min_depth_mm(&self) -> u16 {
325        self.min_depth_mm
326    }
327    fn get_max_depth_mm(&self) -> u16 {
328        self.max_depth_mm
329    }
330    fn current_frame_is_depth(&self) -> bool {
331        self.current_frame_is_depth
332    }
333}
334
335fn get_firmware_version(handle: sys::ScDeviceHandle, buffer: &mut [u8]) -> sys::ScStatus {
336    let len = buffer.len().try_into().unwrap();
337    let ptr: *mut c_char = buffer.as_mut_ptr().cast();
338    unsafe { sys::scGetFirmwareVersion(handle, ptr, len) }
339}
340
341fn initialize(verbose: bool) -> Result<(), String> {
342    if verbose {
343        println!("initializing...");
344    }
345    let status = unsafe { sys::scInitialize() };
346
347    // status -101 is reinitialization
348    if status == -101 {
349        if verbose {
350            println!("reinitializing...");
351        }
352    } else if status != OK {
353        return Err(red!(
354            "initialization failed with status {}",
355            get_message(status)
356        ));
357    }
358    Ok(())
359}
360
361/// Tries to find devices every 200 ms for duration `scan_time`.
362fn get_device_count(scan_time: Duration, verbose: bool) -> Result<u32, String> {
363    if scan_time < Duration::from_secs(1) {
364        println!(
365            "{}",
366            yellow!("vzense-rust warning: scan time might be too short to detect a device")
367        );
368    }
369    let scan_interval = Duration::from_millis(200);
370    let try_count = scan_time.div_duration_f64(scan_interval).ceil() as u64;
371
372    let mut device_count = 0;
373    let mut times_tried = 0;
374    let mut status;
375    if verbose {
376        println!("searching for device...");
377    }
378    loop {
379        status = unsafe { sys::scGetDeviceCount(&mut device_count, 200) };
380
381        if status != OK {
382            return Err(red!(
383                "get device count failed with status {}",
384                get_message(status)
385            ));
386        } else {
387            if device_count > 0 {
388                if verbose {
389                    println!("{}", cyan!("device found"));
390                }
391                break;
392            }
393            times_tried += 1;
394            if times_tried >= try_count {
395                return Err(red!("no device found"));
396            }
397        }
398    }
399    Ok(device_count)
400}
401
402fn get_ip(device_count: u32) -> Result<*const c_char, String> {
403    let mut device_info = sys::ScDeviceInfo::default();
404    unsafe {
405        let status = sys::scGetDeviceInfoList(device_count, &mut device_info);
406        if status != OK {
407            return Err(red!(
408                "get device list info failed with status {}",
409                get_message(status)
410            ));
411        }
412    }
413    Ok(device_info.ip.as_ptr())
414}
vzense_rust/scepter/device.rs

vzense_rust/scepter/
device.rs
