cu_vlp16/
lib.rs

1use velodyne_lidar::{Config, Config16};
2use velodyne_lidar::{DataPacket, Packet};
3
4use cu29::prelude::*;
5use cu_sensor_payloads::{PointCloud, PointCloudSoa};
6use std::net::UdpSocket;
7use std::time::Duration;
8use velodyne_lidar::iter::try_packet_to_frame_xyz;
9use velodyne_lidar::types::frame_xyz::FrameXyz;
10
11pub struct Vlp16 {
12    velo_config: Config,
13    listen_addr: String,
14    #[allow(dead_code)]
15    test_mode: bool,
16    socket: Option<UdpSocket>,
17}
18
19impl Freezable for Vlp16 {}
20
21impl CuSrcTask for Vlp16 {
22    type Output<'m> = output_msg!(PointCloudSoa<10000>);
23
24    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
25    where
26        Self: Sized,
27    {
28        let config: &ComponentConfig = config.expect("Vlp16 requires a config");
29        let listen_addr: String = config
30            .get("listen_addr")
31            .unwrap_or("0.0.0.0:2368".to_string());
32        let return_type: String = config.get("return_type").unwrap_or("last".to_string());
33        let test_mode: String = config.get("test_mode").unwrap_or("false".to_string());
34        let velo_config = match return_type.as_str() {
35            "strongest" => Config16::new_vlp_16_strongest(),
36            "last" => Config16::new_vlp_16_last(),
37            "dual" => Config16::new_vlp_16_dual(),
38            _ => {
39                return Err("Invalid return_type for Vlp16.".into());
40            }
41        };
42        Ok(Vlp16 {
43            velo_config: velo_config.into(),
44            listen_addr,
45            test_mode: test_mode == "true",
46            socket: None,
47        })
48    }
49
50    fn start(&mut self, _clock: &RobotClock) -> CuResult<()> {
51        let socket = UdpSocket::bind(&self.listen_addr).unwrap();
52        socket
53            .set_read_timeout(Some(Duration::from_millis(100)))
54            .unwrap();
55        self.socket = Some(socket);
56        Ok(())
57    }
58
59    fn process(&mut self, _clock: &RobotClock, new_msg: &mut Self::Output<'_>) -> CuResult<()> {
60        let socket = self.socket.as_ref().unwrap();
61        let mut packet = [0u8; size_of::<DataPacket>()];
62        let (read_size, _peer_addr) = socket.recv_from(&mut packet).unwrap();
63        let packet = &packet[..read_size];
64
65        let packet = Packet::from_slice(packet).unwrap();
66        let packets = [Ok::<Packet, ()>(packet)].into_iter(); // 🤮
67        let frame: FrameXyz = try_packet_to_frame_xyz(self.velo_config.clone(), packets)
68            .unwrap()
69            .next()
70            .unwrap()
71            .unwrap();
72        let mut output = PointCloudSoa::<10000>::default();
73
74        frame.firing_iter().for_each(|firing| {
75            firing.point_iter().for_each(|point| {
76                let point = point.as_single().unwrap();
77                let tov = point.toh; // FIXME: Needs to sync the clocks here.
78                let x = point.measurement.xyz[0].as_meters() as f32;
79                let y = point.measurement.xyz[1].as_meters() as f32;
80                let z = point.measurement.xyz[2].as_meters() as f32;
81                let intensity = point.measurement.intensity as f32 / 255.0f32;
82                output.push(PointCloud::new(tov.into(), x, y, z, intensity, None));
83            });
84        });
85        new_msg.set_payload(output);
86        Ok(())
87    }
88    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
89        self.socket = None;
90        Ok(())
91    }
92}
93
94#[cfg(test)]
95mod tests {
96    use super::*;
97    use cu_udp_inject::PcapStreamer;
98
99    #[test]
100    fn vlp16_end_2_end_test() {
101        let clk = RobotClock::new();
102        let cfg = ComponentConfig::new();
103        let mut drv = Vlp16::new(Some(&cfg)).unwrap();
104
105        let mut streamer = PcapStreamer::new("test/VLP_16_Single.pcap", "127.0.0.1:2368");
106
107        drv.start(&clk).unwrap();
108
109        const PACKET_SIZE: usize = size_of::<DataPacket>();
110
111        // Read test.pcap
112        if streamer
113            .send_next::<PACKET_SIZE>()
114            .expect("Failed to send packet")
115        {
116            let mut msg = CuMsg::new(Some(PointCloudSoa::<10000>::default()));
117            drv.process(&clk, &mut msg).unwrap();
118            assert_eq!(-0.05115497, msg.payload().unwrap().x[0].value);
119        }
120        drv.stop(&clk).unwrap();
121    }
122}
cu_vlp16/lib.rs

cu_vlp16/
lib.rs
