Lfra/driver livox (#203)

* Add basic livox driver for Tele15

* Fix clippy

* Fix tmp path

* Appease clippy wrath

Cargo.toml

@@ -22,6 +22,7 @@ members = [
     "components/sinks/cu_lewansoul",
     "components/sources/cu_ads7883",
     "components/sources/cu_hesai",
+    "components/sources/cu_livox",
     "components/sources/cu_iceoryx2_src",
     "components/sources/cu_v4l",
     "components/sources/cu_vlp16",

components/sources/cu_livox/Cargo.toml

@@ -0,0 +1,26 @@
+[package]
+name = "cu-livox"
+description = "Copper driver for Livox Tele15. Note: the actual parsing is usable outside of Copper if you need a Livox Tele15 driver for another project."
+version.workspace = true
+authors.workspace = true
+edition.workspace = true
+license.workspace = true
+keywords.workspace = true
+categories.workspace = true
+homepage.workspace = true
+repository.workspace = true
+
+[dependencies]
+cu29 = { workspace = true }
+cu-sensor-payloads = { workspace = true }
+bytemuck = { version = "1.19.0", features = ["derive"] }
+uom = { workspace = true }
+chrono = { version = "0.4.38", features = ["serde"] }
+socket2 = { version = "0.5.7", features = ["all"] }
+tempfile = {workspace = true}
+
+[dev-dependencies]
+cu-udp-inject = { path = "../../testing/cu_udp_inject" }
+cu29-derive = { workspace = true }
+cu29-helpers = { workspace = true }
+

components/sources/cu_livox/build.rs

@@ -0,0 +1,6 @@
+fn main() {
+    println!(
+        "cargo:rustc-env=LOG_INDEX_DIR={}",
+        std::env::var("OUT_DIR").unwrap()
+    );
+}

components/sources/cu_livox/src/lib.rs

@@ -0,0 +1,131 @@
+pub mod parser;
+
+use crate::parser::RefTime;
+use chrono::Utc;
+use cu29::prelude::*;
+use cu_sensor_payloads::{PointCloud, PointCloudSoa};
+use socket2::{Domain, Protocol, SockAddr, Socket, Type};
+use std::io::{ErrorKind, Read};
+use std::net::SocketAddr;
+const DEFAULT_ADDR: &str = "0.0.0.0:56001";
+
+pub struct Tele15 {
+    socket: Socket,
+    reftime: RefTime,
+}
+
+impl Tele15 {
+    /// This give the matching timestamps between the UTC time to the Robot time.
+    fn sync(&mut self, robot_clock: &RobotClock) {
+        self.reftime = (Utc::now(), robot_clock.now());
+    }
+}
+
+impl Freezable for Tele15 {}
+
+const MAX_POINTS: usize = 100;
+
+pub type LidarCuMsgPayload = PointCloudSoa<MAX_POINTS>;
+
+impl<'cl> CuSrcTask<'cl> for Tele15 {
+    type Output = output_msg!('cl, LidarCuMsgPayload);
+
+    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
+    where
+        Self: Sized,
+    {
+        let addr: SocketAddr = if let Some(cfg) = config {
+            let addr_str = cfg.get("socket_addr").unwrap_or(DEFAULT_ADDR.to_string());
+            addr_str.as_str().parse().unwrap()
+        } else {
+            DEFAULT_ADDR.parse().unwrap()
+        };
+
+        let socket = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP)).unwrap();
+        socket.bind(&SockAddr::from(addr)).unwrap();
+        socket.set_nonblocking(true).unwrap();
+
+        // just a temporary value, it will be redone at start.
+        let rt: RefTime = (Utc::now(), RobotClock::new().now());
+        Ok(Tele15 {
+            socket,
+            reftime: rt,
+        })
+    }
+    fn start(&mut self, robot_clock: &RobotClock) -> CuResult<()> {
+        self.sync(robot_clock);
+        Ok(())
+    }
+    fn process(&mut self, _clock: &RobotClock, new_msg: Self::Output) -> CuResult<()> {
+        let payload = new_msg.payload_mut().insert(LidarCuMsgPayload::default());
+        let mut buf = [0u8; 1500];
+        match self.socket.read(&mut buf) {
+            Ok(size) => {
+                let lidar_packet = parser::parse_frame(&buf[..size])
+                    .map_err(|e| CuError::new_with_cause("Failed to parse Livox UDP packet", e))?;
+
+                // let is_dual = lidar_packet.header.is_dual_return(); TODO: add dual return support
+                for pt in lidar_packet.points.iter() {
+                    payload.push(PointCloud::new_uom(
+                        lidar_packet.header.timestamp(),
+                        pt.x(),
+                        pt.y(),
+                        pt.z(),
+                        pt.reflectivity(),
+                        None,
+                    ));
+                }
+            }
+            Err(ref e) if e.kind() == ErrorKind::WouldBlock => {
+                // Handle no data available (non-blocking behavior)
+                new_msg.clear_payload();
+                return Ok(());
+            }
+            Err(e) => return Err(CuError::new_with_cause("IO Error on UDP socket", e)), // Handle other errors
+        }
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::parser::LidarFrame;
+    use chrono::DateTime;
+    use cu29::cutask::CuMsg;
+    use cu_udp_inject::PcapStreamer;
+
+    #[test]
+    fn test_tele15() {
+        let clock = RobotClock::new();
+        let mut streamer = PcapStreamer::new("tests/livox_tele15_small.pcap", "127.0.0.1:56001");
+        let config = ComponentConfig::new();
+
+        let mut tele15 = Tele15::new(Some(&config)).unwrap();
+
+        let new_payload = LidarCuMsgPayload::default();
+        let mut new_msg = CuMsg::<LidarCuMsgPayload>::new(Some(new_payload));
+
+        // Picking a timestamp from the beginning of the pcap file to align the robot clock with the capture + 1s buffer in the past because ref times are negative.
+        let datetime = DateTime::parse_from_rfc3339("2024-09-17T15:47:11.684855Z")
+            .unwrap()
+            .with_timezone(&Utc);
+
+        tele15.reftime = (datetime, clock.now());
+        const PACKET_SIZE: usize = size_of::<LidarFrame>();
+        while streamer
+            .send_next::<PACKET_SIZE>()
+            .expect("Failed to send next packet")
+        {
+            let err = tele15.process(&clock, &mut new_msg);
+            if let Err(e) = err {
+                println!("Error: {:?}", e);
+                continue;
+            }
+            if let Some(payload) = new_msg.payload() {
+                println!("Lidar Payload: {:?}", payload);
+            }
+            break;
+        }
+    }
+}