diff --git a/Cargo.toml b/Cargo.toml index 44ee0cda48..fe0fa08c34 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,10 @@ authors = [ "Brandon Thomas ", name = "cloudcam_visualizer" path = "src/main_opencv.rs" +[[bin]] +name = "cloudcam_points" +path = "src/main_points.rs" + [[bin]] name = "cloudcam" path = "src/main_zeromq.rs" diff --git a/src/main_points.rs b/src/main_points.rs new file mode 100644 index 0000000000..8a89d49abd --- /dev/null +++ b/src/main_points.rs @@ -0,0 +1,39 @@ + +pub mod zeromq; +pub use anyhow::Result as AnyhowResult; +use anyhow::anyhow; +use crate::zeromq::xy_table::create_xy_table; +use k4a_sys_temp as k4a_sys; +use kinect::{Device, DeviceConfiguration}; +use std::thread; +use std::time::Duration; +use crate::zeromq::calculate_point_cloud::calculate_point_cloud; + +fn main() -> AnyhowResult<()> { + let device = Device::open(0)?; + + let mut config = DeviceConfiguration::init_disable_all(); + config.0.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_WFOV_2X2BINNED; + config.0.camera_fps = k4a_sys::k4a_fps_t_K4A_FRAMES_PER_SECOND_30; + + let calibration = device.get_calibration(config.0.depth_mode, config.0.color_resolution)?; + + let xy_table = create_xy_table(&calibration)?; + + device.start_cameras(&config)?; + + loop { + println!("Get capture"); + + let capture = device.get_capture(500)?; + + let depth_image = capture.get_depth_image() + .ok_or(anyhow!("capture not present"))?; + + let point_cloud = calculate_point_cloud(&depth_image, &xy_table)?; + + println!("Number points: {}", point_cloud.point_count); + } + + Ok(()) +} diff --git a/src/main_zeromq.rs b/src/main_zeromq.rs index 8a89d49abd..a90f6d1878 100644 --- a/src/main_zeromq.rs +++ b/src/main_zeromq.rs @@ -1,39 +1,137 @@ +//! Generate random points to send over ZeroMQ to a consumer + +mod zeromq; -pub mod zeromq; -pub use anyhow::Result as AnyhowResult; -use anyhow::anyhow; -use crate::zeromq::xy_table::create_xy_table; -use k4a_sys_temp as k4a_sys; -use kinect::{Device, DeviceConfiguration}; use std::thread; -use std::time::Duration; -use crate::zeromq::calculate_point_cloud::calculate_point_cloud; +use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use zeromq::color::Color; +use zeromq::point::Point; +use zmq::{Error, Socket, Context, DONTWAIT}; -fn main() -> AnyhowResult<()> { - let device = Device::open(0)?; +use anyhow::Result as AnyhowResult; - let mut config = DeviceConfiguration::init_disable_all(); - config.0.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_WFOV_2X2BINNED; - config.0.camera_fps = k4a_sys::k4a_fps_t_K4A_FRAMES_PER_SECOND_30; +const SOCKET_ADDRESS : &'static str = "tcp://127.0.0.1:8888"; - let calibration = device.get_calibration(config.0.depth_mode, config.0.color_resolution)?; +enum MessagingState { + Sending, + Receiving, +} - let xy_table = create_xy_table(&calibration)?; +fn main() { + let context = zmq::Context::new(); - device.start_cameras(&config)?; + //let socket = ctx.socket(zmq::PUSH).unwrap(); + let mut socket = context.socket(zmq::REQ).unwrap(); - loop { - println!("Get capture"); + //socket.connect("tcp://127.0.0.1:5555").unwrap(); + //socket.bind("tcp://127.0.0.1:5555").unwrap(); + socket.connect(SOCKET_ADDRESS).unwrap(); - let capture = device.get_capture(500)?; + let mut reconnect = false; + let mut fail_count = 0; + let mut messaging_state = MessagingState::Sending; + let mut color = Color::Blue; - let depth_image = capture.get_depth_image() - .ok_or(anyhow!("capture not present"))?; + loop { + let color = get_color(); + let point = Point::at_random_range(-1000.0f32, 1000.0f32, color); + let bytes = point.to_bytes(); + //println!("Point : {}", point.debug_string()); - let point_cloud = calculate_point_cloud(&depth_image, &xy_table)?; - - println!("Number points: {}", point_cloud.point_count); + if reconnect { + socket = reconnect_socket(&context, socket, SOCKET_ADDRESS); + reconnect = false; + messaging_state = MessagingState::Sending; } - Ok(()) + match messaging_state { + MessagingState::Sending => { + //println!("Sending request..."); + //let result = socket.send("hello world!", 0); + let result = socket.send(&bytes, 0); + + match result { + Ok(_) => { + //println!("Sent!"); + //thread::sleep(Duration::from_millis(250)); + messaging_state = MessagingState::Receiving; + }, + Err(e) => { + //eprintln!("Send Error ({}): {:?}", e.to_raw(), e); + //thread::sleep(Duration::from_millis(250)); + fail_count += 1; + }, + } + + }, + MessagingState::Receiving => { + //println!("Awaiting response..."); + let result = socket.recv_bytes(DONTWAIT); + + match result { + Ok(_) => { + //println!("Response received!"); + messaging_state = MessagingState::Sending; + }, + Err(e) => { + //eprintln!("Recv Error ({}): {:?}", e.to_raw(), e); + //thread::sleep(Duration::from_millis(250)); + fail_count += 1; + }, + } + + }, + } + + if fail_count > 5 { + reconnect = true; + fail_count = 0; + //thread::sleep(Duration::from_millis(2000)); + } + } } + +fn reconnect_socket(context: &Context, socket: Socket, address: &str) -> Socket { + //println!("[reconnect] Creating new socket..."); + + let mut socket = match context.socket(zmq::REQ) { + Ok(s) => { + //println!("New socket created."); + s + }, + Err(e) => { + //println!("Error creating new socket: {:?}", e); + return socket; + }, + }; + + //println!("Connecting new socket..."); + match socket.connect(address) { + Ok(_) => { + //println!("New socket connected."); + }, + Err(err) => { + //println!("Error connecting new socket: {:?}", err); + }, + } + + return socket; +} + +fn get_color() -> Color { + let start = SystemTime::now(); + let timestamp = start + .duration_since(UNIX_EPOCH) + .expect("Time should work."); + + let seconds = timestamp.as_secs(); + + match seconds % 10 { + 0 | 1 => Color::Black, + 2 | 3 => Color::Blue, + 4 | 5 => Color::Red, + 6 | 7 => Color::Green, + 8 | 9 => Color::White, + _ => Color::White, + } +} \ No newline at end of file