Get streaming to work

raspi-proxy/src/camera/camera.rs

@@ -0,0 +1,251 @@
+use std::{
+    sync::{
+        Arc, Mutex
+    },
+    thread,
+};
+
+use std::io::Cursor;
+
+use byteorder::{LittleEndian, ReadBytesExt};
+use ndarray::Array2;
+
+use crate::camera::{fetch_frame::{decode_frame, frame_config_encode, normalize, ProcessedFrames}, intrinsics::{depth_to_point_cloud, DEFAULT_INTRINSICS}, PointArr};
+
+
+// Constants (replace with your actual values)
+const HOST: &str = "192.168.233.1";
+const PORT: u16 = 80;
+
+pub struct PrevFrames {
+    pub depth: Array2<u16>,
+    pub status: Array2<u16>,
+}
+
+impl Default for PrevFrames {
+    fn default() -> Self {
+        Self {
+            depth: Array2::from_elem((240, 320), 0),
+            status: Array2::from_elem((240, 320), 0),
+        }
+    }
+}
+
+pub struct SipeedCamera {
+    frames: Arc<Mutex<ProcessedFrames>>,
+    prev_frames: PrevFrames,
+    thread_handle: Option<thread::JoinHandle<()>>,
+    // point_cloud: LivePointView,
+}
+
+impl Default for SipeedCamera {
+    fn default() -> Self {
+        // Shared state for the latest processed frames
+        let frames = Arc::new(Mutex::new(ProcessedFrames::default()));
+
+        let frames_clone = Arc::clone(&frames);
+        let decoder_handle = thread::spawn(move || {
+            loop {
+                match fetch_frame() {
+                    Ok(frame_data) => match decode_frame(&frame_data) {
+                        Ok(processed) => {
+                            *frames_clone.lock().unwrap() = processed;
+                        }
+                        Err(e) => warn!("Error decoding frame: {}", e),
+                    },
+                    Err(e) => warn!("Error fetching frame: {}", e),
+                }
+            }
+        });
+
+        Self {
+            frames,
+            thread_handle: Some(decoder_handle),
+            prev_frames: PrevFrames::default(),
+            // point_cloud: LivePointView::default(),
+        }
+    }
+}
+
+impl SipeedCamera {
+    pub fn get_points(&mut self) -> Option<PointArr> {
+        let frames_lock = self.frames.lock().unwrap();
+        let ref frames = *frames_lock;
+
+        // let prev_status_lock = self.prev_status.lock();
+        // let ref prev_status = *prev_status_lock;
+
+        let mut processed = false;
+
+        // Display depth image
+        if let Some(ref depth) = frames.depth {
+            self.prev_frames.depth = (&self.prev_frames.depth + depth) / 2;
+
+            let depth_viz = normalize(&self.prev_frames.depth);
+            processed = true;
+        }
+
+        // // Display IR image
+        // if let Some(ref ir) = frames.ir {
+        //     let ir_viz = normalize(ir);
+        //     ui.heading("IR");
+        //     image_widget(ui, "ir_img", &ir_viz, [320.0, 240.0]);
+        //     processed = true;
+        // }
+
+        // Display status image if available
+        if let Some(ref status) = frames.status {
+            self.prev_frames.status = (&self.prev_frames.status + (2 - status)) / 2;
+            let status_viz = normalize(&self.prev_frames.status);
+            processed = true;
+        }
+
+        // Display RGB image if available
+        if let Some(ref rgb) = frames.rgb {
+            let rgb_viz = rgb.as_slice().unwrap();
+            let size = match rgb.dim().1 {
+                640 => [640.0, 480.0],
+                800 => [800.0, 600.0],
+                _ => [640.0, 480.0], // Default
+            };
+            processed = true;
+        }
+
+        if let Some(ref status) = frames.status {
+            if let Some(ref rgb) = frames.rgb {
+                let mut points: PointArr = Vec::new();
+
+                for i in 0..(320 * 240) {
+                    let x = i / 240;
+                    let y = i % 320;
+
+                    // println!("{:?}", status.get((x, y)));
+
+                    let status = status.get((x, y));
+
+                    if status.is_none() || *status.unwrap() != 0 {
+                        continue;
+                    }
+
+                    // let prev_status = prev_status.get((x, y));
+
+                    // if prev_status.is_none() || *prev_status.unwrap() != 0 {
+                    //     continue;
+                    // }
+
+                    // if *status.get((x, y)).unwrap() != 0u16 {
+                    //     continue;
+                    // }
+                    //
+
+                    let (r, g, b) = if let Some((rgbx, rgby)) = scale_shift_rgb_xy(x, y) {
+                        (
+                            *rgb.get((rgbx, rgby, 0)).unwrap() as u8,
+                            *rgb.get((rgbx, rgby, 1)).unwrap() as u8,
+                            *rgb.get((rgbx, rgby, 2)).unwrap() as u8,
+                        )
+                    } else {
+                        (255, 255, 255)
+                    };
+
+                    let d = self.prev_frames.depth.get((x, y)).unwrap();
+
+                    let (x, y, z) = depth_to_point_cloud(
+                        x as i32,
+                        y as i32,
+                        d + 50,
+                        &DEFAULT_INTRINSICS,
+                    );
+
+                    points.push((x, y, z, r, g, b))
+                }
+
+                return Some(points)
+            }
+        }
+
+        None
+
+        // self.point_cloud.render(ui);
+    }
+}
+
+fn scale_shift_rgb_xy(x: usize, y: usize) -> Option<(usize, usize)> {
+    static X_OFFSET: f32 = 30.;
+    static Y_OFFSET: f32 = 22.;
+    static SCAME: f32 = 1.75;
+
+    let x = ((x as f32 + X_OFFSET) * SCAME) as usize;
+    let y = ((y as f32 + Y_OFFSET) * SCAME) as usize;
+
+    if y >= 640 {
+        return None;
+    }
+    if x >= 480 {
+        return None;
+    }
+
+    // println!("{}, {}", x, y);
+
+    Some((x as usize, y as usize))
+}
+
+fn fetch_frame() -> Result<Vec<u8>, Box<dyn std::error::Error>> {
+    is_success(&frame_config_encode(1, 0, 255, 0, 2, 7, 1, 0, 0))?;
+
+
+    let url = format!("http://{}:{}/getdeep", HOST, PORT);
+
+    trace!("Fetching images from: {}", url);
+
+    let response = ureq::get(url).call()?;
+
+    if response.status() != 200 {
+        return Err(format!("Failed to get frame: HTTP {}", response.status()).into());
+    }
+
+    trace!("Got deep image");
+    let deep_img = response.into_body().read_to_vec()?;
+    trace!("Length={}", deep_img.len());
+
+    // Parse frame ID and timestamp
+    if deep_img.len() >= 16 {
+        let mut cursor = Cursor::new(&deep_img[0..16]);
+        let frame_id = cursor.read_u64::<LittleEndian>()?;
+        let stamp_msec = cursor.read_u64::<LittleEndian>()?;
+        trace!(
+            "Frame ID: {}, Timestamp: {:.3}s",
+            frame_id,
+            stamp_msec as f64 / 1000.0
+        );
+    }
+
+    return Ok(deep_img);
+}
+
+fn is_success(data: &[u8]) -> Result<(), Box<dyn std::error::Error>> {
+    let url = format!("http://{}:{}/set_cfg", HOST, PORT);
+
+    trace!("Sending request to: {}", url);
+
+    let response = ureq::post(url).send(data.to_vec())?;
+    if response.status() == 200 {
+        return Ok(());
+    } else {
+        return Err(format!("Status code: {}", response.status().to_string()).into());
+    }
+}
+
+// Helper to display images in egui
+// fn image_widget(ui: &mut egui::Ui, id: &str, rgb_data: &[u8], size: [f32; 2]) {
+//     let color_image = egui::ColorImage::from_rgb([size[0] as usize, size[1] as usize], rgb_data);
+
+//     let handle = ui
+//         .ctx()
+//         .load_texture(id, color_image, egui::TextureOptions::LINEAR);
+
+//     let sized_image =
+//         egui::load::SizedTexture::new(handle.id(), egui::vec2(size[0] as f32, size[1] as f32));
+
+//     ui.image(sized_image);
+// }

raspi-proxy/src/camera/fetch_frame.rs

@@ -0,0 +1,335 @@
+use byteorder::{LittleEndian, ReadBytesExt};
+use ndarray::{Array2, Array3};
+use std::io::Cursor;
+
+pub struct ProcessedFrames {
+    pub depth: Option<Array2<u16>>,
+    pub ir: Option<Array2<u16>>,
+    pub status: Option<Array2<u16>>,
+    pub rgb: Option<Array3<u8>>,
+}
+
+impl Default for ProcessedFrames {
+    fn default() -> Self {
+        Self {
+            depth: None,
+            ir: None,
+            status: None,
+            rgb: None,
+        }
+    }
+}
+
+// Messages between threads
+pub enum FrameMessage {
+    RawFrame(Vec<u8>),
+    DecodedFrame(ProcessedFrames),
+    Shutdown,
+}
+
+// Frame data structures
+#[allow(dead_code)]
+pub struct FrameConfig {
+    trigger_mode: u8,
+    deep_mode: u8,
+    deep_shift: u8,
+    ir_mode: u8,
+    status_mode: u8,
+    status_mask: u8,
+    rgb_mode: u8,
+    rgb_res: u8,
+    expose_time: i32,
+}
+
+pub struct FramePayload {
+    depth_img: Option<Vec<u8>>,
+    ir_img: Option<Vec<u8>>,
+    status_img: Option<Vec<u8>>,
+    rgb_img: Option<Vec<u8>>,
+}
+
+// Helper functions
+fn frame_config_decode(frame_config: &[u8]) -> Result<FrameConfig, Box<dyn std::error::Error>> {
+    if frame_config.len() < 12 {
+        return Err("Frame config data too short".into());
+    }
+
+    let mut cursor = Cursor::new(frame_config);
+
+    Ok(FrameConfig {
+        trigger_mode: cursor.read_u8()?,
+        deep_mode: cursor.read_u8()?,
+        deep_shift: cursor.read_u8()?,
+        ir_mode: cursor.read_u8()?,
+        status_mode: cursor.read_u8()?,
+        status_mask: cursor.read_u8()?,
+        rgb_mode: cursor.read_u8()?,
+        rgb_res: cursor.read_u8()?,
+        expose_time: cursor.read_i32::<LittleEndian>()?,
+    })
+}
+
+pub fn frame_config_encode(
+    trigger_mode: u8,
+    deep_mode: u8,
+    deep_shift: u8,
+    ir_mode: u8,
+    status_mode: u8,
+    status_mask: u8,
+    rgb_mode: u8,
+    rgb_res: u8,
+    expose_time: i32,
+) -> Vec<u8> {
+    let mut result = Vec::with_capacity(12);
+    result.push(trigger_mode);
+    result.push(deep_mode);
+    result.push(deep_shift);
+    result.push(ir_mode);
+    result.push(status_mode);
+    result.push(status_mask);
+    result.push(rgb_mode);
+    result.push(rgb_res);
+
+    // Add expose_time as little endian
+    result.extend_from_slice(&expose_time.to_le_bytes());
+
+    result
+}
+
+fn frame_payload_decode(
+    frame_data: &[u8],
+    config: &FrameConfig,
+) -> Result<FramePayload, Box<dyn std::error::Error>> {
+    if frame_data.len() < 8 {
+        return Err("Frame data too short".into());
+    }
+
+    let mut cursor = Cursor::new(&frame_data[0..8]);
+    let deep_data_size = cursor.read_i32::<LittleEndian>()?;
+    let rgb_data_size = cursor.read_i32::<LittleEndian>()?;
+
+    let mut payload = &frame_data[8..];
+
+    // Depth image
+    let depth_size = (320 * 240 * 2) >> config.deep_mode;
+    let depth_img = if depth_size > 0 && payload.len() >= depth_size {
+        let result = payload[..depth_size].to_vec();
+        payload = &payload[depth_size..];
+        Some(result)
+    } else {
+        None
+    };
+
+    // IR image
+    let ir_size = (320 * 240 * 2) >> config.ir_mode;
+    let ir_img = if ir_size > 0 && payload.len() >= ir_size {
+        let result = payload[..ir_size].to_vec();
+        payload = &payload[ir_size..];
+        Some(result)
+    } else {
+        None
+    };
+
+    // Status image
+    let status_size = (320 * 240 / 8)
+        * match config.status_mode {
+            0 => 16,
+            1 => 2,
+            2 => 8,
+            _ => 1,
+        };
+
+    let status_img = if status_size > 0 && payload.len() >= status_size {
+        let result = payload[..status_size].to_vec();
+        payload = &payload[status_size..];
+        Some(result)
+    } else {
+        None
+    };
+
+    // Verify deep data size
+    let calculated_deep_size = depth_size + ir_size + status_size;
+    if calculated_deep_size != deep_data_size as usize {
+        warn!(
+            "Warning: Deep data size mismatch: {} vs {}",
+            calculated_deep_size, deep_data_size
+        );
+    }
+
+    // RGB image
+    let rgb_size = payload.len();
+    if rgb_size != rgb_data_size as usize {
+        warn!(
+            "Warning: RGB data size mismatch: {} vs {}",
+            rgb_size, rgb_data_size
+        );
+    }
+
+    let rgb_img = if rgb_size > 0 {
+        // Process RGB image based on config
+        if config.rgb_mode == 1 {
+            // JPEG decode using OpenCV
+            let rgb_data = payload.to_vec();
+            let decoded = decode_jpeg(&rgb_data);
+            decoded
+        } else {
+            Some(payload.to_vec())
+        }
+    } else {
+        None
+    };
+
+    Ok(FramePayload {
+        depth_img,
+        ir_img,
+        status_img,
+        rgb_img,
+    })
+}
+
+fn decode_jpeg(jpeg_data: &[u8]) -> Option<Vec<u8>> {
+    // Use the image crate to decode JPEG and convert to RGB
+    let img = image::load_from_memory(jpeg_data).ok()?;
+    let rgb_img = img.to_rgb8();
+
+    // Convert to raw bytes
+    Some(rgb_img.into_raw())
+}
+
+pub fn decode_frame(frame_data: &[u8]) -> Result<ProcessedFrames, Box<dyn std::error::Error>> {
+    if frame_data.len() < 28 {
+        // 16 (header) + 12 (config)
+        return Err("Frame data too short".into());
+    }
+
+    // Extract config
+    let config = frame_config_decode(&frame_data[16..28])?;
+
+    // Decode payload
+    let payload = frame_payload_decode(&frame_data[28..], &config)?;
+
+    // Process depth image
+    let depth = if let Some(depth_data) = payload.depth_img {
+        if config.deep_mode == 0 {
+            let data = depth_data.as_slice();
+            let depth_array = Array2::from_shape_fn((240, 320), |(y, x)| {
+                let idx = (y * 320 + x) * 2;
+                if idx + 1 < data.len() {
+                    u16::from_le_bytes([data[idx], data[idx + 1]])
+                } else {
+                    0
+                }
+            });
+            Some(depth_array)
+        } else {
+            let data = depth_data.as_slice();
+            let depth_array = Array2::from_shape_fn((240, 320), |(y, x)| {
+                let idx = y * 320 + x;
+                if idx < data.len() {
+                    u16::from(data[idx])
+                } else {
+                    0
+                }
+            });
+            Some(depth_array)
+        }
+    } else {
+        None
+    };
+
+    // Process IR image
+    let ir = if let Some(ir_data) = payload.ir_img {
+        if config.ir_mode == 0 {
+            let data = ir_data.as_slice();
+            let ir_array = Array2::from_shape_fn((240, 320), |(y, x)| {
+                let idx = (y * 320 + x) * 2;
+                if idx + 1 < data.len() {
+                    u16::from_le_bytes([data[idx], data[idx + 1]])
+                } else {
+                    0
+                }
+            });
+            Some(ir_array)
+        } else {
+            let data = ir_data.as_slice();
+            let ir_array = Array2::from_shape_fn((240, 320), |(y, x)| {
+                let idx = y * 320 + x;
+                if idx < data.len() {
+                    u16::from(data[idx])
+                } else {
+                    0
+                }
+            });
+            Some(ir_array)
+        }
+    } else {
+        None
+    };
+
+    // Process status image
+    let status = if let Some(status_data) = payload.status_img {
+        // Process according to status_mode
+        let data = status_data.as_slice();
+        let status_array = Array2::from_shape_fn((240, 320), |(y, x)| {
+            // This is a simplified approach - actual processing depends on status_mode
+            let idx = y * 320 + x;
+            if idx < data.len() {
+                u16::from(data[idx])
+            } else {
+                0
+            }
+        });
+        Some(status_array)
+    } else {
+        None
+    };
+
+    // Process RGB image
+    let rgb = if let Some(rgb_data) = payload.rgb_img {
+        let shape = if config.rgb_mode == 1 {
+            match config.rgb_res {
+                0 => (480, 640, 3), // Default resolution
+                _ => (600, 800, 3), // Alternative resolution
+            }
+        } else {
+            (480, 640, 3) // Default for non-JPEG
+        };
+
+        if rgb_data.len() >= shape.0 * shape.1 * shape.2 {
+            let rgb_array = Array3::from_shape_vec((shape.0, shape.1, shape.2), rgb_data)?;
+            Some(rgb_array)
+        } else {
+            trace!(
+                "RGB data size ({}) doesn't match expected size ({})",
+                rgb_data.len(),
+                shape.0 * shape.1 * shape.2
+            );
+            None
+        }
+    } else {
+        None
+    };
+
+    Ok(ProcessedFrames {
+        depth,
+        ir,
+        status,
+        rgb,
+    })
+}
+
+pub fn normalize(data: &Array2<u16>) -> Vec<u8> {
+    let mut result = Vec::with_capacity(data.dim().0 * data.dim().1 * 3);
+
+    let max = 255. / (*data.iter().max().unwrap_or(&255u16) as f32);
+
+    for &value in data.iter() {
+        let num = (value as f32 * max) as u8;
+
+        result.push(num);
+        result.push(num);
+        result.push(num);
+    }
+
+    result
+}

raspi-proxy/src/camera/intrinsics.rs

@@ -0,0 +1,128 @@
+use std::f64;
+
+pub static DEFAULT_INTRINSICS: CameraIntrinsics = CameraIntrinsics {
+    fx: 2.318290e+02,
+    fy: 2.327785e+02,
+    u0: 1.669372e+02,
+    v0: 1.235151e+02,
+    k1: 5.857900e-02,
+    k2: 2.431399e-02,
+    k3: -2.737180e-01,
+    p1: 2.299994e-05,
+    p2: 1.658998e-03,
+};
+
+/// Camera intrinsic parameters
+#[derive(Debug, Clone, Copy)]
+pub struct CameraIntrinsics {
+    pub fx: f64, // focal length x
+    pub fy: f64, // focal length y
+    pub u0: f64, // principal point x
+    pub v0: f64, // principal point y
+    pub k1: f64, // radial distortion coefficient 1
+    pub k2: f64, // radial distortion coefficient 2
+    pub k3: f64, // radial distortion coefficient 3
+    pub p1: f64, // tangential distortion coefficient 1
+    pub p2: f64, // tangential distortion coefficient 2
+}
+
+impl CameraIntrinsics {
+    pub fn new(
+        fx: f64,
+        fy: f64,
+        u0: f64,
+        v0: f64,
+        k1: f64,
+        k2: f64,
+        k3: f64,
+        p1: f64,
+        p2: f64,
+    ) -> Self {
+        Self {
+            fx,
+            fy,
+            u0,
+            v0,
+            k1,
+            k2,
+            k3,
+            p1,
+            p2,
+        }
+    }
+}
+
+/// Convert a pixel with depth information to a 3D point
+///
+/// # Arguments
+/// * `x` - x-coordinate in the image (columns)
+/// * `y` - y-coordinate in the image (rows)
+/// * `depth` - depth value at the pixel (typically in millimeters)
+/// * `intrinsics` - camera intrinsic parameters
+///
+/// # Returns
+/// A 3D point in the camera coordinate system
+pub fn depth_to_point_cloud(
+    x: i32,
+    y: i32,
+    depth: u16,
+    intrinsics: &CameraIntrinsics,
+) -> (i32, i32, i32) {
+    // Convert pixel coordinates to normalized image coordinates
+    let x_f = x as f64;
+    let y_f = y as f64;
+
+    // Apply distortion correction if needed
+    let (x_corrected, y_corrected) = correct_distortion(x_f, y_f, intrinsics);
+
+    // Convert normalized image coordinates to camera coordinates
+    let z = depth as f64; // depth in camera z-direction
+
+    // Back-project using the pinhole camera model
+    // (x - u0) / fx = X / Z
+    // (y - v0) / fy = Y / Z
+    let x_3d = (x_corrected - intrinsics.u0) * z / intrinsics.fx;
+    let y_3d = (y_corrected - intrinsics.v0) * z / intrinsics.fy;
+
+    // Return the 3D point, converting to integer values if needed
+    (x_3d.round() as i32, y_3d.round() as i32, z.round() as i32)
+}
+
+/// Corrects for lens distortion based on the Brown-Conrady model
+///
+/// # Arguments
+/// * `x` - uncorrected x coordinate in pixel space
+/// * `y` - uncorrected y coordinate in pixel space
+/// * `intrinsics` - camera intrinsic parameters with distortion coefficients
+///
+/// # Returns
+/// Corrected (x, y) coordinates
+fn correct_distortion(x: f64, y: f64, intrinsics: &CameraIntrinsics) -> (f64, f64) {
+    // Convert to normalized image coordinates (centered at principal point)
+    let x_norm = (x - intrinsics.u0) / intrinsics.fx;
+    let y_norm = (y - intrinsics.v0) / intrinsics.fy;
+
+    // Calculate squared radius for radial distortion
+    let r2 = x_norm * x_norm + y_norm * y_norm;
+    let r4 = r2 * r2;
+    let r6 = r4 * r2;
+
+    // Calculate radial distortion factor
+    let radial_factor = 1.0 + intrinsics.k1 * r2 + intrinsics.k2 * r4 + intrinsics.k3 * r6;
+
+    // Calculate tangential distortion
+    let tangential_x =
+        2.0 * intrinsics.p1 * x_norm * y_norm + intrinsics.p2 * (r2 + 2.0 * x_norm * x_norm);
+    let tangential_y =
+        intrinsics.p1 * (r2 + 2.0 * y_norm * y_norm) + 2.0 * intrinsics.p2 * x_norm * y_norm;
+
+    // Apply distortion correction
+    let x_corrected = x_norm * radial_factor + tangential_x;
+    let y_corrected = y_norm * radial_factor + tangential_y;
+
+    // Convert back to pixel coordinates
+    let x_pixel = x_corrected * intrinsics.fx + intrinsics.u0;
+    let y_pixel = y_corrected * intrinsics.fy + intrinsics.v0;
+
+    (x_pixel, y_pixel)
+}

raspi-proxy/src/camera/mod.rs

@@ -0,0 +1,8 @@
+mod fetch_frame;
+mod intrinsics;
+mod camera;
+
+pub use camera::SipeedCamera;
+
+pub type Point = (i32, i32, i32, u8, u8, u8);
+pub type PointArr = Vec<Point>;

raspi-proxy/src/main.rs

@@ -0,0 +1,99 @@
+#[macro_use]
+extern crate log;
+mod camera;
+
+use std::{io::{Read, Write}, net::{TcpListener, TcpStream}};
+
+use camera::SipeedCamera;
+
+const SOCKET: &'static str = "0.0.0.0:1234";
+
+enum DataBlocks {
+    Error = 0,
+    PointCloudData = 1,
+    ReadyData = 2
+}
+
+pub fn main() {
+    pretty_env_logger::init();
+
+    let mut camera = SipeedCamera::default();
+
+    info!("Connection established with camera");
+
+    // loop {
+    //     let points = camera.get_points();
+    //     info!("{:?}", points);
+    // }
+
+
+    let mut listener = TcpListener::bind(SOCKET).expect("Failed to bind");
+
+    info!("Server listening on {}", SOCKET);
+
+    loop {
+        match run_server(&mut camera, &mut listener) {
+            Err(e) => {
+                error!("{}", e);
+            }
+            _ => {}
+        }
+    }
+
+    // println!("Test!");
+}
+
+fn run_server(camera: &mut SipeedCamera, listener: &mut TcpListener) -> Result<(), std::io::Error> {
+    for stream in listener.incoming() {
+        let mut stream = stream?;
+
+        info!("New connection: {}", stream.peer_addr().unwrap());
+        run_stream(camera, &mut stream)?;
+
+    }
+
+    Ok(())
+}
+
+fn run_stream(camera: &mut SipeedCamera,stream: &mut TcpStream) ->  Result<(), std::io::Error> {
+    loop {
+        let mut recv_buf = [0u8; 1];
+        stream.read_exact(&mut recv_buf)?;
+        assert!(recv_buf[0] == DataBlocks::ReadyData as u8);
+
+
+        let mut bytes = Vec::new();
+
+        let points = camera.get_points();
+
+        if let Some(points) = points {
+
+
+            // bytes.append(&mut ("A").as_bytes().to_vec());
+            // bytes.append(&mut "B".as_bytes().to_vec());
+
+            bytes.append(&mut (DataBlocks::PointCloudData as i32).to_le_bytes().to_vec());
+            bytes.append(&mut (points.len() as i32).to_le_bytes().to_vec());
+            // bytes.append(&mut points.len().to_le_bytes().to_vec());
+
+            for point in points {
+                bytes.append(&mut point.0.to_le_bytes().to_vec());
+                bytes.append(&mut point.1.to_le_bytes().to_vec());
+                bytes.append(&mut point.2.to_le_bytes().to_vec());
+                bytes.append(&mut point.3.to_le_bytes().to_vec());
+                bytes.append(&mut point.4.to_le_bytes().to_vec());
+                bytes.append(&mut point.5.to_le_bytes().to_vec());
+            }
+
+        } else {
+            bytes.append(&mut (DataBlocks::Error as i32).to_le_bytes().to_vec());
+        }
+
+
+
+        stream.write_all(&bytes)?;
+        stream.flush()?;
+
+        info!("Sent {} bytes", bytes.len());
+    }
+}