diff --git a/src/capture/capture_buffer.rs b/src/capture/capture_buffer.rs new file mode 100644 index 0000000..379562a --- /dev/null +++ b/src/capture/capture_buffer.rs @@ -0,0 +1,46 @@ +use kinect::Capture; +use std::sync::{RwLock, Arc, PoisonError, RwLockWriteGuard, Mutex}; +use crate::AnyhowResult; +use anyhow::anyhow; + +pub struct CaptureBuffer { + num_devices: usize, + captures: Arc>>, +} + +impl CaptureBuffer { + + pub fn new(num_devices: usize) -> Self { + CaptureBuffer { + num_devices, + captures: Arc::new(Mutex::new(Vec::with_capacity(num_devices))) + } + } + + pub fn set_captures(&self) -> AnyhowResult<()> { + /*if captures.len() != self.num_devices { + return Err(anyhow!("Number of captures ({}) != number of devices ({})", + captures.len(), self.num_devices)); + }*/ + + /*match self.captures.write() { + Err(err) => { + return Err(anyhow!("Lock error: {:?}", err)) + }, + Ok(mut caps) => { + caps.clear(); + caps.extend(captures); + }, + }*/ + + match self.captures.lock() { + Ok(mut lock) => { + //*lock = captures; + lock.clear(); + }, + Err(_) => {}, + } + + Ok(()) + } +} \ No newline at end of file diff --git a/src/capture/capture_ensemble.rs b/src/capture/capture_ensemble.rs new file mode 100644 index 0000000..78b929d --- /dev/null +++ b/src/capture/capture_ensemble.rs @@ -0,0 +1,5 @@ +use kinect::Capture; + +pub struct CaptureEnsemble { + pub captures: Vec, +} \ No newline at end of file diff --git a/src/capture/mod.rs b/src/capture/mod.rs new file mode 100644 index 0000000..5fd1f99 --- /dev/null +++ b/src/capture/mod.rs @@ -0,0 +1,3 @@ +pub mod capture_buffer; +pub mod capture_ensemble; +pub mod multi_device_capturer; \ No newline at end of file diff --git a/src/capture/multi_device_capturer.rs b/src/capture/multi_device_capturer.rs new file mode 100644 index 0000000..f51b8c8 --- /dev/null +++ b/src/capture/multi_device_capturer.rs @@ -0,0 +1,49 @@ +use kinect::{Device, Capture}; +use crate::capture::capture_buffer::CaptureBuffer; +use crate::AnyhowResult; +use std::sync::Arc; + +const WAIT_INFINITE : i32 = -1; + +pub struct MultiDeviceCapturer { + devices: Vec, + capture_buffer: Arc, +} + +impl MultiDeviceCapturer { + + pub fn new(devices: Vec) -> Self { + Self { + capture_buffer: Arc::new(CaptureBuffer::new(devices.len())), + devices, + } + } + + pub fn get_capture_buffer(&self) -> Arc { + self.capture_buffer.clone() + } + + pub fn run_capture_loop(&self) { + loop { + match self.get_synchronized_captures() { + Ok(captures) => { + self.capture_buffer.set_captures(); + }, + Err(e) => { + println!("Error with captures: {:?}", e); + continue; + } + } + } + } + + fn get_synchronized_captures(&self) -> AnyhowResult> { + let mut captures = Vec::with_capacity(self.devices.len()); + for device in self.devices.iter() { + let capture = device.get_capture(WAIT_INFINITE)?; + captures.push(capture); + } + // TODO: Apparently there's some sync logic that goes here. + Ok(captures) + } +} \ No newline at end of file diff --git a/src/main_zeromq.rs b/src/main_zeromq.rs index f848911..d12d71d 100644 --- a/src/main_zeromq.rs +++ b/src/main_zeromq.rs @@ -1,9 +1,11 @@ //! Generate random points to send over ZeroMQ to a consumer +#![cfg_attr(debug_assertions, allow(dead_code, unused_imports))] #[allow(unused_imports)] #[macro_use] extern crate clap; +mod capture; mod pointcloud; mod setup; mod zeromq; @@ -30,6 +32,8 @@ use zmq::{Error, Socket, Context, DONTWAIT}; use crate::zeromq::protocol::{encode_point_data, send_message}; use crate::setup::open_devices::open_and_configure_camera; use crate::setup::camera_info::CameraInfo; +use crate::capture::multi_device_capturer::MultiDeviceCapturer; +use std::sync::{Arc, RwLock}; const SOCKET_ADDRESS : &'static str = "tcp://127.0.0.1:8888"; @@ -101,22 +105,34 @@ fn main() -> AnyhowResult<()> { println!("Opening device..."); thread::sleep(Duration::from_millis(100)); - let mut cameras: Vec = Vec::with_capacity(args.num_cameras as usize); - - for index in 0 .. args.num_cameras { - let camera_info = open_and_configure_camera(&args, index as u32)?; - cameras.push(camera_info); - } + let mut devices = Vec::with_capacity(args.num_cameras as usize); + let mut camera_infos: Vec = Vec::with_capacity(args.num_cameras as usize); println!("Starting cameras with supplied config..."); - thread::sleep(Duration::from_millis(100)); + for index in 0 .. args.num_cameras { + thread::sleep(Duration::from_millis(100)); - for camera in cameras.iter() { - camera.device.start_cameras(&camera.config)?; + let (device, camera_info) = open_and_configure_camera(&args, index as u32)?; + + device.start_cameras(&camera_info.config)?; + + devices.push(device); + camera_infos.push(camera_info); } + let multi_device_capturer = MultiDeviceCapturer::new(devices); + + thread::spawn(move || { + multi_device_capturer.run_capture_loop(); + }); + // =============================================================== + loop { + thread::sleep(Duration::from_millis(5000)); + } + + /* let context = zmq::Context::new(); println!("Connecting ZeroMQ socket..."); @@ -146,6 +162,8 @@ fn main() -> AnyhowResult<()> { break; }; + println!("Main loop begin..."); + let mut packet_number = 0; let mut frame_number : u64 = 0; @@ -154,7 +172,13 @@ fn main() -> AnyhowResult<()> { MessagingState::GrabPointCloud => { //println!("Grabbing another frame..."); color = Color::get_time_based_color(); - points = get_point_cloud(&cameras, color, &args)?; + points = match get_point_cloud(&cameras, color, &args) { + Ok(p) => p, + Err(e) => { + thread::sleep(Duration::from_millis(50)); + continue + }, + }; messaging_state = MessagingState::Sending_PointDataBegin; frame_number += 1; @@ -197,6 +221,7 @@ fn main() -> AnyhowResult<()> { }, } } + */ Ok(()) } @@ -209,14 +234,20 @@ fn get_point_cloud( { let mut points = Vec::with_capacity(10000); + /*let mut index = 0; for camera in cameras.iter() { let capture = camera.device.get_capture(5000)?; - let depth_image = capture.get_depth_image() - .ok_or(anyhow!("depth image not present"))?; - let color_image = capture.get_color_image() - .ok_or(anyhow!("color image not present"))?; + let depth_image = match capture.get_depth_image() { + Some(img) => img, + None => continue, + }; + + let color_image = match capture.get_color_image() { + Some(img) => img, + None => continue, + }; let depth_image2 = camera.transformer.transform(&depth_image)?; @@ -229,10 +260,17 @@ fn get_point_cloud( command_args, )?; + //camera_points.truncate(6000); + + println!("Points (camera {}): {}", index, camera_points.len()); points.extend(camera_points); - //println!("Points: {}", camera_points.len()); + index += 1; } + if points.is_empty() { + return Err(anyhow!("No points captured!")); + }*/ + Ok(points) } diff --git a/src/setup/camera_info.rs b/src/setup/camera_info.rs index a97ce63..841c78b 100644 --- a/src/setup/camera_info.rs +++ b/src/setup/camera_info.rs @@ -1,8 +1,8 @@ use kinect::{Device, Calibration, Image, DeviceConfiguration}; use crate::pointcloud::cpu_transformation::DepthTransformer; +/// Extra metadata about a camera. pub struct CameraInfo { - pub device: Device, pub config: DeviceConfiguration, pub calibration: Calibration, pub xy_table: Image, diff --git a/src/setup/open_devices.rs b/src/setup/open_devices.rs index 21368b3..10436c3 100644 --- a/src/setup/open_devices.rs +++ b/src/setup/open_devices.rs @@ -6,7 +6,7 @@ use crate::pointcloud::cpu_transformation::DepthTransformer; use crate::AnyhowResult; use k4a_sys_temp as k4a_sys; -pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult +pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult<(Device, CameraInfo)> { let device = Device::open(index)?; @@ -37,11 +37,12 @@ pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult let transformer = DepthTransformer::new(&calibration); - Ok(CameraInfo { - device, + let camera_info = CameraInfo { config, calibration, xy_table, transformer - }) + }; + + Ok((device, camera_info)) }