Get streaming to work

.gitignore

@@ -1,3 +1,6 @@
+# Rust 
+target/
+
 # Godot 4+ specific ignores
 .godot/
 

new-game-project/node_3d.gd

@@ -1,19 +1,23 @@
 extends Node3D
 
 const num_points: int = 50
-const scale_pos: float = 0.01
-const scale_color: float = 255. / num_points
+const scale_pos: float = -0.001
+const scale_color: float = 1 / num_points
 
 @onready var right_hand: XRController3D = get_node("XROrigin3D/RightHand")
 
 @onready var world_cloud: Pointcloud = get_node("World_Pointcloud")
 @onready var hand_cloud: Pointcloud = get_node("XROrigin3D/RightHand/Hand_Pointcloud")
 
+var stream: Stream
+
 var points: Array[Vector3] = []
-var colors: Array[Vector3] = []
+var colors: Array[Color] = []
 
 var xr_interface: XRInterface
 
+var thread: Thread
+
 func _ready():
 	xr_interface = XRServer.find_interface("OpenXR")
 	if xr_interface and xr_interface.is_initialized():
@@ -26,30 +30,40 @@ func _ready():
 		get_viewport().use_xr = true
 		xr_interface.environment_blend_mode = XRInterface.XR_ENV_BLEND_MODE_ALPHA_BLEND
 		get_viewport().transparent_bg = true
+		hand_cloud.global_scale(Vector3(scale_pos, scale_pos, scale_pos))
+		#world_cloud.global_scale(Vector3(scale_pos, scale_pos, scale_pos))
 	else:
 		print("OpenXR not initialized, please check if your headset is connected")
 		
 		
-	
 	for x in 50:
 		for y in 50:
-			points.append(Vector3(x * scale_pos, y * scale_pos, 0))
-			colors.append(Vector3(x * scale_color, y * scale_color, 0))
+			points.append(Vector3(x * 0.01, y * 0.01, 0))
+			colors.append(Color(x * 0.01, y * 0.01, 0))
 	
 	hand_cloud.set_points(points, colors)
 	
+	stream = Stream.new()
+	stream.start("10.42.0.1", 1234)
+
+
 var last_pressed = false
 
 func _process(delta):
+	if(stream.new_points()):
+		hand_cloud.set_points(stream.current_points, stream.current_colors)
+	
 	var pressed = right_hand.get_float("trigger") > 0.
 	if(pressed && !last_pressed):
 		var points_clone: Array[Vector3] = []
 		print(right_hand.global_position)
-		for point in points:
 		
-			points_clone.append(right_hand.to_global(point))
+		for point in stream.current_points:
+			points_clone.append(right_hand.to_global(point * scale_pos))
 		
-		world_cloud.add_points(points_clone, colors.duplicate())
+		world_cloud.add_points(points_clone, stream.current_colors)
 		last_pressed = true
 		print("points! ", len(points_clone))
 	elif (!pressed): last_pressed = false
+	
+	

new-game-project/node_3d.tscn

@@ -26,7 +26,6 @@ pose = &"aim"
 script = ExtResource("2_a0tk4")
 
 [node name="CSGBox3D2" type="CSGBox3D" parent="XROrigin3D/RightHand"]
-visible = false
 size = Vector3(0.05, 0.05, 0.05)
 
 [node name="Hand_Pointcloud" type="MultiMeshInstance3D" parent="XROrigin3D/RightHand"]

new-game-project/pointcloud.gd

@@ -4,7 +4,7 @@ class_name Pointcloud
 extends MultiMeshInstance3D
 
 var points: Array[Vector3] = []
-var colors: Array[Vector3] = []
+var colors: Array[Color] = []
 
 func _ready():
 	multimesh = MultiMesh.new()
@@ -21,14 +21,14 @@ func _ready():
 	
 	multimesh.mesh=pmesh
 
-func add_points(new_points:Array[Vector3],new_colors:Array[Vector3]):
+func add_points(new_points:Array[Vector3],new_colors:Array[Color]):
 	points.append_array(new_points)
 	colors.append_array(new_colors)
 	set_points(points, colors)
 
-func set_points(points:Array[Vector3],colors:Array[Vector3]):
+func set_points(points:Array[Vector3],colors:Array[Color]):
 	multimesh.instance_count = len(points)
 	
 	for i in multimesh.instance_count:
 		multimesh.set_instance_transform(i, Transform3D(Basis(), points[i]))
-		multimesh.set_instance_color(i,Color(colors[i].x/256.0,colors[i].y/256.0,colors[i].z/256.0))
+		multimesh.set_instance_color(i,colors[i])

new-game-project/stream.gd

@@ -0,0 +1,105 @@
+class_name Stream
+extends Node
+
+var thread: Thread
+var _status: int = 0
+var _stream: StreamPeerTCP = StreamPeerTCP.new()
+
+func start(host: String, port: int):
+	thread = Thread.new()
+	thread.start(tcp_server.bind(host, port))
+
+func tcp_server(host: String, port: int):
+	while true:
+		connect_to_host(host, port)
+		
+		while process():
+			pass	
+		
+		OS.delay_msec(1000)
+
+func connect_to_host(host: String, port: int) -> bool:
+	print("Connecting to %s:%d" % [host, port])
+	# Reset status so we can tell if it changes to error again.
+	_status = _stream.STATUS_NONE
+	if _stream.connect_to_host(host, port) != OK:
+		print("Error connecting to host.")
+		return false
+	return true
+		#emit_signal("error")
+
+func process() -> bool:
+	_stream.poll()
+	var new_status: int = _stream.get_status()
+	if new_status != _status:
+		_status = new_status
+		match _status:
+			_stream.STATUS_NONE:
+				print("Disconnected from host.")
+				return false
+				#emit_signal("disconnected")
+			_stream.STATUS_CONNECTING:
+				print("Connecting to host.")
+				return true
+			_stream.STATUS_CONNECTED:
+				print("Connected to host.")
+				#emit_signal("connected")
+			_stream.STATUS_ERROR:
+				print("Error with socket stream.")
+				return false
+				#emit_signal("error")
+
+	if _status == _stream.STATUS_CONNECTED:
+		parse_bytes()
+		return true
+		
+	return false
+
+
+var current_packet_length: int = -1
+var current_index: int = 0
+
+var current_points: Array[Vector3] = []
+var current_colors: Array[Color] = []
+var has_new_points = false
+
+var overflow_bytes: Array = []
+
+
+func parse_bytes():
+	_stream.put_u8(2);
+	
+	var type = _stream.get_32()
+	
+	match type:
+		1:
+			var len = _stream.get_32()
+			
+			print("Got type %d with len %d" % [type, len])
+			
+			var points: Array[Vector3] = []
+			var colors: Array[Color] = []
+			#
+			for i in len:
+				points.append(Vector3(
+					_stream.get_32(),
+					_stream.get_32(),
+					_stream.get_32()
+				))
+				#
+				colors.append(Color(
+					_stream.get_u8() / 256.,
+					_stream.get_u8() / 256.,
+					_stream.get_u8() / 256.
+				))
+			
+			current_points = points
+			current_colors = colors
+			has_new_points = true
+
+
+func new_points() -> bool:
+	if(has_new_points):
+		has_new_points = false
+		return true
+	return false

new-game-project/stream.gd.uid

@@ -0,0 +1 @@
+uid://bxmxpyrxv8wfr

raspi-proxy/.cargo/config.toml

@@ -0,0 +1,3 @@
+[target.aarch64-unknown-linux-gnu]
+linker = "/usr/bin/aarch64-linux-gnu-g++"
+

raspi-proxy/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "raspi-proxy"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]
+byteorder = "1.5.0"
+image = "0.25.6"
+log = "0.4.27"
+ndarray = "0.16.1"
+pretty_env_logger = "0.5.0"
+ureq = "3.1.0"
+warn = "0.2.2"

raspi-proxy/run.sh

@@ -0,0 +1,20 @@
+set -o errexit
+set -o nounset
+set -o pipefail
+set -o xtrace
+
+# SSH Config
+REMOTE_LOCATION="10.42.0.1"
+REMOTE_USERNAME="user"
+REMOTE_PASSWORD="pass"
+
+# Other constants
+BINARY_NAME="raspi-proxy"
+TARGET_HOST=${REMOTE_USERNAME}@${REMOTE_LOCATION}
+TARGET_ARCH=aarch64-unknown-linux-gnu
+SOURCE_PATH="./target/${TARGET_ARCH}/release/${BINARY_NAME}"
+TARGET_PATH="/home/${REMOTE_USERNAME}/${BINARY_NAME}"
+
+cargo build --release --target=$TARGET_ARCH $@ # Build
+sshpass -p ${REMOTE_PASSWORD} scp ${SOURCE_PATH} ${TARGET_HOST}:${TARGET_PATH} # Upload
+sshpass -p ${REMOTE_PASSWORD} ssh -t ${TARGET_HOST} "RUST_BACKTRACE=full RUST_LOG=info ${TARGET_PATH}" # Run

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());
+    }
+}