From 193fd8d3d6d71e0c7aaecabfb00c3294cd8911fc Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Tue, 2 Feb 2021 03:56:46 -0500 Subject: [PATCH] refactor debug routines, add time-based color --- src/main_zeromq.rs | 66 ++++------------------------- src/zeromq/calculate_point_cloud.rs | 4 +- src/zeromq/color.rs | 21 +++++++++ src/zeromq/debug.rs | 38 +++++++++++++++++ src/zeromq/mod.rs | 1 + 5 files changed, 71 insertions(+), 59 deletions(-) create mode 100644 src/zeromq/debug.rs diff --git a/src/main_zeromq.rs b/src/main_zeromq.rs index 2e0618d65b..788e3a35c5 100644 --- a/src/main_zeromq.rs +++ b/src/main_zeromq.rs @@ -14,10 +14,10 @@ use zeromq::calculate_point_cloud::PointCloudResult; use zeromq::calculate_point_cloud::calculate_point_cloud2; use zeromq::calculate_point_cloud::calculate_point_cloud; use zeromq::color::Color; +use zeromq::color::get_time_based_color; use zeromq::point::Point; use zeromq::xy_table::create_xy_table; use zmq::{Error, Socket, Context, DONTWAIT}; -use cgmath::num_traits::Float; const SOCKET_ADDRESS : &'static str = "tcp://127.0.0.1:8888"; @@ -60,9 +60,10 @@ fn main() -> AnyhowResult<()> { let mut reconnect = false; let mut fail_count = 0; let mut messaging_state = MessagingState::Sending_DataLength; - let mut color = Color::Blue; + let mut color = get_time_based_color(); + + let mut points = get_point_cloud(&device, &xy_table, color)?; - let mut points = get_point_cloud(&device, &xy_table)?; if !points.is_empty() { //print_pointcloud_maxima(&points); } else { @@ -114,7 +115,8 @@ fn main() -> AnyhowResult<()> { }, MessagingState::GrabPointCloud => { println!("Grabbing another frame..."); - points = get_point_cloud(&device, &xy_table)?; + color = get_time_based_color(); + points = get_point_cloud(&device, &xy_table, color)?; if !points.is_empty() { //print_pointcloud_maxima(&points); @@ -130,13 +132,14 @@ fn main() -> AnyhowResult<()> { Ok(()) } -fn get_point_cloud(device: &Device, xy_table: &Image) -> AnyhowResult> { +fn get_point_cloud(device: &Device, xy_table: &Image, color: Color) -> AnyhowResult> { let capture = device.get_capture(500)?; let depth_image = capture.get_depth_image() .ok_or(anyhow!("capture not present"))?; - let mut points = calculate_point_cloud2(&depth_image, &xy_table)?; + let mut points = + calculate_point_cloud2(&depth_image, &xy_table, color)?; Ok(points) } @@ -190,40 +193,6 @@ fn receive_ack(socket: &Socket) -> AnyhowResult<()> { Ok(()) } -fn print_pointcloud_maxima(points: &Vec) { - let mut max_x = f32::min_value(); - let mut min_x = f32::max_value(); - let mut max_y = f32::min_value(); - let mut min_y = f32::max_value(); - let mut max_z = f32::min_value(); - let mut min_z = f32::max_value(); - - for pt in points.iter() { - if pt.x < min_x { - min_x = pt.x; - } - if pt.x > max_x { - max_x = pt.x; - } - if pt.y < min_y { - min_y = pt.y; - } - if pt.y > max_y { - max_y = pt.y; - } - if pt.z < min_z { - min_z = pt.z; - } - if pt.z > max_z { - max_z = pt.z; - } - } - - // minmax x : -2612.5303, 4902.1655 | y: -3491.938, 1000.92487 | z: 163, 5494 - // minmax x : -2615.739, 4918.527 | y: -3507.2483, 1007.1275 | z: 163, 5518 - println!("minmax x : {}, {} | y: {}, {} | z: {}, {}", min_x, max_x, min_y, max_y, min_z, max_z); -} - /* @@ -311,20 +280,3 @@ fn reconnect_socket(context: &Context, socket: Socket, address: &str) -> Socket 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 diff --git a/src/zeromq/calculate_point_cloud.rs b/src/zeromq/calculate_point_cloud.rs index 86df83e42c..ea31272a0c 100644 --- a/src/zeromq/calculate_point_cloud.rs +++ b/src/zeromq/calculate_point_cloud.rs @@ -56,7 +56,7 @@ pub fn calculate_point_cloud(depth_image: &Image, xy_table_image: &Image) -> Any } // Directly return as a vector. -pub fn calculate_point_cloud2(depth_image: &Image, xy_table_image: &Image) -> AnyhowResult> { +pub fn calculate_point_cloud2(depth_image: &Image, xy_table_image: &Image, color: Color) -> AnyhowResult> { let width = depth_image.get_width_pixels(); let height = depth_image.get_height_pixels(); @@ -75,7 +75,7 @@ pub fn calculate_point_cloud2(depth_image: &Image, xy_table_image: &Image) -> An let x = (*xy_table_data.offset(i)).xy.x * ((*depth_data.offset(i)) as f32); let y = (*xy_table_data.offset(i)).xy.y * ((*depth_data.offset(i)) as f32); let z = (*depth_data.offset(i)) as f32; - points.push(Point::at(x, y, z, Color::Yellow)); + points.push(Point::at(x, y, z, color)); } } } diff --git a/src/zeromq/color.rs b/src/zeromq/color.rs index cffb3f46ce..d3d38a6841 100644 --- a/src/zeromq/color.rs +++ b/src/zeromq/color.rs @@ -1,3 +1,5 @@ +use std::time::{UNIX_EPOCH, SystemTime}; + #[derive(Copy, Clone, Debug)] pub enum Color { Black, @@ -68,3 +70,22 @@ impl Color { } } } + +/// Fun function to retrieve a color based on the time. +pub fn get_time_based_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::Purple, //Color::Black, + 2 | 3 => Color::Blue, + 4 | 5 => Color::Red, + 6 | 7 => Color::Green, + 8 | 9 => Color::White, + _ => Color::White, + } +} diff --git a/src/zeromq/debug.rs b/src/zeromq/debug.rs new file mode 100644 index 0000000000..3f1ca6155c --- /dev/null +++ b/src/zeromq/debug.rs @@ -0,0 +1,38 @@ +use cgmath::num_traits::Float; +use crate::zeromq::point::Point; + +/// Print the minima and maxima along each dimension. +pub fn print_pointcloud_maxima(points: &Vec) { + let mut max_x = f32::min_value(); + let mut min_x = f32::max_value(); + let mut max_y = f32::min_value(); + let mut min_y = f32::max_value(); + let mut max_z = f32::min_value(); + let mut min_z = f32::max_value(); + + for pt in points.iter() { + if pt.x < min_x { + min_x = pt.x; + } + if pt.x > max_x { + max_x = pt.x; + } + if pt.y < min_y { + min_y = pt.y; + } + if pt.y > max_y { + max_y = pt.y; + } + if pt.z < min_z { + min_z = pt.z; + } + if pt.z > max_z { + max_z = pt.z; + } + } + + // minmax x : -2612.5303, 4902.1655 | y: -3491.938, 1000.92487 | z: 163, 5494 + // minmax x : -2615.739, 4918.527 | y: -3507.2483, 1007.1275 | z: 163, 5518 + println!("minmax x : {}, {} | y: {}, {} | z: {}, {}", + min_x, max_x, min_y, max_y, min_z, max_z); +} diff --git a/src/zeromq/mod.rs b/src/zeromq/mod.rs index e7ff7575fe..546cdf0c57 100644 --- a/src/zeromq/mod.rs +++ b/src/zeromq/mod.rs @@ -1,4 +1,5 @@ pub mod calculate_point_cloud; pub mod color; +pub mod debug; pub mod point; pub mod xy_table; \ No newline at end of file