cu_livox/
lib.rs

1pub mod parser;
2
3use crate::parser::RefTime;
4use chrono::Utc;
5use cu_sensor_payloads::{PointCloud, PointCloudSoa};
6use cu29::prelude::*;
7use socket2::{Domain, Protocol, SockAddr, Socket, Type};
8use std::io::{ErrorKind, Read};
9use std::net::SocketAddr;
10const DEFAULT_ADDR: &str = "0.0.0.0:56001";
11
12pub struct Tele15 {
13    socket: Socket,
14    reftime: RefTime,
15}
16
17impl Tele15 {
18    /// This give the matching timestamps between the UTC time to the Robot time.
19    fn sync(&mut self, robot_clock: &RobotClock) {
20        self.reftime = (Utc::now(), robot_clock.now());
21    }
22}
23
24impl Freezable for Tele15 {}
25
26const MAX_POINTS: usize = 100;
27
28pub type LidarCuMsgPayload = PointCloudSoa<MAX_POINTS>;
29
30impl CuSrcTask for Tele15 {
31    type Resources<'r> = ();
32    type Output<'m> = output_msg!(LidarCuMsgPayload);
33
34    fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
35    where
36        Self: Sized,
37    {
38        let addr: SocketAddr = if let Some(cfg) = config {
39            let addr_str = cfg.get("socket_addr").unwrap_or(DEFAULT_ADDR.to_string());
40            addr_str.as_str().parse().unwrap()
41        } else {
42            DEFAULT_ADDR.parse().unwrap()
43        };
44
45        let socket = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP)).unwrap();
46        socket.bind(&SockAddr::from(addr)).unwrap();
47        socket.set_nonblocking(true).unwrap();
48
49        // just a temporary value, it will be redone at start.
50        let rt: RefTime = (Utc::now(), RobotClock::new().now());
51        Ok(Tele15 {
52            socket,
53            reftime: rt,
54        })
55    }
56    fn start(&mut self, robot_clock: &RobotClock) -> CuResult<()> {
57        self.sync(robot_clock);
58        Ok(())
59    }
60    fn process(&mut self, _clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
61        let payload = new_msg.payload_mut().insert(LidarCuMsgPayload::default());
62        let mut buf = [0u8; 1500];
63        match self.socket.read(&mut buf) {
64            Ok(size) => {
65                let lidar_packet = parser::parse_frame(&buf[..size])
66                    .map_err(|e| CuError::new_with_cause("Failed to parse Livox UDP packet", e))?;
67
68                // let is_dual = lidar_packet.header.is_dual_return(); TODO: add dual return support
69                for pt in lidar_packet.points.iter() {
70                    payload.push(PointCloud::new_uom(
71                        lidar_packet.header.timestamp(),
72                        pt.x(),
73                        pt.y(),
74                        pt.z(),
75                        pt.reflectivity(),
76                        None,
77                    ));
78                }
79            }
80            Err(ref e) if e.kind() == ErrorKind::WouldBlock => {
81                // Handle no data available (non-blocking behavior)
82                new_msg.clear_payload();
83                return Ok(());
84            }
85            Err(e) => return Err(CuError::new_with_cause("IO Error on UDP socket", e)), // Handle other errors
86        }
87        Ok(())
88    }
89}
90
91#[cfg(test)]
92mod tests {
93    use super::*;
94    use crate::parser::LidarFrame;
95    use chrono::DateTime;
96    use cu_udp_inject::PcapStreamer;
97    use cu29::cutask::CuMsg;
98
99    #[test]
100    fn test_tele15() {
101        let clock = RobotClock::new();
102        let mut streamer = PcapStreamer::new("tests/livox_tele15_small.pcap", "127.0.0.1:56001");
103        let config = ComponentConfig::new();
104
105        let mut tele15 = Tele15::new(Some(&config), ()).unwrap();
106
107        let new_payload = LidarCuMsgPayload::default();
108        let mut new_msg = CuMsg::<LidarCuMsgPayload>::new(Some(new_payload));
109
110        // 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.
111        let datetime = DateTime::parse_from_rfc3339("2024-09-17T15:47:11.684855Z")
112            .unwrap()
113            .with_timezone(&Utc);
114
115        tele15.reftime = (datetime, clock.now());
116        const PACKET_SIZE: usize = size_of::<LidarFrame>();
117        while streamer
118            .send_next::<PACKET_SIZE>()
119            .expect("Failed to send next packet")
120        {
121            let err = tele15.process(&clock, &mut new_msg);
122            if let Err(e) = err {
123                println!("Error: {e:?}");
124                continue;
125            }
126            if let Some(payload) = new_msg.payload() {
127                println!("Lidar Payload: {payload:?}");
128            }
129            break;
130        }
131    }
132}
cu_livox/lib.rs

cu_livox/
lib.rs
