streaming realsense depth map video over network

lib.rs

pub mod network;
pub mod realsense_bindings;

pub type CameraTimestamp = u64;
pub const WIDTH: usize = 640;
pub const HEIGHT: usize = 480;
pub const FPS: usize = 30;
pub const FRAME_SIZE: usize = WIDTH * HEIGHT * 2; //16 bit depth info

pub const RECIEVER_ADDR: &'static str = "192.168.1.2:9898";

//TODO: switch from mio to tokio?
//example https://medium.com/tresorit-engineering/collecting-broadcast-udp-packets-using-async-networking-in-rust-7fd93a631eac

network.rs

#![allow(non_upper_case_globals)]

use crate::*;
use serde::{Deserialize, Serialize};
#[derive(Serialize, Deserialize, PartialEq, Debug)]
pub struct FramePacket<'a> {
    pub camera_timestamp: CameraTimestamp,
    pub packet_number: u16,
    pub data: &'a [u8],
}

//TODO: Is there a reason to keep my UDP packets below the ethernet MTU of 1500? Or should I just not worry about it, and use max UDP MTU of 16kB?
//const MTU: usize = 1500;
pub const MTU: usize = 1472; //1500 - 20 ip header - 8 udp header. https://stackoverflow.com/questions/14993000/the-most-reliable-and-efficient-udp-packet-size

//MTU minus MY packet overhead
pub const EffectiveMTU: usize = MTU - std::mem::size_of::<FramePacket>();
//TODO: why isn't MTU - 8 - 2? It's 8 larger than I'd expect --- maybe due to struct alignment?

//FrameSize / EffectiveMTU rounded up in integer division
pub const PacketsPerFrame: usize = (FRAME_SIZE + EffectiveMTU - 1) / EffectiveMTU;

//how many udp packets could a frame be?
// (/ (* 1920 1280 4) 1500) => 6554 packets, so u16 should be more than enough for a packet counter
// since UDP packets can arrive out of order or be dropped entirely, lets try this algo:
// use camera_timestamp as frame ID
// if packet with later timestamp arrives, clear buffer and start collecting these packets.

pub fn frame_packets(timestamp: CameraTimestamp, data: &[u8]) -> Vec<FramePacket> {
    let packets: Vec<FramePacket> = data
        .chunks(EffectiveMTU)
        .enumerate()
        .map(|(i, c)| FramePacket {
            camera_timestamp: timestamp,
            packet_number: i as u16,
            data: c,
        })
        .collect();

    assert!(packets.len() == PacketsPerFrame);

    return packets;
}

reciever.rs

use mio::net::UdpSocket;
use mio::*;
use realsense::network::*;
use realsense::*;
use std::error::Error;

pub fn write_frame(frame: &[u8]) {
    lodepng::encode_file(
        "test.png",
        frame,
        WIDTH,
        HEIGHT,
        lodepng::ffi::ColorType::GREY,
        16,
    )
    .unwrap();
}

pub fn recieve_packets(socket: UdpSocket) {
    let poll = Poll::new().unwrap();
    poll.register(&socket, Token(0), Ready::readable(), PollOpt::edge())
        .unwrap();

    let mut frame_buffer = vec![0; FRAME_SIZE];
    let mut packet_buffer = [0; MTU];
    let mut current_frame_timestamp: CameraTimestamp = 0;
    let mut packets_remaining = PacketsPerFrame;

    let mut events = Events::with_capacity(128);
    loop {
        poll.poll(&mut events, None).unwrap();
        for _ in events.iter() {
            //TODO: should I be able to recieve an exact number of bytes here? I.e., the full packet?
            let num_recv = socket.recv(&mut packet_buffer).unwrap();
            println!("read {:?} bytes", num_recv);

            let p: FramePacket = bincode::deserialize(&packet_buffer).unwrap();

            if p.camera_timestamp > current_frame_timestamp {
                //new frame, reset counters
                print!("Recieving new frame");
                current_frame_timestamp = p.camera_timestamp;
                packets_remaining = PacketsPerFrame;
            }

            if p.camera_timestamp == current_frame_timestamp {
                packets_remaining = packets_remaining - 1;
                let i = p.packet_number as usize;
                let offset = i * EffectiveMTU;
                frame_buffer[offset..(offset + p.data.len())].copy_from_slice(p.data);

                if 0 == packets_remaining {
                    println!("Full frame recieved!");
                    write_frame(&frame_buffer);
                    return;
                }
            }

            if p.camera_timestamp < current_frame_timestamp {
                println!("Delayed packed arrived!");
            }
        }
    }
}

pub fn main() -> Result<(), Box<dyn Error>> {
    let socket = UdpSocket::bind(&RECIEVER_ADDR.parse()?)?;

    recieve_packets(socket);

    return Ok(());
}

sender.rs

use mio::net::UdpSocket;
use mio::*;

use realsense::network;
use realsense::realsense_bindings::*;
use realsense::*;
use std::process::exit;

fn check_error(e: *mut *mut rs2_error) {
    use std::ffi::CStr;
    if !e.is_null() {
        unsafe {
            println!("Error: {:?}", CStr::from_ptr(rs2_get_error_message(*e)));
        }
        exit(1);
    }
}

use std::error::Error;
fn main() -> Result<(), Box<dyn Error>> {
    let sender_socket = UdpSocket::bind(&"0.0.0.0:0".parse()?)?;

    sender_socket.connect(RECIEVER_ADDR.parse()?)?;

    let poll = Poll::new().unwrap();
    poll.register(&sender_socket, Token(0), Ready::writable(), PollOpt::edge())
        .unwrap();

    let mut events = Events::with_capacity(128);

    unsafe {
        let e = std::ptr::null_mut();
        let ctx = rs2_create_context(RS2_API_VERSION as i32, e);

        let devices: *mut rs2_device_list = rs2_query_devices(ctx, e);
        if 1 != rs2_get_device_count(devices, e) {
            exit(1);
        }

        let d = rs2_create_device(devices, 0, e);
        check_error(e);
        let config = rs2_create_config(e);
        check_error(e);

        rs2_config_enable_stream(
            config,
            rs2_stream_RS2_STREAM_DEPTH,
            0,
            WIDTH as i32,
            HEIGHT as i32,
            rs2_format_RS2_FORMAT_Z16,
            FPS as i32,
            e,
        );
        check_error(e);

        let pipeline = rs2_create_pipeline(ctx, e);
        check_error(e);

        let pipeline_profile = rs2_pipeline_start_with_config(pipeline, config, e);
        check_error(e);

        loop {
            let frames = rs2_pipeline_wait_for_frames(pipeline, RS2_DEFAULT_TIMEOUT, e);
            check_error(e);
            let n = rs2_embedded_frames_count(frames, e);
            check_error(e);

            if 0 == n {
                continue;
            }

            for i in 0..n {
                let frame = rs2_extract_frame(frames, i, e);
                //skip non-depth frames
                if 0 == rs2_is_frame_extendable_to(
                    frame,
                    rs2_extension_RS2_EXTENSION_DEPTH_FRAME,
                    e,
                ) {
                    rs2_release_frame(frame);
                    continue;
                }

                let f = std::slice::from_raw_parts(
                    rs2_get_frame_data(frame, e) as *const u16,
                    WIDTH * HEIGHT,
                );

                let sensor = rs2_get_frame_sensor(frame, e);
                dbg!(rs2_get_depth_scale(sensor, e));

                let ts = rs2_get_frame_number(frame, e);

                //coerce u16 slice to u8 slice
                let (_, f, _) = f.align_to::<u8>();
                for packet in network::frame_packets(ts, f) {
                    poll.poll(&mut events, None).unwrap();
                    //TODO: this allocates to vec, can I write to UDP socket directly?
                    let msg = bincode::serialize(&packet).unwrap();
                    for _ in events.iter() {
                        let bytes_sent = sender_socket.send(&msg).unwrap();
                        if bytes_sent != msg.len() {
                            println!("socket didn't send packet!");
                        }
                        println!("sent {:?} -> {:?} bytes", msg.len(), bytes_sent);
                    }
                }

                rs2_release_frame(frame);
            }
            rs2_release_frame(frames);
            break;
        }

        //example from
        //https://github.com/IntelRealSense/librealsense/blob/master/examples/C/distance/rs-distance.c

        // Stop the pipeline streaming, release resources
        rs2_pipeline_stop(pipeline, e);
        rs2_delete_pipeline_profile(pipeline_profile);
        rs2_delete_config(config);
        rs2_delete_pipeline(pipeline);
        rs2_delete_device(d);
        rs2_delete_device_list(devices);
        rs2_delete_context(ctx);

        exit(0)
    }
}