cu_livox/
parser.rs

1use bytemuck::{Pod, Zeroable};
2use chrono::{DateTime, Utc};
3use cu29::prelude::{CuDuration, CuTime};
4use cu29::units::si::f32::{Length, Ratio};
5use cu29::units::si::length::millimeter;
6use cu29::units::si::ratio::ratio;
7use std::error::Error;
8use std::fmt;
9use std::fmt::{Debug, Formatter};
10use std::mem::size_of;
11
12#[inline(always)]
13fn u16_endianness(val: u16) -> u16 {
14    if cfg!(target_endian = "little") {
15        val
16    } else {
17        u16::from_le(val)
18    }
19}
20
21#[inline(always)]
22fn u32_endianness(val: u32) -> u32 {
23    if cfg!(target_endian = "little") {
24        val
25    } else {
26        u32::from_le(val)
27    }
28}
29
30#[inline(always)]
31fn i32_endianness(val: i32) -> i32 {
32    if cfg!(target_endian = "little") {
33        val
34    } else {
35        i32::from_le(val)
36    }
37}
38
39#[inline(always)]
40fn u64_endianness(val: u64) -> u64 {
41    if cfg!(target_endian = "little") {
42        val
43    } else {
44        u64::from_le(val)
45    }
46}
47
48//Tele15
49//https://github.com/Livox-SDK/Livox-SDK/wiki/Livox-SDK-Communication-Protocol
50// HAP
51//https://github.com/Livox-SDK/Livox-SDK2/wiki/Livox-SDK-Communication-Protocol-HAP%28English%29
52#[derive(Debug)]
53pub enum LivoxError {
54    InvalidFrame(String),
55    InvalidTimestamp(String),
56}
57
58#[repr(C, packed)]
59#[derive(Copy, Clone, Zeroable, Pod)]
60pub struct CommandHeader {
61    sof: u8,      // Start of frame
62    version: u8,  // Protocol version
63    length: u16,  // frame length
64    cmd_type: u8, // command type
65    seq_num: u8,  // Frame Sequence Number                    |
66    crc_16: u16,  // Frame Header Checksum
67}
68
69impl Debug for CommandHeader {
70    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
71        f.write_fmt(format_args!("Magic: {:2X}{:2X}", self.sof, self.version))?;
72        f.write_fmt(format_args!(
73            "Frame: length {}, cmd_type{:2X}",
74            u16_endianness(self.length),
75            self.cmd_type
76        ))
77    }
78}
79
80// Command frame format
81//
82// The Protocol Frame is the smallest unit for control command. Format is as
83// follows:
84//
85// | Field    | Index (byte) | Size (byte) | Description                              |
86// | -------- | ------------ | ----------- | ---------------------------------------- |
87// | sof      | 0            | 1           | Starting Byte, Fixed to be 0xAA          |
88// | version  | 1            | 1           | Protocol Version, 1 for The Current Version |
89// | length   | 2            | 2           | Length of The Frame, Max Value: 1400     |
90// | cmd_type | 4            | 1           | Command Type: <br>0x00: CMD <br>0x01: ACK <br>0x02: MSG |
91// | seq_num  | 5            | 2           | Frame Sequence Number                    |
92// | crc_16   | 7            | 2           | Frame Header Checksum                    |
93// | data     | 9            | n           | Payload Data                             |
94// | crc_32   | 9+n          | 4           | Whole Frame Checksum                     |
95//
96// Command type description:
97// > - **CMD (request)**: Actively send data request - need to return a corresponding ACK;
98// > - **ACK (response)**: Response to CMD data;
99// > - **MSG (message)**: Actively pushed message data - no need to return response data,
100//
101// > e.g. broadcast data, pushed message when an error occurs;
102
103#[repr(C, packed)]
104#[derive(Copy, Clone, Zeroable, Pod, Debug)]
105pub struct CommandFrame {
106    pub header: CommandHeader,
107    pub date: u8,
108    pub crc_32: u32,
109}
110
111// LiDAR Status Code
112//
113// LiDAR status_code consists of 32 bits, which has the following meanings:
114//
115// | Bits     | Data             | Description                                                  |
116// | -------- | ---------------- | ------------------------------------------------------------ |
117// | Bit0:1   | temp_status      | 0: Temperature in Normal State <br>1: High or Low <br>2: Extremely High or Extremely Low |
118// | Bit2:3   | volt_status      | Voltage Status of Internal Module<br> 0: Voltage in Normal State <br>1: High <br>2: Extremely High |
119// | Bit4:5   | motor_status     | 0: Motor in Normal State <br>1: Motor in Warning State <br>2: Motor in Error State, Unable to Work |
120// | Bit6:7   | dirty_warn       | 0: Not Dirty or Blocked <br>1: Dirty or Blocked              |
121// | Bit8     | firmware_status  | 0: Firmware is OK <br>1: Firmware is Abnormal, Need to be Upgraded |
122// | Bit9     | pps_status       | 0: No PPS Signal <br>1: PPS Signal is OK                     |
123// | Bit10    | device_status    | 0: Normal <br>1: Warning for Approaching the End of Service Life |
124// | Bit11    | fan_status       | 0: Fan in Normal State <br>1: Fan in Warning State<br>**Supported devices:** <br/>Mid-40/03.07.0000+ <br/>Horizon/06.04.0000+ <br/>Tele-15/07.03.0000+ |
125// | Bit12    | self_heating     | 0: Low Temperature Self Heating On<br>1: Low Temperature Self Heating Off<br>**Supported devices:** <br/>Horizon/06.04.0000+ <br/>Tele-15/07.03.0000+<br/>Mid-70/10.03.0000+<br />Avia/11.06.0000+<br> |
126// | Bit13    | ptp_status       | 0: No 1588 Signal <br>1: 1588 Signal is OK                   |
127// | Bit14:16 | time_sync_status | 0: System dose not start time synchronization <br>1: Using PTP 1588 synchronization <br>2: Using GPS synchronization <br>3: Using PPS synchronization <br>4: System time synchronization is abnormal (The highest priority synchronization signal is abnormal) |
128// | Bit17:29 | RSVD             |                                                              |
129// | Bit30:31 | system_status    | 0: Normal <br>1: Warning <br>Any of the following situations will trigger warning: <br>  1.1 `temp_status` is 1;<br>  1.2 `volt_status` is 1;<br>  1.3 `motor_status` is 1;<br>  1.4 `dirty_warn` is 1;<br>  1.5 `device_status` is 1; <br>  1.6 `fan_status` is 1;<br>2: Error <br>Causes the LiDAR to Shut Down and Enter the Error State. <br>Any of the following situations will trigger error: <br>  2.1 `temp_status` is 2;<br>  2.2 `volt_status` is 2;<br>  2.3 `motor_status` is 2;<br>  2.4 `firmware_status` is 1; |
130
131// Timestamp Type:
132//
133// | Timestamp Type | Source         | Data Type | Description                       |
134// | -------------- | -------------- | --------- | --------------------------------- |
135// | 0              | No sync source | uint64_t  | Unit: ns                          |
136// | 1              | PTP            | uint64_t  | Unit: ns                          |
137// | 2              | Reserved       |           |                                   |
138// | 3              | GPS            | UTC       | UTC                               |
139// | 4              | PPS            | int64_t   | Unit: ns, only supported by LiDAR |
140
141#[repr(C, packed)]
142#[derive(Copy, Clone, Zeroable, Pod)]
143pub struct LidarHeader {
144    pub version: u8,
145    pub slot_id: u8,
146    pub lidar_id: u8,
147    reserved: u8,
148    pub status_code: u32,
149    pub timestamp_type: u8,
150    pub data_type: u8,
151    pub timestamp: u64,
152}
153
154impl Debug for crate::parser::LidarHeader {
155    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
156        f.write_fmt(format_args!(
157            "Version: {:2X} slot_id: {:2X} lidar_id: {:2X} ",
158            self.version, self.slot_id, self.lidar_id
159        ))?;
160        f.write_fmt(format_args!(
161            "Status: {}, ts_type{:2X}, data_type{:2X}, timestamp{}",
162            u32_endianness(self.status_code),
163            self.timestamp_type,
164            self.data_type,
165            u64_endianness(self.timestamp)
166        ))
167    }
168}
169
170impl LidarHeader {
171    pub fn timestamp(&self) -> CuDuration {
172        CuDuration(u64_endianness(self.timestamp))
173    }
174}
175
176#[repr(C, packed)]
177#[derive(Copy, Clone, Zeroable, Pod)]
178pub struct PointType2 {
179    x: i32,
180    y: i32,
181    z: i32,
182    pub reflectivity: u8,
183    pub tag: u8,
184}
185
186impl PointType2 {
187    pub fn x(&self) -> Length {
188        Length::new::<millimeter>(i32_endianness(self.x) as f32)
189    }
190    pub fn y(&self) -> Length {
191        Length::new::<millimeter>(i32_endianness(self.y) as f32)
192    }
193    pub fn z(&self) -> Length {
194        Length::new::<millimeter>(i32_endianness(self.z) as f32)
195    }
196    pub fn reflectivity(&self) -> Ratio {
197        Ratio::new::<ratio>(self.reflectivity as f32 / 255.0)
198    }
199    pub fn check_invariants(&self) -> Result<(), LivoxError> {
200        // TODO: determine the valid range for x,y,z
201        Ok(())
202    }
203}
204
205impl Debug for crate::parser::PointType2 {
206    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
207        f.write_fmt(format_args!(
208            "Point: ({}, {}, {}) reflectivity {:2X} tag  {:2X}",
209            i32_endianness(self.x),
210            i32_endianness(self.y),
211            i32_endianness(self.z),
212            self.reflectivity,
213            self.tag
214        ))
215    }
216}
217
218// **Data Type 2**
219//
220// Single return cartesian coordinate data format:
221//
222// | Field        | Offset (byte) | Data Type | Description                              |
223// | ------------ | ------------- | --------- | ---------------------------------------- |
224// | x            | 0             | int32_t   | X axis, Unit: mm                         |
225// | y            | 4             | int32_t   | Y axis, Unit: mm                         |
226// | z            | 8             | int32_t   | Z axis, Unit: mm                         |
227// | reflectivity | 12            | uint8_t   | Reflectivity                             |
228// | tag          | 13            | uint8_t   | For details, see [tag information](#3.4 Tag Information {#tag_info) |
229//
230
231// 3.4 Tag Information {#tag_info}
232//
233// | Bit  | Description                              |
234// | ---- | ---------------------------------------- |
235// | 7:6  | Reserved                                 |
236// | 5:4  | Return Number: <br>00: Return 0<br>01: Return 1<br>02: Return 2<br>03: Return 4 |
237// | 3:2  | Point property based on return intensity: <br>00: Normal<br>01: High confidence level of the noise<br>02: Moderate confidence level of the noise<br>03: Reserved |
238// | 1:0  | Point property based on spatial position:<br>0: Normal <br>1: High confidence level of the noise<br>2: Moderate confidence level of the noise<br>3: Low confidence level of the noise |
239//
240
241// Lidar frame format
242// | Field          | Index (byte) | Size (byte) | Description                              |
243// | -------------- | ------------ | ----------- | ---------------------------------------- |
244// | version        | 0            | 1           | Packet Protocol Version, 5 for the Current Version |
245// | slot_id        | 1            | 1           | ID of the Slot Connecting LiDAR Device:<br> If LiDAR connects directly to computer without Hub, default 1;<br> If LiDAR connects to computer through Hub,<br> then the ‘slot_id’ is the corresponding slot number of the hub (range: 1 ~ 9); |
246// | LiDAR_id       | 2            | 1           | LiDAR ID:<br> 1: Mid-100 Left / Mid-40 / Tele-15 / Horizon/Mid-70/Avia<br> 2: Mid-100 Middle<br> 3: Mid-100 Right<br>![connection_flow_chart](images/Livox-Mid100.png) |
247// | reserved       | 3            | 1           |                                          |
248// | status_code    | 4            | 4           | LiDAR Status Indicator Information, For details, see 3.4 |
249// | timestamp_type | 8            | 1           | Timestamp Type, For details, see [3.2](#3.2 Time Stamp {#timestamp}) |
250// | data_type      | 9            | 1           | Data Type, For details, see [3.3](#3.3 Point Cloud/IMU Data {#data_type}) |
251// | timestamp      | 10           | 8           | Nanosecond or UTC Format Timestamp, For details, see 3.2 |
252// | data           | 18           | --          | Data information, For details, see [3.3](#3.3 Point Cloud/IMU Data {#data_type}) |
253
254const MAX_POINTS_TYPE2: usize = 96;
255pub const DATA_FRAME_TYPE2_SIZE: usize = size_of::<LidarFrame>();
256
257#[repr(C, packed)]
258#[derive(Copy, Clone, Zeroable, Pod, Debug)]
259pub struct LidarFrame {
260    pub header: LidarHeader,
261    pub points: [PointType2; MAX_POINTS_TYPE2],
262}
263
264impl fmt::Display for LivoxError {
265    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
266        match self {
267            LivoxError::InvalidFrame(msg) => write!(f, "Invalid frame: {msg}"),
268            LivoxError::InvalidTimestamp(msg) => write!(f, "Invalid timestamp: {msg}"),
269        }
270    }
271}
272
273impl Error for LivoxError {}
274
275pub type RefTime = (DateTime<Utc>, CuTime);
276
277pub fn parse_frame(data: &[u8]) -> Result<&LidarFrame, LivoxError> {
278    if data[0] != 0x05
279    // Protocol version
280    {
281        return Err(LivoxError::InvalidFrame(format!(
282            "Not a Livox SDK protocol V1 frame: {:2X}",
283            data[0],
284        )));
285    }
286
287    if data.len() < DATA_FRAME_TYPE2_SIZE {
288        return Err(LivoxError::InvalidFrame(format!(
289            "Frame too short: {} < {}",
290            data.len(),
291            DATA_FRAME_TYPE2_SIZE
292        )));
293    }
294    if data.len() > DATA_FRAME_TYPE2_SIZE {
295        return Err(LivoxError::InvalidFrame(format!(
296            "Frame too long: {} > {}",
297            data.len(),
298            DATA_FRAME_TYPE2_SIZE
299        )));
300    }
301    let packet: &LidarFrame = bytemuck::from_bytes(&data[..DATA_FRAME_TYPE2_SIZE]);
302    if packet.header.data_type != 0x02 {
303        return Err(LivoxError::InvalidFrame(format!(
304            "Point Cloud Data Type 2 expected, received : {:2X}",
305            packet.header.data_type
306        )));
307    }
308    Ok(packet)
309}
310
311#[cfg(test)]
312mod tests {
313    use crate::parser::{LidarFrame, RefTime, parse_frame};
314    use chrono::prelude::*;
315    use cu29::prelude::{CuDuration, RobotClock};
316
317    #[test]
318    fn test_tele15_packet() {
319        let (robot_clock, mock) = RobotClock::mock();
320        // push the time by 1s because the first emulated test packet could end up in negative time.
321        mock.increment(CuDuration::from_secs(1));
322
323        let packet_data: [u8; 1362] = [
324            0x05, 0x01, 0x01, 0x00, 0x40, 0x68, 0x00, 0x40, 0x01, 0x02, 0x8B, 0x06, 0xAE, 0xE5,
325            0xB4, 0x12, 0xF6, 0x17, 0xF6, 0x5C, 0x00, 0x00, 0x08, 0x08, 0x00, 0x00, 0xAB, 0x05,
326            0x00, 0x00, 0x0F, 0x10, 0xFE, 0x5C, 0x00, 0x00, 0x0E, 0x08, 0x00, 0x00, 0x27, 0x05,
327            0x00, 0x00, 0x0E, 0x10, 0xE8, 0x5C, 0x00, 0x00, 0x11, 0x08, 0x00, 0x00, 0xA0, 0x04,
328            0x00, 0x00, 0x24, 0x10, 0x17, 0x5D, 0x00, 0x00, 0x1A, 0x08, 0x00, 0x00, 0x1B, 0x04,
329            0x00, 0x00, 0x0C, 0x10, 0xBD, 0x59, 0x00, 0x00, 0xD4, 0x07, 0x00, 0x00, 0x74, 0x03,
330            0x00, 0x00, 0x16, 0x10, 0xDD, 0x58, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0xED, 0x02,
331            0x00, 0x00, 0x09, 0x10, 0xE3, 0x5C, 0x00, 0x00, 0x2D, 0x08, 0x00, 0x00, 0xA7, 0x05,
332            0x00, 0x00, 0x0A, 0x10, 0xDE, 0x5C, 0x00, 0x00, 0x31, 0x08, 0x00, 0x00, 0x22, 0x05,
333            0x00, 0x00, 0x09, 0x10, 0xE0, 0x5C, 0x00, 0x00, 0x37, 0x08, 0x00, 0x00, 0x9D, 0x04,
334            0x00, 0x00, 0x1D, 0x10, 0x32, 0x5C, 0x00, 0x00, 0x2C, 0x08, 0x00, 0x00, 0x0D, 0x04,
335            0x00, 0x00, 0x07, 0x10, 0x27, 0x59, 0x00, 0x00, 0xEC, 0x07, 0x00, 0x00, 0x6B, 0x03,
336            0x00, 0x00, 0x0E, 0x10, 0x67, 0x54, 0x00, 0x00, 0x84, 0x07, 0x00, 0x00, 0xC4, 0x02,
337            0x00, 0x00, 0x1A, 0x10, 0xD5, 0x5C, 0x00, 0x00, 0x52, 0x08, 0x00, 0x00, 0xA3, 0x05,
338            0x00, 0x00, 0x0F, 0x10, 0xFA, 0x5C, 0x00, 0x00, 0x5A, 0x08, 0x00, 0x00, 0x1F, 0x05,
339            0x00, 0x00, 0x0B, 0x10, 0xAD, 0x5B, 0x00, 0x00, 0x41, 0x08, 0x00, 0x00, 0x89, 0x04,
340            0x00, 0x00, 0x04, 0x10, 0x0D, 0x5C, 0x00, 0x00, 0x4E, 0x08, 0x00, 0x00, 0x07, 0x04,
341            0x00, 0x00, 0x07, 0x10, 0xDD, 0x59, 0x00, 0x00, 0x21, 0x08, 0x00, 0x00, 0x6E, 0x03,
342            0x00, 0x00, 0x13, 0x10, 0x55, 0x54, 0x00, 0x00, 0xA5, 0x07, 0x00, 0x00, 0xBF, 0x02,
343            0x00, 0x00, 0x18, 0x10, 0xC4, 0x5B, 0x00, 0x00, 0x5E, 0x08, 0x00, 0x00, 0x8D, 0x05,
344            0x00, 0x00, 0x64, 0x10, 0xC1, 0x5A, 0x00, 0x00, 0x4B, 0x08, 0x00, 0x00, 0xFB, 0x04,
345            0x00, 0x00, 0x04, 0x10, 0xA4, 0x5B, 0x00, 0x00, 0x65, 0x08, 0x00, 0x00, 0x84, 0x04,
346            0x00, 0x00, 0x10, 0x10, 0x2C, 0x5C, 0x00, 0x00, 0x76, 0x08, 0x00, 0x00, 0x04, 0x04,
347            0x00, 0x00, 0x14, 0x10, 0xAD, 0x56, 0x00, 0x00, 0xF9, 0x07, 0x00, 0x00, 0x4A, 0x03,
348            0x00, 0x00, 0x04, 0x10, 0x37, 0x54, 0x00, 0x00, 0xC4, 0x07, 0x00, 0x00, 0xB9, 0x02,
349            0x00, 0x00, 0x18, 0x10, 0xE4, 0x59, 0x00, 0x00, 0x56, 0x08, 0x00, 0x00, 0x6B, 0x05,
350            0x00, 0x00, 0x04, 0x10, 0xA4, 0x5B, 0x00, 0x00, 0x84, 0x08, 0x00, 0x00, 0x02, 0x05,
351            0x00, 0x00, 0x0C, 0x10, 0xB4, 0x5B, 0x00, 0x00, 0x8A, 0x08, 0x00, 0x00, 0x7F, 0x04,
352            0x00, 0x00, 0x13, 0x10, 0xC8, 0x5B, 0x00, 0x00, 0x91, 0x08, 0x00, 0x00, 0xF9, 0x03,
353            0x00, 0x00, 0x0D, 0x10, 0xF6, 0x58, 0x00, 0x00, 0x52, 0x08, 0x00, 0x00, 0x5A, 0x03,
354            0x00, 0x00, 0x0E, 0x10, 0x6E, 0x52, 0x00, 0x00, 0xBA, 0x07, 0x00, 0x00, 0xA5, 0x02,
355            0x00, 0x00, 0x13, 0x10, 0x00, 0x5B, 0x00, 0x00, 0x95, 0x08, 0x00, 0x00, 0x76, 0x05,
356            0x00, 0x00, 0x0B, 0x10, 0x21, 0x5B, 0x00, 0x00, 0x9C, 0x08, 0x00, 0x00, 0xF4, 0x04,
357            0x00, 0x00, 0x11, 0x10, 0xAB, 0x5A, 0x00, 0x00, 0x95, 0x08, 0x00, 0x00, 0x6B, 0x04,
358            0x00, 0x00, 0x12, 0x10, 0xC1, 0x5A, 0x00, 0x00, 0x9B, 0x08, 0x00, 0x00, 0xE7, 0x03,
359            0x00, 0x00, 0x0B, 0x10, 0x14, 0x58, 0x00, 0x00, 0x5F, 0x08, 0x00, 0x00, 0x4B, 0x03,
360            0x00, 0x00, 0x0E, 0x10, 0x92, 0x52, 0x00, 0x00, 0xDD, 0x07, 0x00, 0x00, 0xA0, 0x02,
361            0x00, 0x00, 0x10, 0x10, 0x4A, 0x5A, 0x00, 0x00, 0xA6, 0x08, 0x00, 0x00, 0x64, 0x05,
362            0x00, 0x00, 0x0E, 0x10, 0x10, 0x5A, 0x00, 0x00, 0xA4, 0x08, 0x00, 0x00, 0xDE, 0x04,
363            0x00, 0x00, 0x12, 0x10, 0xAE, 0x59, 0x00, 0x00, 0x9F, 0x08, 0x00, 0x00, 0x57, 0x04,
364            0x00, 0x00, 0x13, 0x10, 0x92, 0x59, 0x00, 0x00, 0xA1, 0x08, 0x00, 0x00, 0xD3, 0x03,
365            0x00, 0x00, 0x0A, 0x10, 0x13, 0x57, 0x00, 0x00, 0x68, 0x08, 0x00, 0x00, 0x3A, 0x03,
366            0x00, 0x00, 0x0D, 0x10, 0x4B, 0x4F, 0x00, 0x00, 0xAB, 0x07, 0x00, 0x00, 0x7E, 0x02,
367            0x00, 0x00, 0x11, 0x10, 0x51, 0x59, 0x00, 0x00, 0xB0, 0x08, 0x00, 0x00, 0x4D, 0x05,
368            0x00, 0x00, 0x0D, 0x10, 0x14, 0x59, 0x00, 0x00, 0xAE, 0x08, 0x00, 0x00, 0xC8, 0x04,
369            0x00, 0x00, 0x12, 0x10, 0xBE, 0x58, 0x00, 0x00, 0xAA, 0x08, 0x00, 0x00, 0x43, 0x04,
370            0x00, 0x00, 0x12, 0x10, 0x89, 0x58, 0x00, 0x00, 0xA9, 0x08, 0x00, 0x00, 0xBF, 0x03,
371            0x00, 0x00, 0x0B, 0x10, 0x3C, 0x56, 0x00, 0x00, 0x74, 0x08, 0x00, 0x00, 0x2A, 0x03,
372            0x00, 0x00, 0x0E, 0x10, 0xB2, 0x4E, 0x00, 0x00, 0xBA, 0x07, 0x00, 0x00, 0x71, 0x02,
373            0x00, 0x00, 0x08, 0x10, 0x5E, 0x58, 0x00, 0x00, 0xBA, 0x08, 0x00, 0x00, 0x35, 0x05,
374            0x00, 0x00, 0x0C, 0x10, 0x21, 0x58, 0x00, 0x00, 0xB8, 0x08, 0x00, 0x00, 0xB2, 0x04,
375            0x00, 0x00, 0x12, 0x10, 0xD0, 0x57, 0x00, 0x00, 0xB3, 0x08, 0x00, 0x00, 0x2F, 0x04,
376            0x00, 0x00, 0x12, 0x10, 0x89, 0x57, 0x00, 0x00, 0xB0, 0x08, 0x00, 0x00, 0xAB, 0x03,
377            0x00, 0x00, 0x0E, 0x10, 0x34, 0x55, 0x00, 0x00, 0x79, 0x08, 0x00, 0x00, 0x17, 0x03,
378            0x00, 0x00, 0x0E, 0x10, 0x06, 0x4E, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0x64, 0x02,
379            0x00, 0x00, 0x11, 0x10, 0x6C, 0x57, 0x00, 0x00, 0xC2, 0x08, 0x00, 0x00, 0x1E, 0x05,
380            0x00, 0x00, 0x0D, 0x10, 0x35, 0x57, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x9C, 0x04,
381            0x00, 0x00, 0x12, 0x10, 0xEB, 0x56, 0x00, 0x00, 0xBC, 0x08, 0x00, 0x00, 0x1B, 0x04,
382            0x00, 0x00, 0x12, 0x10, 0xB3, 0x56, 0x00, 0x00, 0xBA, 0x08, 0x00, 0x00, 0x98, 0x03,
383            0x00, 0x00, 0x0D, 0x10, 0x27, 0x54, 0x00, 0x00, 0x7C, 0x08, 0x00, 0x00, 0x04, 0x03,
384            0x00, 0x00, 0x0E, 0x10, 0x64, 0x4D, 0x00, 0x00, 0xD1, 0x07, 0x00, 0x00, 0x56, 0x02,
385            0x00, 0x00, 0x16, 0x10, 0x81, 0x56, 0x00, 0x00, 0xC9, 0x08, 0x00, 0x00, 0x06, 0x05,
386            0x00, 0x00, 0x0E, 0x10, 0x58, 0x56, 0x00, 0x00, 0xC8, 0x08, 0x00, 0x00, 0x87, 0x04,
387            0x00, 0x00, 0x12, 0x10, 0x18, 0x56, 0x00, 0x00, 0xC5, 0x08, 0x00, 0x00, 0x07, 0x04,
388            0x00, 0x00, 0x10, 0x10, 0xF6, 0x55, 0x00, 0x00, 0xC5, 0x08, 0x00, 0x00, 0x86, 0x03,
389            0x00, 0x00, 0x0A, 0x10, 0x81, 0x4A, 0x00, 0x00, 0x9D, 0x07, 0x00, 0x00, 0xA2, 0x02,
390            0x00, 0x00, 0x00, 0x10, 0x64, 0x4A, 0x00, 0x00, 0x9D, 0x07, 0x00, 0x00, 0x36, 0x02,
391            0x00, 0x00, 0x25, 0x10, 0xA5, 0x55, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0xEF, 0x04,
392            0x00, 0x00, 0x0E, 0x10, 0x80, 0x55, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x71, 0x04,
393            0x00, 0x00, 0x11, 0x10, 0x4A, 0x55, 0x00, 0x00, 0xCE, 0x08, 0x00, 0x00, 0xF2, 0x03,
394            0x00, 0x00, 0x0E, 0x10, 0x36, 0x55, 0x00, 0x00, 0xCF, 0x08, 0x00, 0x00, 0x74, 0x03,
395            0x00, 0x00, 0x09, 0x10, 0x8F, 0x4A, 0x00, 0x00, 0xB8, 0x07, 0x00, 0x00, 0x99, 0x02,
396            0x00, 0x00, 0x03, 0x10, 0x10, 0x4A, 0x00, 0x00, 0xAE, 0x07, 0x00, 0x00, 0x2A, 0x02,
397            0x00, 0x00, 0x23, 0x10, 0xB1, 0x54, 0x00, 0x00, 0xD4, 0x08, 0x00, 0x00, 0xD5, 0x04,
398            0x00, 0x00, 0x0E, 0x10, 0xA7, 0x54, 0x00, 0x00, 0xD6, 0x08, 0x00, 0x00, 0x5A, 0x04,
399            0x00, 0x00, 0x0E, 0x10, 0x91, 0x54, 0x00, 0x00, 0xD7, 0x08, 0x00, 0x00, 0xDE, 0x03,
400            0x00, 0x00, 0x0B, 0x10, 0x7E, 0x54, 0x00, 0x00, 0xD8, 0x08, 0x00, 0x00, 0x61, 0x03,
401            0x00, 0x00, 0x09, 0x10, 0x8F, 0x4A, 0x00, 0x00, 0xD1, 0x07, 0x00, 0x00, 0x8F, 0x02,
402            0x00, 0x00, 0x07, 0x10, 0x6E, 0x49, 0x00, 0x00, 0xB6, 0x07, 0x00, 0x00, 0x1B, 0x02,
403            0x00, 0x00, 0x1D, 0x10, 0xDD, 0x53, 0x00, 0x00, 0xDA, 0x08, 0x00, 0x00, 0xBD, 0x04,
404            0x00, 0x00, 0x0E, 0x10, 0xD9, 0x53, 0x00, 0x00, 0xDD, 0x08, 0x00, 0x00, 0x43, 0x04,
405            0x00, 0x00, 0x0B, 0x10, 0xC9, 0x53, 0x00, 0x00, 0xDE, 0x08, 0x00, 0x00, 0xC9, 0x03,
406            0x00, 0x00, 0x0B, 0x10, 0xDE, 0x53, 0x00, 0x00, 0xE4, 0x08, 0x00, 0x00, 0x4E, 0x03,
407            0x00, 0x00, 0x08, 0x10, 0x8D, 0x4B, 0x00, 0x00, 0x05, 0x08, 0x00, 0x00, 0x8C, 0x02,
408            0x00, 0x00, 0x09, 0x10, 0x1E, 0x49, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0x0E, 0x02,
409            0x00, 0x00, 0x14, 0x10, 0x24, 0x53, 0x00, 0x00, 0xE1, 0x08, 0x00, 0x00, 0xA6, 0x04,
410            0x00, 0x00, 0x0E, 0x10, 0x21, 0x53, 0x00, 0x00, 0xE4, 0x08, 0x00, 0x00, 0x2D, 0x04,
411            0x00, 0x00, 0x0A, 0x10, 0x16, 0x53, 0x00, 0x00, 0xE6, 0x08, 0x00, 0x00, 0xB4, 0x03,
412            0x00, 0x00, 0x0B, 0x10, 0x32, 0x53, 0x00, 0x00, 0xEC, 0x08, 0x00, 0x00, 0x3B, 0x03,
413            0x00, 0x00, 0x08, 0x10, 0x1E, 0x4B, 0x00, 0x00, 0x11, 0x08, 0x00, 0x00, 0x7D, 0x02,
414            0x00, 0x00, 0x11, 0x10, 0x81, 0x48, 0x00, 0x00, 0xCC, 0x07, 0x00, 0x00, 0xFE, 0x01,
415            0x00, 0x00, 0x16, 0x10, 0x7E, 0x52, 0x00, 0x00, 0xEA, 0x08, 0x00, 0x00, 0x90, 0x04,
416            0x00, 0x00, 0x0B, 0x10, 0x6F, 0x52, 0x00, 0x00, 0xEB, 0x08, 0x00, 0x00, 0x17, 0x04,
417            0x00, 0x00, 0x0B, 0x10, 0x67, 0x52, 0x00, 0x00, 0xED, 0x08, 0x00, 0x00, 0x9F, 0x03,
418            0x00, 0x00, 0x0A, 0x10, 0x8C, 0x52, 0x00, 0x00, 0xF4, 0x08, 0x00, 0x00, 0x27, 0x03,
419            0x00, 0x00, 0x08, 0x10, 0xA9, 0x4A, 0x00, 0x00, 0x1C, 0x08, 0x00, 0x00, 0x6D, 0x02,
420            0x00, 0x00, 0x11, 0x10, 0x85, 0x48, 0x00, 0x00, 0xE3, 0x07, 0x00, 0x00, 0xF2, 0x01,
421            0x00, 0x00, 0x15, 0x10,
422        ];
423
424        if packet_data.len() < size_of::<LidarFrame>() {
425            panic!("Packet too short: {}", packet_data.len());
426        }
427
428        let packet = parse_frame(&packet_data).unwrap();
429
430        let datetime = Utc.with_ymd_and_hms(1970, 1, 1, 0, 0, 0).unwrap();
431        let _rt: RefTime = (datetime, robot_clock.now());
432        let timestamp = packet.header.timestamp;
433        println!("Tov: {timestamp}");
434    }
435}
cu_livox/parser.rs

cu_livox/
parser.rs
