cu_hesai/
parser.rs

1use bytemuck::{Pod, Zeroable};
2use chrono::{DateTime, MappedLocalTime, TimeZone, Utc};
3use cu29::prelude::{CuDuration, CuTime};
4use cu29::units::si::angle::degree;
5use cu29::units::si::angular_velocity::revolution_per_minute;
6use cu29::units::si::f32::{Angle, AngularVelocity, Length, Ratio};
7use cu29::units::si::length::millimeter;
8use cu29::units::si::ratio::{percent, ratio};
9use std::error::Error;
10use std::fmt;
11use std::fmt::{Debug, Formatter};
12use std::mem::size_of;
13
14#[derive(Debug)]
15pub enum HesaiError {
16    InvalidPacket(String),
17    InvalidTimestamp(String),
18}
19
20impl fmt::Display for HesaiError {
21    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
22        match self {
23            HesaiError::InvalidPacket(msg) => write!(f, "Invalid packet: {msg}"),
24            HesaiError::InvalidTimestamp(msg) => write!(f, "Invalid timestamp: {msg}"),
25        }
26    }
27}
28
29impl Error for HesaiError {}
30
31// ╭──────────────────────────────────────────────────────────────────────────────╮
32// │                              Pre-Header (6 bytes)                            │
33// ├──────────────────────────────┬─────────┬─────────────────────────────────────┤
34// │ Field                        │ Bytes   │ Description                         │
35// ├──────────────────────────────┼─────────┼─────────────────────────────────────┤
36// │ 0xEE (SOP)                   │ 1       │ Start of packet (constant: 0xEE)    │
37// │ 0xFF (SOP)                   │ 1       │ Start of packet (constant: 0xFF)    │
38// │ Protocol Version Major       │ 1       │ PandarXT series uses 0x06           │
39// │ Protocol Version Minor       │ 1       │ Current protocol version (0x01)     │
40// │ Reserved                     │ 2       │ Reserved bytes                      │
41// ╰──────────────────────────────┴─────────┴─────────────────────────────────────╯
42#[repr(C, packed)]
43#[derive(Copy, Clone, Zeroable, Pod)]
44pub struct PreHeader {
45    sop1: u8,
46    sop2: u8,
47    protocol_version_major: u8,
48    protocol_version_minor: u8,
49    reserved: [u8; 2],
50}
51
52impl Debug for PreHeader {
53    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
54        f.write_fmt(format_args!("Magic: {:2X}{:2X}", self.sop1, self.sop2))?;
55        f.write_fmt(format_args!(
56            "\nVersion {}.{}",
57            self.protocol_version_major, self.protocol_version_minor
58        ))
59    }
60}
61
62// ╭──────────────────────────────────────────────────────────────────────────────╮
63// │                                 Header (6 bytes)                             │
64// ├──────────────────────────────┬─────────┬─────────────────────────────────────┤
65// │ Field                        │ Bytes   │ Description                         │
66// ├──────────────────────────────┼─────────┼─────────────────────────────────────┤
67// │ Laser Num                    │ 1       │ Constant 0x20 (32 channels)         │
68// │ Block Num                    │ 1       │ Constant 0x08 (8 blocks per packet) │
69// │ First Block Return           │ 1       │ 0x00 = Single Return                │
70// │                              │         │ 0x01 = Last Return in Dual Return   │
71// │ Dis Unit                     │ 1       │ Constant 0x04 (4 mm)                │
72// │ Return Number                │ 1       │ 0x01 = One return (max)             │
73// │                              │         │ 0x02 = Two returns (max             │
74// │ UDP Seq                      │ 1       │ Always 0x01 for PandarXT            │
75// ╰──────────────────────────────┴─────────┴─────────────────────────────────────╯
76#[repr(C, packed)]
77#[derive(Copy, Clone, Zeroable, Pod)]
78pub struct Header {
79    laser_num: u8,
80    block_num: u8,
81    first_block_return: u8,
82    dis_unit: u8,
83    return_number: u8,
84    udp_seq: u8,
85}
86
87impl Header {
88    pub fn is_dual_return(self) -> bool {
89        self.return_number == 1
90    }
91
92    pub fn distance_unit(self) -> Length {
93        Length::new::<millimeter>(self.dis_unit as f32)
94    }
95
96    pub fn check_invariants(self) -> Result<(), HesaiError> {
97        if self.laser_num != 0x20 {
98            return Err(HesaiError::InvalidPacket(format!(
99                "Invalid laser number: 0x{:x}",
100                self.laser_num
101            )));
102        }
103        if self.block_num != 0x08 {
104            return Err(HesaiError::InvalidPacket(format!(
105                "Invalid block number: 0x{:x}",
106                self.block_num
107            )));
108        }
109        if self.dis_unit != 0x04 {
110            return Err(HesaiError::InvalidPacket(format!(
111                "Invalid distance unit: 0x{:x}",
112                self.dis_unit
113            )));
114        }
115        if self.udp_seq != 0x01 {
116            return Err(HesaiError::InvalidPacket(format!(
117                "Invalid UDP sequence: 0x{:x}",
118                self.udp_seq
119            )));
120        }
121        Ok(())
122    }
123}
124
125impl Debug for Header {
126    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
127        writeln!(f, "Laser num: {:02x}", self.laser_num)?;
128        writeln!(f, "Block num: {:02x}", self.block_num)?;
129        writeln!(f, "First Block return: {}", self.is_dual_return())?;
130        writeln!(
131            f,
132            "Distance unit: {} mm",
133            self.distance_unit().get::<millimeter>()
134        )?;
135        writeln!(f, "UDP Seq: {}", self.udp_seq)
136    }
137}
138
139// Body Block Definition (each block of 130 bytes)
140//
141// ╭──────────────────────────────────────────────────────────────────────────────╮
142// │                             Body (1040 bytes)                                │
143// │                            (8 blocks, 130 bytes each)                        │
144// ├──────────────────────────────┬─────────┬─────────────────────────────────────┤
145// │ Field                        │ Bytes   │ Description                         │
146// ├──────────────────────────────┼─────────┼─────────────────────────────────────┤
147// │ Azimuth                      │ 2       │ Azimuth angle (in hundredths of a   │
148// │                              │         │ degree, little-endian)              │
149// │ Channels 1 to 32             │ 128     │ Distance (2 bytes), Reflectivity    │
150// │                              │         │ (1 byte), Reserved (1 byte)         │
151// ├──────────────────────────────┴─────────┴─────────────────────────────────────┤
152// │ Notes:                                                                       │
153// │ - Each block consists of 130 bytes, where Azimuth data and Channels are      │
154// │   stored. The Distance value must be multiplied by 4 to get mm.              │
155// │ - Reflectivity is stored as a 1-byte percentage (0-255).                     │
156// ╰──────────────────────────────────────────────────────────────────────────────╯
157#[repr(C, packed)]
158#[derive(Copy, Clone, Zeroable, Pod)]
159pub struct Block {
160    azimuth: u16,                // Azimuth Angle
161    pub channels: [Channel; 32], // 32 channels per block
162}
163
164impl Block {
165    pub fn azimuth(&self) -> Angle {
166        // it is in 100th of degrees.
167        Angle::new::<degree>(self.azimuth as f32 / 100.0)
168    }
169
170    pub fn check_invariants(self) -> Result<(), HesaiError> {
171        if self.azimuth > 36000 {
172            return Err(HesaiError::InvalidPacket(format!(
173                "Invalid azimuth: {} deg",
174                self.azimuth().get::<degree>()
175            )));
176        }
177        for channel in self.channels.iter() {
178            channel.check_invariants()?;
179        }
180        Ok(())
181    }
182}
183
184impl Debug for Block {
185    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
186        writeln!(f, "Azimuth: {:>06.2} deg", self.azimuth().get::<degree>(),)?;
187        writeln!(f, "Channels:\n{:?}", self.channels)
188    }
189}
190
191// Channel Definition
192//
193// ╭──────────────────────────────────────────────────────────────────────────────╮
194// │                             Channel (4 bytes)                                │
195// ├──────────────────────────────┬─────────┬─────────────────────────────────────┤
196// │ Field                        │ Bytes   │ Description                         │
197// ├──────────────────────────────┼─────────┼─────────────────────────────────────┤
198// │ Distance                     │ 2       │ Distance / 4mm (little-endian)      │
199// │ Reflectivity                 │ 1       │ Reflectivity in percentage          │
200// │ Reserved                     │ 1       │ Reserved byte                       │
201// ╰──────────────────────────────┴─────────┴─────────────────────────────────────╯
202//
203#[repr(C, packed)]
204#[derive(Copy, Clone, Zeroable, Pod)]
205pub struct Channel {
206    distance: u16,    // !! raw endianness
207    reflectivity: u8, // Reflectivity in percentage
208    reserved: u8,     // Reserved byte
209}
210
211impl Channel {
212    pub fn distance(&self) -> Length {
213        Length::new::<millimeter>(u16_endianness(self.distance) as f32 * 4.0) // unharcode 4mm if we port this to another sensor.
214    }
215    pub fn reflectivity(&self) -> Ratio {
216        Ratio::new::<ratio>(self.reflectivity as f32 / 255.0)
217    }
218    pub fn check_invariants(&self) -> Result<(), HesaiError> {
219        // TODO: determine the valid range for distance
220        Ok(())
221    }
222}
223
224impl Debug for Channel {
225    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
226        writeln!(f, "Distance: {} mm", self.distance().get::<millimeter>())?;
227        writeln!(
228            f,
229            "Reflectivity: {:>06.5} %",
230            self.reflectivity().get::<percent>()
231        )
232    }
233}
234
235// Tail Definition
236//
237// ╭──────────────────────────────────────────────────────────────────────────────╮
238// │                                  Tail (24 bytes)                             │
239// ├──────────────────────────────┬─────────┬─────────────────────────────────────┤
240// │ Field                        │ Bytes   │ Description                         │
241// ├──────────────────────────────┼─────────┼─────────────────────────────────────┤
242// │ Reserved                     │ 10      │ Reserved bytes                      │
243// │ Return Mode                  │ 1       │ 0x37 = Strongest Return             │
244// │                              │         │ 0x38 = Last Return                  │
245// │                              │         │ 0x39 = Dual Return                  │
246// │ High Temp Shutdown Flag      │ 1       │ 0x01 = High temp, 0x00 = Normal     │
247// │ Motor Speed                  │ 2       │ Motor speed in RPM                  │
248// │ Date & Time                  │ 6       │ Timestamp (year, month, day, etc.)  │
249// │ Timestamp (µs)               │ 4       │ Timestamp in microseconds           │
250// │ Factory Info                 │ 1       │ Factory-specific info  (0x42)       │
251// ╰──────────────────────────────┴─────────┴─────────────────────────────────────╯
252
253#[repr(C, packed)]
254#[derive(Copy, Clone, Zeroable, Pod)]
255pub struct Tail {
256    reserved: [u8; 10],
257    return_mode: u8,
258    motor_speed: u16, // !! raw endianness, use u16_endianness to convert
259
260    // ╭─────────────────────────────────────────────╮
261    // │ The absolute UTC time of this data packet,  │
262    // │ accurate to the second.                     │
263    // ├───────────────────────┬─────────────────────┤
264    // │       Each Byte       │        Range        │
265    // ├───────────────────────┼─────────────────────┤
266    // │ Year (current year    │       ≥70           │
267    // │ minus 1900)           │                     │
268    // │ Month                 │       1 to 12       │
269    // │ Day                   │       1 to 31       │
270    // │ Hour                  │       0 to 23       │
271    // │ Minute                │       0 to 59       │
272    // │ Second                │       0 to 59       │
273    // ╰───────────────────────┴─────────────────────╯
274    date_time: [u8; 6],
275
276    // The "μs time" part of the absolute time of this data packet (defined in Appendix II)
277    // Unit: μs
278    // Range: 0 to 1000000 μs (1 s)
279    timestamp: u32,
280
281    // Should be 0x42
282    factory_info: u8,
283}
284
285#[derive(Debug)]
286enum ReturnMode {
287    First,
288    Strongest,
289    Last,
290    LastAndStrongest, // Default
291    LastAndFirst,
292    FirstAndStrongest,
293}
294
295impl Tail {
296    fn motor_speed(&self) -> AngularVelocity {
297        AngularVelocity::new::<revolution_per_minute>(u16_endianness(self.motor_speed) as f32)
298    }
299
300    fn return_mode(&self) -> Result<ReturnMode, HesaiError> {
301        match self.return_mode {
302            0x33 => Ok(ReturnMode::First),
303            0x37 => Ok(ReturnMode::Strongest),
304            0x38 => Ok(ReturnMode::Last),
305            0x39 => Ok(ReturnMode::LastAndStrongest),
306            0x3B => Ok(ReturnMode::LastAndFirst),
307            0x3C => Ok(ReturnMode::FirstAndStrongest),
308            _ => Err(HesaiError::InvalidPacket(format!(
309                "Invalid return mode: 0x{:x}",
310                self.return_mode
311            ))),
312        }
313    }
314
315    fn utc_tov(&self) -> Result<DateTime<Utc>, HesaiError> {
316        match Utc.with_ymd_and_hms(
317            self.date_time[0] as i32 + 1900,
318            self.date_time[1] as u32,
319            self.date_time[2] as u32,
320            self.date_time[3] as u32,
321            self.date_time[4] as u32,
322            self.date_time[5] as u32,
323        ) {
324            MappedLocalTime::None => Err(HesaiError::InvalidTimestamp("No such local time".into())),
325            MappedLocalTime::Single(t) => {
326                Ok(t + chrono::Duration::microseconds(self.timestamp as i64))
327            }
328            MappedLocalTime::Ambiguous(_t1, _t2) => {
329                Err(HesaiError::InvalidTimestamp("Ambiguous time".into()))
330                // If that happens, good luck. ¯\_(ツ)_/¯
331            }
332        }
333    }
334
335    // Lidar timestamp to monotonic time of validity.
336    // RefTime is a tuple of (DateTime<Utc>, CuTime) to convert the UTC time to a monotonic time.
337    // You can create it and update it doing DateTime<Utc>::now() and CuTime::now() respectively
338    // if the system clock is precise enough and sync'ed with the lidar.
339    fn tov(&self, reftime: &(DateTime<Utc>, CuTime)) -> Result<CuTime, HesaiError> {
340        // This hesai API is terrible and based on UTC, here we give a function to convert it to a monotonic robot time.
341        // UTC is corrected to match earth rotation so it is NOT suitable for robotic applications.
342        let (ref_date, ref_cu_time) = reftime;
343        let utc_tov = self.utc_tov()?;
344
345        let elapsed = utc_tov
346            .signed_duration_since(*ref_date)
347            .num_nanoseconds()
348            .unwrap() as u64;
349
350        let cu_time = *ref_cu_time + CuTime::from(elapsed);
351        Ok(cu_time)
352    }
353}
354
355impl Debug for Tail {
356    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
357        writeln!(f, "Return Mode: {:?}", self.return_mode())?;
358        writeln!(
359            f,
360            "Motor Speed: {} rpm",
361            self.motor_speed().get::<revolution_per_minute>()
362        )?;
363        writeln!(f, "UTC Time: {:?}", self.utc_tov())?;
364        writeln!(f, "Factory Info: {:x}", self.factory_info)
365    }
366}
367
368#[inline(always)]
369fn u16_endianness(val: u16) -> u16 {
370    if cfg!(target_endian = "little") {
371        val
372    } else {
373        u16::from_le(val)
374    }
375}
376
377#[allow(dead_code)]
378#[inline(always)]
379fn u32_endianness(val: u32) -> u32 {
380    if cfg!(target_endian = "little") {
381        val
382    } else {
383        u32::from_le(val)
384    }
385}
386
387// Type to map the lidar timestamp to the robot monotonic time.
388pub type RefTime = (DateTime<Utc>, CuTime);
389
390#[repr(C, packed)]
391#[derive(Copy, Clone, Zeroable, Pod, Debug)]
392pub struct Packet {
393    pub pre_header: PreHeader,
394    pub header: Header,
395    pub blocks: [Block; 8],
396    pub tail: Tail,
397}
398
399const FIRING_OFFSET: CuDuration = CuDuration(5_632); // this is in ns
400const FIRING_DELAY: CuDuration = CuDuration(50_000);
401const DUAL_RETURN_OFFSETS: [i32; 8] = [3, 3, 2, 2, 1, 1, 0, 0];
402const SINGLE_RETURN_OFFSETS: [i32; 8] = [7, 6, 5, 4, 3, 2, 1, 0];
403
404impl Packet {
405    // ┌──────────────────────────────────────────────────────────────────────┐
406    // │ Start time of each block                                             │
407    // │ Given the absolute time of point cloud data packets as t₀, the start │
408    // │ time of each block (i.e., the time when the first firing starts) can │
409    // │ be calculated.                                                       │
410    // ├──────────────────────────────────────────────────────────────────────┤
411    // │ Single return mode                                                   │
412    // ├───────────┬──────────────────────────────────────────────────────────┤
413    // │  Block    │ Start time (µs)                                          │
414    // ├───────────┼──────────────────────────────────────────────────────────┤
415    // │  Block 8  │ t₀ + 5.632                                               │
416    // │  Block N  │ t₀ + 5.632 − 50 × (8 − N)                                │
417    // │  Block 3  │ t₀ + 5.632 − 50 × 5                                      │
418    // │  Block 2  │ t₀ + 5.632 − 50 × 6                                      │
419    // │  Block 1  │ t₀ + 5.632 − 50 × 7                                      │
420    // ├───────────┴──────────────────────────────────────────────────────────┤
421    // │ Dual return mode                                                     │
422    // ├─────────────┬────────────────────────────────────────────────────────┤
423    // │    Block    │ Start time (µs)                                        │
424    // ├─────────────┼────────────────────────────────────────────────────────┤
425    // │ Blocks 8&7  │ t₀ + 5.632                                             │
426    // │ Blocks 6&5  │ t₀ + 5.632 − 50 × 1                                    │
427    // │ Blocks 4&3  │ t₀ + 5.632 − 50 × 2                                    │
428    // │ Blocks 2&1  │ t₀ + 5.632 − 50 × 3                                    │
429    // └─────────────┴────────────────────────────────────────────────────────┘
430    pub fn block_ts(self, reftime: &RefTime) -> Result<[CuTime; 8], HesaiError> {
431        let t_zero = self.tail.tov(reftime)? + FIRING_OFFSET;
432        let offsets = if self.header.is_dual_return() {
433            DUAL_RETURN_OFFSETS
434        } else {
435            SINGLE_RETURN_OFFSETS
436        };
437        let result = offsets.map(|offset| t_zero - offset * FIRING_DELAY);
438        Ok(result)
439    }
440
441    pub fn check_invariants(self) -> Result<(), HesaiError> {
442        self.header.check_invariants()?;
443        self.blocks
444            .iter()
445            .try_for_each(|block| block.check_invariants())?;
446        Ok(())
447    }
448}
449
450pub fn parse_packet(data: &[u8]) -> Result<&Packet, HesaiError> {
451    if data[0] != 0xEE || data[1] != 0xFF {
452        return Err(HesaiError::InvalidPacket(format!(
453            "Not an Xt32 packet: {:2X}{:2X}",
454            data[0], data[1],
455        )));
456    }
457
458    if data.len() < size_of::<Packet>() {
459        return Err(HesaiError::InvalidPacket(format!(
460            "Packet too short: {} < {}",
461            data.len(),
462            size_of::<Packet>()
463        )));
464    }
465    if data.len() > size_of::<Packet>() {
466        return Err(HesaiError::InvalidPacket(format!(
467            "Packet too long: {} > {}",
468            data.len(),
469            size_of::<Packet>()
470        )));
471    }
472    let packet: &Packet = bytemuck::from_bytes(data);
473    packet.check_invariants()?;
474    Ok(packet)
475}
476
477/// Generate the default elevation calibration for the Xt32 Hesai sensor.
478/// The sensor has 32 channels, each with a different elevation angle.
479/// The elevation angles are in degrees and range from 15 to -16.
480pub fn generate_default_elevation_calibration() -> [Angle; 32] {
481    let mut elevations = [Angle::default(); 32];
482    elevations.iter_mut().enumerate().for_each(|(i, x)| {
483        *x = Angle::new::<degree>(15.0 - i as f32);
484    });
485    elevations
486}
487
488#[cfg(test)]
489mod tests {
490    use crate::parser::{Packet, RefTime, parse_packet};
491    use cu29::clock::CuDuration;
492    use cu29::prelude::RobotClock;
493
494    #[test]
495    fn test_packet() {
496        // Taken from a real world packet
497        let packet: [u8; 1122] = [
498            0xB4, 0x96, 0x91, 0x72, 0x1D, 0x12, 0xEC, 0x9F, 0x0D, 0x01, 0x00, 0x69, 0x08, 0x00,
499            0x45, 0x00, 0x04, 0x54, 0x57, 0xF2, 0x40, 0x00, 0x40, 0x11, 0xC6, 0xDF, 0x0A, 0xDE,
500            0x01, 0x0B, 0x0A, 0xDE, 0x01, 0x01, 0x27, 0x10, 0x09, 0x40, 0x04, 0x40, 0x1B, 0xAC,
501            0xEE, 0xFF, 0x06, 0x01, 0x00, 0x00, 0x20, 0x08, 0x01, 0x04, 0x02, 0x01, 0x8B, 0x77,
502            0xBB, 0x01, 0x0A, 0xFF, 0xC1, 0x01, 0x0C, 0xFF, 0xC8, 0x01, 0x0B, 0xFF, 0xCD, 0x01,
503            0x0B, 0xFF, 0xD5, 0x01, 0x0C, 0xFF, 0xDB, 0x01, 0x0C, 0xFF, 0xE3, 0x01, 0x0B, 0xFF,
504            0xEA, 0x01, 0x0B, 0xFF, 0xF2, 0x01, 0x0B, 0xFF, 0xFA, 0x01, 0x0A, 0xFF, 0x02, 0x02,
505            0x0A, 0xFF, 0x14, 0x02, 0x06, 0xFF, 0x42, 0x02, 0x06, 0xFF, 0x7C, 0x02, 0x07, 0xFF,
506            0x8B, 0x02, 0x08, 0xFF, 0x9F, 0x02, 0x0A, 0xFF, 0xAA, 0x02, 0x08, 0xFF, 0xBA, 0x02,
507            0x09, 0xFF, 0xCE, 0x02, 0x08, 0xFF, 0xDA, 0x02, 0x07, 0xFF, 0xEC, 0x02, 0x07, 0xFF,
508            0x01, 0x03, 0x0A, 0xFF, 0x1C, 0x03, 0x07, 0xFF, 0x2F, 0x03, 0x05, 0xFF, 0x47, 0x03,
509            0x07, 0xFF, 0x64, 0x03, 0x07, 0xFF, 0x80, 0x03, 0x07, 0xFF, 0xA4, 0x03, 0x08, 0xFF,
510            0xBE, 0x03, 0x0A, 0xFF, 0xE5, 0x03, 0x06, 0xFF, 0x15, 0x04, 0x0B, 0xFF, 0x3D, 0x04,
511            0x06, 0xFF, 0x8B, 0x77, 0xBB, 0x01, 0x0A, 0xFF, 0xC1, 0x01, 0x0C, 0xFF, 0xC8, 0x01,
512            0x0B, 0xFF, 0xCD, 0x01, 0x0B, 0xFF, 0xD5, 0x01, 0x0C, 0xFF, 0xDB, 0x01, 0x0C, 0xFF,
513            0xE3, 0x01, 0x0B, 0xFF, 0xEA, 0x01, 0x0B, 0xFF, 0xF2, 0x01, 0x0B, 0xFF, 0xFA, 0x01,
514            0x0A, 0xFF, 0x02, 0x02, 0x0A, 0xFF, 0x14, 0x02, 0x06, 0xFF, 0x42, 0x02, 0x06, 0xFF,
515            0x7C, 0x02, 0x07, 0xFF, 0x8B, 0x02, 0x08, 0xFF, 0x9F, 0x02, 0x0A, 0xFF, 0xAA, 0x02,
516            0x08, 0xFF, 0xBA, 0x02, 0x09, 0xFF, 0xCE, 0x02, 0x08, 0xFF, 0xDA, 0x02, 0x07, 0xFF,
517            0xEC, 0x02, 0x07, 0xFF, 0x01, 0x03, 0x0A, 0xFF, 0x1C, 0x03, 0x07, 0xFF, 0x2F, 0x03,
518            0x05, 0xFF, 0x47, 0x03, 0x07, 0xFF, 0x64, 0x03, 0x07, 0xFF, 0x80, 0x03, 0x07, 0xFF,
519            0xA4, 0x03, 0x08, 0xFF, 0xBE, 0x03, 0x0A, 0xFF, 0xE5, 0x03, 0x06, 0xFF, 0x15, 0x04,
520            0x0B, 0xFF, 0x3D, 0x04, 0x06, 0xFF, 0x9D, 0x77, 0xBB, 0x01, 0x09, 0xFF, 0xC2, 0x01,
521            0x0C, 0xFF, 0xC9, 0x01, 0x0C, 0xFF, 0xCF, 0x01, 0x0B, 0xFF, 0xD6, 0x01, 0x0C, 0xFF,
522            0xDB, 0x01, 0x0B, 0xFF, 0xE3, 0x01, 0x0B, 0xFF, 0xEB, 0x01, 0x0B, 0xFF, 0xF6, 0x01,
523            0x0B, 0xFF, 0xFC, 0x01, 0x0B, 0xFF, 0x02, 0x02, 0x08, 0xFF, 0x2C, 0x02, 0x06, 0xFF,
524            0x47, 0x02, 0x06, 0xFF, 0x81, 0x02, 0x08, 0xFF, 0x8D, 0x02, 0x09, 0xFF, 0xA1, 0x02,
525            0x0A, 0xFF, 0xAC, 0x02, 0x08, 0xFF, 0xBB, 0x02, 0x09, 0xFF, 0xD1, 0x02, 0x08, 0xFF,
526            0xDE, 0x02, 0x07, 0xFF, 0xF1, 0x02, 0x07, 0xFF, 0x03, 0x03, 0x09, 0xFF, 0x1F, 0x03,
527            0x07, 0xFF, 0x35, 0x03, 0x06, 0xFF, 0x4B, 0x03, 0x07, 0xFF, 0x68, 0x03, 0x07, 0xFF,
528            0x82, 0x03, 0x08, 0xFF, 0xA8, 0x03, 0x09, 0xFF, 0xC3, 0x03, 0x0A, 0xFF, 0xE9, 0x03,
529            0x06, 0xFF, 0x15, 0x04, 0x0B, 0xFF, 0x45, 0x04, 0x07, 0xFF, 0x9D, 0x77, 0xBB, 0x01,
530            0x09, 0xFF, 0xC2, 0x01, 0x0C, 0xFF, 0xC9, 0x01, 0x0C, 0xFF, 0xCF, 0x01, 0x0B, 0xFF,
531            0xD6, 0x01, 0x0C, 0xFF, 0xDB, 0x01, 0x0B, 0xFF, 0xE3, 0x01, 0x0B, 0xFF, 0xEB, 0x01,
532            0x0B, 0xFF, 0xF6, 0x01, 0x0B, 0xFF, 0xFC, 0x01, 0x0B, 0xFF, 0x02, 0x02, 0x08, 0xFF,
533            0x2C, 0x02, 0x06, 0xFF, 0x47, 0x02, 0x06, 0xFF, 0x81, 0x02, 0x08, 0xFF, 0x8D, 0x02,
534            0x09, 0xFF, 0xA1, 0x02, 0x0A, 0xFF, 0xAC, 0x02, 0x08, 0xFF, 0xBB, 0x02, 0x09, 0xFF,
535            0xD1, 0x02, 0x08, 0xFF, 0xDE, 0x02, 0x07, 0xFF, 0xF1, 0x02, 0x07, 0xFF, 0x03, 0x03,
536            0x09, 0xFF, 0x1F, 0x03, 0x07, 0xFF, 0x35, 0x03, 0x06, 0xFF, 0x4B, 0x03, 0x07, 0xFF,
537            0x68, 0x03, 0x07, 0xFF, 0x82, 0x03, 0x08, 0xFF, 0xA8, 0x03, 0x09, 0xFF, 0xC3, 0x03,
538            0x0A, 0xFF, 0xE9, 0x03, 0x06, 0xFF, 0x15, 0x04, 0x0B, 0xFF, 0x45, 0x04, 0x07, 0xFF,
539            0xAF, 0x77, 0xBE, 0x01, 0x0A, 0xFF, 0xC2, 0x01, 0x0C, 0xFF, 0xC9, 0x01, 0x0B, 0xFF,
540            0xD0, 0x01, 0x0B, 0xFF, 0xD5, 0x01, 0x0C, 0xFF, 0xDD, 0x01, 0x0C, 0xFF, 0xE4, 0x01,
541            0x0B, 0xFF, 0xEE, 0x01, 0x0B, 0xFF, 0xF5, 0x01, 0x0B, 0xFF, 0xFC, 0x01, 0x0B, 0xFF,
542            0x04, 0x02, 0x07, 0xFF, 0x32, 0x02, 0x05, 0xFF, 0x64, 0x02, 0x06, 0xFF, 0x86, 0x02,
543            0x08, 0xFF, 0x8F, 0x02, 0x08, 0xFF, 0xA2, 0x02, 0x09, 0xFF, 0xAE, 0x02, 0x09, 0xFF,
544            0xBD, 0x02, 0x08, 0xFF, 0xD1, 0x02, 0x08, 0xFF, 0xDF, 0x02, 0x07, 0xFF, 0xF3, 0x02,
545            0x07, 0xFF, 0x06, 0x03, 0x09, 0xFF, 0x21, 0x03, 0x07, 0xFF, 0x3A, 0x03, 0x05, 0xFF,
546            0x4F, 0x03, 0x07, 0xFF, 0x6B, 0x03, 0x08, 0xFF, 0x8A, 0x03, 0x07, 0xFF, 0xAB, 0x03,
547            0x09, 0xFF, 0xC6, 0x03, 0x08, 0xFF, 0xED, 0x03, 0x07, 0xFF, 0x1C, 0x04, 0x0B, 0xFF,
548            0x4C, 0x04, 0x07, 0xFF, 0xAF, 0x77, 0xBE, 0x01, 0x0A, 0xFF, 0xC2, 0x01, 0x0C, 0xFF,
549            0xC9, 0x01, 0x0B, 0xFF, 0xD0, 0x01, 0x0B, 0xFF, 0xD5, 0x01, 0x0C, 0xFF, 0xDD, 0x01,
550            0x0C, 0xFF, 0xE4, 0x01, 0x0B, 0xFF, 0xEE, 0x01, 0x0B, 0xFF, 0xF5, 0x01, 0x0B, 0xFF,
551            0xFC, 0x01, 0x0B, 0xFF, 0x04, 0x02, 0x07, 0xFF, 0x32, 0x02, 0x05, 0xFF, 0x64, 0x02,
552            0x06, 0xFF, 0x86, 0x02, 0x08, 0xFF, 0x8F, 0x02, 0x08, 0xFF, 0xA2, 0x02, 0x09, 0xFF,
553            0xAE, 0x02, 0x09, 0xFF, 0xBD, 0x02, 0x08, 0xFF, 0xD1, 0x02, 0x08, 0xFF, 0xDF, 0x02,
554            0x07, 0xFF, 0xF3, 0x02, 0x07, 0xFF, 0x06, 0x03, 0x09, 0xFF, 0x21, 0x03, 0x07, 0xFF,
555            0x3A, 0x03, 0x05, 0xFF, 0x4F, 0x03, 0x07, 0xFF, 0x6B, 0x03, 0x08, 0xFF, 0x8A, 0x03,
556            0x07, 0xFF, 0xAB, 0x03, 0x09, 0xFF, 0xC6, 0x03, 0x08, 0xFF, 0xED, 0x03, 0x07, 0xFF,
557            0x1C, 0x04, 0x0B, 0xFF, 0x4C, 0x04, 0x07, 0xFF, 0xC0, 0x77, 0xBC, 0x01, 0x0B, 0xFF,
558            0xC3, 0x01, 0x0C, 0xFF, 0xCA, 0x01, 0x0B, 0xFF, 0xD0, 0x01, 0x0B, 0xFF, 0xD7, 0x01,
559            0x0C, 0xFF, 0xDE, 0x01, 0x0B, 0xFF, 0xE6, 0x01, 0x0B, 0xFF, 0xEE, 0x01, 0x0B, 0xFF,
560            0xF6, 0x01, 0x0B, 0xFF, 0xFE, 0x01, 0x0B, 0xFF, 0x0F, 0x02, 0x07, 0xFF, 0x37, 0x02,
561            0x06, 0xFF, 0x72, 0x02, 0x08, 0xFF, 0x89, 0x02, 0x08, 0xFF, 0x91, 0x02, 0x09, 0xFF,
562            0xA4, 0x02, 0x09, 0xFF, 0xB0, 0x02, 0x09, 0xFF, 0xBF, 0x02, 0x08, 0xFF, 0xD4, 0x02,
563            0x08, 0xFF, 0xE3, 0x02, 0x07, 0xFF, 0xF5, 0x02, 0x07, 0xFF, 0x08, 0x03, 0x09, 0xFF,
564            0x23, 0x03, 0x08, 0xFF, 0x3D, 0x03, 0x06, 0xFF, 0x55, 0x03, 0x08, 0xFF, 0x6F, 0x03,
565            0x07, 0xFF, 0x8D, 0x03, 0x07, 0xFF, 0xAE, 0x03, 0x0A, 0xFF, 0xC9, 0x03, 0x06, 0xFF,
566            0xF2, 0x03, 0x06, 0xFF, 0x1D, 0x04, 0x0B, 0xFF, 0x4F, 0x04, 0x08, 0xFF, 0xC0, 0x77,
567            0xBC, 0x01, 0x0B, 0xFF, 0xC3, 0x01, 0x0C, 0xFF, 0xCA, 0x01, 0x0B, 0xFF, 0xD0, 0x01,
568            0x0B, 0xFF, 0xD7, 0x01, 0x0C, 0xFF, 0xDE, 0x01, 0x0B, 0xFF, 0xE6, 0x01, 0x0B, 0xFF,
569            0xEE, 0x01, 0x0B, 0xFF, 0xF6, 0x01, 0x0B, 0xFF, 0xFE, 0x01, 0x0B, 0xFF, 0x0F, 0x02,
570            0x07, 0xFF, 0x37, 0x02, 0x06, 0xFF, 0x72, 0x02, 0x08, 0xFF, 0x89, 0x02, 0x08, 0xFF,
571            0x91, 0x02, 0x09, 0xFF, 0xA4, 0x02, 0x09, 0xFF, 0xB0, 0x02, 0x09, 0xFF, 0xBF, 0x02,
572            0x08, 0xFF, 0xD4, 0x02, 0x08, 0xFF, 0xE3, 0x02, 0x07, 0xFF, 0xF5, 0x02, 0x07, 0xFF,
573            0x08, 0x03, 0x09, 0xFF, 0x23, 0x03, 0x08, 0xFF, 0x3D, 0x03, 0x06, 0xFF, 0x55, 0x03,
574            0x08, 0xFF, 0x6F, 0x03, 0x07, 0xFF, 0x8D, 0x03, 0x07, 0xFF, 0xAE, 0x03, 0x0A, 0xFF,
575            0xC9, 0x03, 0x06, 0xFF, 0xF2, 0x03, 0x06, 0xFF, 0x1D, 0x04, 0x0B, 0xFF, 0x4F, 0x04,
576            0x08, 0xFF, 0x00, 0x00, 0x86, 0xF4, 0x07, 0x83, 0x07, 0x00, 0x01, 0x00, 0x3C, 0x58,
577            0x02, 0x7C, 0x09, 0x11, 0x0F, 0x2F, 0x0C, 0x37, 0x73, 0x0A, 0x00, 0x42, 0x96, 0x13,
578            0x85, 0x01,
579        ];
580
581        let (robot_clock, mock) = RobotClock::mock();
582        // push the time by 1s because the first emulated test packet could end up in negative time.
583        mock.increment(CuDuration::from_secs(1));
584
585        let udp_header_size = 0x2A;
586        if packet.len() < udp_header_size + size_of::<Packet>() {
587            panic!("Packet too short: {}", packet.len());
588        }
589        let packet_data = &packet[udp_header_size..udp_header_size + size_of::<Packet>()];
590        let packet = parse_packet(packet_data).unwrap();
591
592        let rt: RefTime = (
593            packet.tail.utc_tov().unwrap(), // emulates a packet coming in recently
594            robot_clock.now(),
595        );
596        for (bid, ts) in packet.block_ts(&rt).unwrap().iter().enumerate() {
597            println!("Block {bid} tov: {ts}");
598        }
599    }
600}
cu_hesai/parser.rs

cu_hesai/
parser.rs
