use bytemuck::{Pod, Zeroable};
use chrono::{DateTime, Utc};
use cu29::prelude::{CuDuration, CuTime};
use cu29::units::si::f32::{Length, Ratio};
use cu29::units::si::length::millimeter;
use cu29::units::si::ratio::ratio;
use std::error::Error;
use std::fmt;
use std::fmt::{Debug, Formatter};
use std::mem::size_of;
#[inline(always)]
fn u16_endianness(val: u16) -> u16 {
if cfg!(target_endian = "little") {
val
} else {
u16::from_le(val)
}
}
#[inline(always)]
fn u32_endianness(val: u32) -> u32 {
if cfg!(target_endian = "little") {
val
} else {
u32::from_le(val)
}
}
#[inline(always)]
fn i32_endianness(val: i32) -> i32 {
if cfg!(target_endian = "little") {
val
} else {
i32::from_le(val)
}
}
#[inline(always)]
fn u64_endianness(val: u64) -> u64 {
if cfg!(target_endian = "little") {
val
} else {
u64::from_le(val)
}
}
//Tele15
//https://github.com/Livox-SDK/Livox-SDK/wiki/Livox-SDK-Communication-Protocol
// HAP
//https://github.com/Livox-SDK/Livox-SDK2/wiki/Livox-SDK-Communication-Protocol-HAP%28English%29
#[derive(Debug)]
pub enum LivoxError {
InvalidFrame(String),
InvalidTimestamp(String),
}
#[repr(C, packed)]
#[derive(Copy, Clone, Zeroable, Pod)]
pub struct CommandHeader {
sof: u8, // Start of frame
version: u8, // Protocol version
length: u16, // frame length
cmd_type: u8, // command type
seq_num: u8, // Frame Sequence Number |
crc_16: u16, // Frame Header Checksum
}
impl Debug for CommandHeader {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
f.write_fmt(format_args!("Magic: {:2X}{:2X}", self.sof, self.version))?;
f.write_fmt(format_args!(
"Frame: length {}, cmd_type{:2X}",
u16_endianness(self.length),
self.cmd_type
))
}
}
// Command frame format
//
// The Protocol Frame is the smallest unit for control command. Format is as
// follows:
//
// | Field | Index (byte) | Size (byte) | Description |
// | -------- | ------------ | ----------- | ---------------------------------------- |
// | sof | 0 | 1 | Starting Byte, Fixed to be 0xAA |
// | version | 1 | 1 | Protocol Version, 1 for The Current Version |
// | length | 2 | 2 | Length of The Frame, Max Value: 1400 |
// | cmd_type | 4 | 1 | Command Type: <br>0x00: CMD <br>0x01: ACK <br>0x02: MSG |
// | seq_num | 5 | 2 | Frame Sequence Number |
// | crc_16 | 7 | 2 | Frame Header Checksum |
// | data | 9 | n | Payload Data |
// | crc_32 | 9+n | 4 | Whole Frame Checksum |
//
// Command type description:
// > - **CMD (request)**: Actively send data request - need to return a corresponding ACK;
// > - **ACK (response)**: Response to CMD data;
// > - **MSG (message)**: Actively pushed message data - no need to return response data,
//
// > e.g. broadcast data, pushed message when an error occurs;
#[repr(C, packed)]
#[derive(Copy, Clone, Zeroable, Pod, Debug)]
pub struct CommandFrame {
pub header: CommandHeader,
pub date: u8,
pub crc_32: u32,
}
// LiDAR Status Code
//
// LiDAR status_code consists of 32 bits, which has the following meanings:
//
// | Bits | Data | Description |
// | -------- | ---------------- | ------------------------------------------------------------ |
// | Bit0:1 | temp_status | 0: Temperature in Normal State <br>1: High or Low <br>2: Extremely High or Extremely Low |
// | Bit2:3 | volt_status | Voltage Status of Internal Module<br> 0: Voltage in Normal State <br>1: High <br>2: Extremely High |
// | Bit4:5 | motor_status | 0: Motor in Normal State <br>1: Motor in Warning State <br>2: Motor in Error State, Unable to Work |
// | Bit6:7 | dirty_warn | 0: Not Dirty or Blocked <br>1: Dirty or Blocked |
// | Bit8 | firmware_status | 0: Firmware is OK <br>1: Firmware is Abnormal, Need to be Upgraded |
// | Bit9 | pps_status | 0: No PPS Signal <br>1: PPS Signal is OK |
// | Bit10 | device_status | 0: Normal <br>1: Warning for Approaching the End of Service Life |
// | 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+ |
// | 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> |
// | Bit13 | ptp_status | 0: No 1588 Signal <br>1: 1588 Signal is OK |
// | 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) |
// | Bit17:29 | RSVD | |
// | 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; |
// Timestamp Type:
//
// | Timestamp Type | Source | Data Type | Description |
// | -------------- | -------------- | --------- | --------------------------------- |
// | 0 | No sync source | uint64_t | Unit: ns |
// | 1 | PTP | uint64_t | Unit: ns |
// | 2 | Reserved | | |
// | 3 | GPS | UTC | UTC |
// | 4 | PPS | int64_t | Unit: ns, only supported by LiDAR |
#[repr(C, packed)]
#[derive(Copy, Clone, Zeroable, Pod)]
pub struct LidarHeader {
pub version: u8,
pub slot_id: u8,
pub lidar_id: u8,
reserved: u8,
pub status_code: u32,
pub timestamp_type: u8,
pub data_type: u8,
pub timestamp: u64,
}
impl Debug for crate::parser::LidarHeader {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
f.write_fmt(format_args!(
"Version: {:2X} slot_id: {:2X} lidar_id: {:2X} ",
self.version, self.slot_id, self.lidar_id
))?;
f.write_fmt(format_args!(
"Status: {}, ts_type{:2X}, data_type{:2X}, timestamp{}",
u32_endianness(self.status_code),
self.timestamp_type,
self.data_type,
u64_endianness(self.timestamp)
))
}
}
impl LidarHeader {
pub fn timestamp(&self) -> CuDuration {
CuDuration(u64_endianness(self.timestamp))
}
}
#[repr(C, packed)]
#[derive(Copy, Clone, Zeroable, Pod)]
pub struct PointType2 {
x: i32,
y: i32,
z: i32,
pub reflectivity: u8,
pub tag: u8,
}
impl PointType2 {
pub fn x(&self) -> Length {
Length::new::<millimeter>(i32_endianness(self.x) as f32)
}
pub fn y(&self) -> Length {
Length::new::<millimeter>(i32_endianness(self.y) as f32)
}
pub fn z(&self) -> Length {
Length::new::<millimeter>(i32_endianness(self.z) as f32)
}
pub fn reflectivity(&self) -> Ratio {
Ratio::new::<ratio>(self.reflectivity as f32 / 255.0)
}
pub fn check_invariants(&self) -> Result<(), LivoxError> {
// TODO: determine the valid range for x,y,z
Ok(())
}
}
impl Debug for crate::parser::PointType2 {
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
f.write_fmt(format_args!(
"Point: ({}, {}, {}) reflectivity {:2X} tag {:2X}",
i32_endianness(self.x),
i32_endianness(self.y),
i32_endianness(self.z),
self.reflectivity,
self.tag
))
}
}
// **Data Type 2**
//
// Single return cartesian coordinate data format:
//
// | Field | Offset (byte) | Data Type | Description |
// | ------------ | ------------- | --------- | ---------------------------------------- |
// | x | 0 | int32_t | X axis, Unit: mm |
// | y | 4 | int32_t | Y axis, Unit: mm |
// | z | 8 | int32_t | Z axis, Unit: mm |
// | reflectivity | 12 | uint8_t | Reflectivity |
// | tag | 13 | uint8_t | For details, see [tag information](#3.4 Tag Information {#tag_info) |
//
// 3.4 Tag Information {#tag_info}
//
// | Bit | Description |
// | ---- | ---------------------------------------- |
// | 7:6 | Reserved |
// | 5:4 | Return Number: <br>00: Return 0<br>01: Return 1<br>02: Return 2<br>03: Return 4 |
// | 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 |
// | 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 |
//
// Lidar frame format
// | Field | Index (byte) | Size (byte) | Description |
// | -------------- | ------------ | ----------- | ---------------------------------------- |
// | version | 0 | 1 | Packet Protocol Version, 5 for the Current Version |
// | 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); |
// | 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> |
// | reserved | 3 | 1 | |
// | status_code | 4 | 4 | LiDAR Status Indicator Information, For details, see 3.4 |
// | timestamp_type | 8 | 1 | Timestamp Type, For details, see [3.2](#3.2 Time Stamp {#timestamp}) |
// | data_type | 9 | 1 | Data Type, For details, see [3.3](#3.3 Point Cloud/IMU Data {#data_type}) |
// | timestamp | 10 | 8 | Nanosecond or UTC Format Timestamp, For details, see 3.2 |
// | data | 18 | -- | Data information, For details, see [3.3](#3.3 Point Cloud/IMU Data {#data_type}) |
const MAX_POINTS_TYPE2: usize = 96;
pub const DATA_FRAME_TYPE2_SIZE: usize = size_of::<LidarFrame>();
#[repr(C, packed)]
#[derive(Copy, Clone, Zeroable, Pod, Debug)]
pub struct LidarFrame {
pub header: LidarHeader,
pub points: [PointType2; MAX_POINTS_TYPE2],
}
impl fmt::Display for LivoxError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
LivoxError::InvalidFrame(msg) => write!(f, "Invalid frame: {msg}"),
LivoxError::InvalidTimestamp(msg) => write!(f, "Invalid timestamp: {msg}"),
}
}
}
impl Error for LivoxError {}
pub type RefTime = (DateTime<Utc>, CuTime);
pub fn parse_frame(data: &[u8]) -> Result<&LidarFrame, LivoxError> {
if data[0] != 0x05
// Protocol version
{
return Err(LivoxError::InvalidFrame(format!(
"Not a Livox SDK protocol V1 frame: {:2X}",
data[0],
)));
}
if data.len() < DATA_FRAME_TYPE2_SIZE {
return Err(LivoxError::InvalidFrame(format!(
"Frame too short: {} < {}",
data.len(),
DATA_FRAME_TYPE2_SIZE
)));
}
if data.len() > DATA_FRAME_TYPE2_SIZE {
return Err(LivoxError::InvalidFrame(format!(
"Frame too long: {} > {}",
data.len(),
DATA_FRAME_TYPE2_SIZE
)));
}
let packet: &LidarFrame = bytemuck::from_bytes(&data[..DATA_FRAME_TYPE2_SIZE]);
if packet.header.data_type != 0x02 {
return Err(LivoxError::InvalidFrame(format!(
"Point Cloud Data Type 2 expected, received : {:2X}",
packet.header.data_type
)));
}
Ok(packet)
}
#[cfg(test)]
mod tests {
use crate::parser::{LidarFrame, RefTime, parse_frame};
use chrono::prelude::*;
use cu29::prelude::{CuDuration, RobotClock};
#[test]
fn test_tele15_packet() {
let (robot_clock, mock) = RobotClock::mock();
// push the time by 1s because the first emulated test packet could end up in negative time.
mock.increment(CuDuration::from_secs(1));
let packet_data: [u8; 1362] = [
0x05, 0x01, 0x01, 0x00, 0x40, 0x68, 0x00, 0x40, 0x01, 0x02, 0x8B, 0x06, 0xAE, 0xE5,
0xB4, 0x12, 0xF6, 0x17, 0xF6, 0x5C, 0x00, 0x00, 0x08, 0x08, 0x00, 0x00, 0xAB, 0x05,
0x00, 0x00, 0x0F, 0x10, 0xFE, 0x5C, 0x00, 0x00, 0x0E, 0x08, 0x00, 0x00, 0x27, 0x05,
0x00, 0x00, 0x0E, 0x10, 0xE8, 0x5C, 0x00, 0x00, 0x11, 0x08, 0x00, 0x00, 0xA0, 0x04,
0x00, 0x00, 0x24, 0x10, 0x17, 0x5D, 0x00, 0x00, 0x1A, 0x08, 0x00, 0x00, 0x1B, 0x04,
0x00, 0x00, 0x0C, 0x10, 0xBD, 0x59, 0x00, 0x00, 0xD4, 0x07, 0x00, 0x00, 0x74, 0x03,
0x00, 0x00, 0x16, 0x10, 0xDD, 0x58, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0xED, 0x02,
0x00, 0x00, 0x09, 0x10, 0xE3, 0x5C, 0x00, 0x00, 0x2D, 0x08, 0x00, 0x00, 0xA7, 0x05,
0x00, 0x00, 0x0A, 0x10, 0xDE, 0x5C, 0x00, 0x00, 0x31, 0x08, 0x00, 0x00, 0x22, 0x05,
0x00, 0x00, 0x09, 0x10, 0xE0, 0x5C, 0x00, 0x00, 0x37, 0x08, 0x00, 0x00, 0x9D, 0x04,
0x00, 0x00, 0x1D, 0x10, 0x32, 0x5C, 0x00, 0x00, 0x2C, 0x08, 0x00, 0x00, 0x0D, 0x04,
0x00, 0x00, 0x07, 0x10, 0x27, 0x59, 0x00, 0x00, 0xEC, 0x07, 0x00, 0x00, 0x6B, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x67, 0x54, 0x00, 0x00, 0x84, 0x07, 0x00, 0x00, 0xC4, 0x02,
0x00, 0x00, 0x1A, 0x10, 0xD5, 0x5C, 0x00, 0x00, 0x52, 0x08, 0x00, 0x00, 0xA3, 0x05,
0x00, 0x00, 0x0F, 0x10, 0xFA, 0x5C, 0x00, 0x00, 0x5A, 0x08, 0x00, 0x00, 0x1F, 0x05,
0x00, 0x00, 0x0B, 0x10, 0xAD, 0x5B, 0x00, 0x00, 0x41, 0x08, 0x00, 0x00, 0x89, 0x04,
0x00, 0x00, 0x04, 0x10, 0x0D, 0x5C, 0x00, 0x00, 0x4E, 0x08, 0x00, 0x00, 0x07, 0x04,
0x00, 0x00, 0x07, 0x10, 0xDD, 0x59, 0x00, 0x00, 0x21, 0x08, 0x00, 0x00, 0x6E, 0x03,
0x00, 0x00, 0x13, 0x10, 0x55, 0x54, 0x00, 0x00, 0xA5, 0x07, 0x00, 0x00, 0xBF, 0x02,
0x00, 0x00, 0x18, 0x10, 0xC4, 0x5B, 0x00, 0x00, 0x5E, 0x08, 0x00, 0x00, 0x8D, 0x05,
0x00, 0x00, 0x64, 0x10, 0xC1, 0x5A, 0x00, 0x00, 0x4B, 0x08, 0x00, 0x00, 0xFB, 0x04,
0x00, 0x00, 0x04, 0x10, 0xA4, 0x5B, 0x00, 0x00, 0x65, 0x08, 0x00, 0x00, 0x84, 0x04,
0x00, 0x00, 0x10, 0x10, 0x2C, 0x5C, 0x00, 0x00, 0x76, 0x08, 0x00, 0x00, 0x04, 0x04,
0x00, 0x00, 0x14, 0x10, 0xAD, 0x56, 0x00, 0x00, 0xF9, 0x07, 0x00, 0x00, 0x4A, 0x03,
0x00, 0x00, 0x04, 0x10, 0x37, 0x54, 0x00, 0x00, 0xC4, 0x07, 0x00, 0x00, 0xB9, 0x02,
0x00, 0x00, 0x18, 0x10, 0xE4, 0x59, 0x00, 0x00, 0x56, 0x08, 0x00, 0x00, 0x6B, 0x05,
0x00, 0x00, 0x04, 0x10, 0xA4, 0x5B, 0x00, 0x00, 0x84, 0x08, 0x00, 0x00, 0x02, 0x05,
0x00, 0x00, 0x0C, 0x10, 0xB4, 0x5B, 0x00, 0x00, 0x8A, 0x08, 0x00, 0x00, 0x7F, 0x04,
0x00, 0x00, 0x13, 0x10, 0xC8, 0x5B, 0x00, 0x00, 0x91, 0x08, 0x00, 0x00, 0xF9, 0x03,
0x00, 0x00, 0x0D, 0x10, 0xF6, 0x58, 0x00, 0x00, 0x52, 0x08, 0x00, 0x00, 0x5A, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x6E, 0x52, 0x00, 0x00, 0xBA, 0x07, 0x00, 0x00, 0xA5, 0x02,
0x00, 0x00, 0x13, 0x10, 0x00, 0x5B, 0x00, 0x00, 0x95, 0x08, 0x00, 0x00, 0x76, 0x05,
0x00, 0x00, 0x0B, 0x10, 0x21, 0x5B, 0x00, 0x00, 0x9C, 0x08, 0x00, 0x00, 0xF4, 0x04,
0x00, 0x00, 0x11, 0x10, 0xAB, 0x5A, 0x00, 0x00, 0x95, 0x08, 0x00, 0x00, 0x6B, 0x04,
0x00, 0x00, 0x12, 0x10, 0xC1, 0x5A, 0x00, 0x00, 0x9B, 0x08, 0x00, 0x00, 0xE7, 0x03,
0x00, 0x00, 0x0B, 0x10, 0x14, 0x58, 0x00, 0x00, 0x5F, 0x08, 0x00, 0x00, 0x4B, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x92, 0x52, 0x00, 0x00, 0xDD, 0x07, 0x00, 0x00, 0xA0, 0x02,
0x00, 0x00, 0x10, 0x10, 0x4A, 0x5A, 0x00, 0x00, 0xA6, 0x08, 0x00, 0x00, 0x64, 0x05,
0x00, 0x00, 0x0E, 0x10, 0x10, 0x5A, 0x00, 0x00, 0xA4, 0x08, 0x00, 0x00, 0xDE, 0x04,
0x00, 0x00, 0x12, 0x10, 0xAE, 0x59, 0x00, 0x00, 0x9F, 0x08, 0x00, 0x00, 0x57, 0x04,
0x00, 0x00, 0x13, 0x10, 0x92, 0x59, 0x00, 0x00, 0xA1, 0x08, 0x00, 0x00, 0xD3, 0x03,
0x00, 0x00, 0x0A, 0x10, 0x13, 0x57, 0x00, 0x00, 0x68, 0x08, 0x00, 0x00, 0x3A, 0x03,
0x00, 0x00, 0x0D, 0x10, 0x4B, 0x4F, 0x00, 0x00, 0xAB, 0x07, 0x00, 0x00, 0x7E, 0x02,
0x00, 0x00, 0x11, 0x10, 0x51, 0x59, 0x00, 0x00, 0xB0, 0x08, 0x00, 0x00, 0x4D, 0x05,
0x00, 0x00, 0x0D, 0x10, 0x14, 0x59, 0x00, 0x00, 0xAE, 0x08, 0x00, 0x00, 0xC8, 0x04,
0x00, 0x00, 0x12, 0x10, 0xBE, 0x58, 0x00, 0x00, 0xAA, 0x08, 0x00, 0x00, 0x43, 0x04,
0x00, 0x00, 0x12, 0x10, 0x89, 0x58, 0x00, 0x00, 0xA9, 0x08, 0x00, 0x00, 0xBF, 0x03,
0x00, 0x00, 0x0B, 0x10, 0x3C, 0x56, 0x00, 0x00, 0x74, 0x08, 0x00, 0x00, 0x2A, 0x03,
0x00, 0x00, 0x0E, 0x10, 0xB2, 0x4E, 0x00, 0x00, 0xBA, 0x07, 0x00, 0x00, 0x71, 0x02,
0x00, 0x00, 0x08, 0x10, 0x5E, 0x58, 0x00, 0x00, 0xBA, 0x08, 0x00, 0x00, 0x35, 0x05,
0x00, 0x00, 0x0C, 0x10, 0x21, 0x58, 0x00, 0x00, 0xB8, 0x08, 0x00, 0x00, 0xB2, 0x04,
0x00, 0x00, 0x12, 0x10, 0xD0, 0x57, 0x00, 0x00, 0xB3, 0x08, 0x00, 0x00, 0x2F, 0x04,
0x00, 0x00, 0x12, 0x10, 0x89, 0x57, 0x00, 0x00, 0xB0, 0x08, 0x00, 0x00, 0xAB, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x34, 0x55, 0x00, 0x00, 0x79, 0x08, 0x00, 0x00, 0x17, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x06, 0x4E, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0x64, 0x02,
0x00, 0x00, 0x11, 0x10, 0x6C, 0x57, 0x00, 0x00, 0xC2, 0x08, 0x00, 0x00, 0x1E, 0x05,
0x00, 0x00, 0x0D, 0x10, 0x35, 0x57, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x9C, 0x04,
0x00, 0x00, 0x12, 0x10, 0xEB, 0x56, 0x00, 0x00, 0xBC, 0x08, 0x00, 0x00, 0x1B, 0x04,
0x00, 0x00, 0x12, 0x10, 0xB3, 0x56, 0x00, 0x00, 0xBA, 0x08, 0x00, 0x00, 0x98, 0x03,
0x00, 0x00, 0x0D, 0x10, 0x27, 0x54, 0x00, 0x00, 0x7C, 0x08, 0x00, 0x00, 0x04, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x64, 0x4D, 0x00, 0x00, 0xD1, 0x07, 0x00, 0x00, 0x56, 0x02,
0x00, 0x00, 0x16, 0x10, 0x81, 0x56, 0x00, 0x00, 0xC9, 0x08, 0x00, 0x00, 0x06, 0x05,
0x00, 0x00, 0x0E, 0x10, 0x58, 0x56, 0x00, 0x00, 0xC8, 0x08, 0x00, 0x00, 0x87, 0x04,
0x00, 0x00, 0x12, 0x10, 0x18, 0x56, 0x00, 0x00, 0xC5, 0x08, 0x00, 0x00, 0x07, 0x04,
0x00, 0x00, 0x10, 0x10, 0xF6, 0x55, 0x00, 0x00, 0xC5, 0x08, 0x00, 0x00, 0x86, 0x03,
0x00, 0x00, 0x0A, 0x10, 0x81, 0x4A, 0x00, 0x00, 0x9D, 0x07, 0x00, 0x00, 0xA2, 0x02,
0x00, 0x00, 0x00, 0x10, 0x64, 0x4A, 0x00, 0x00, 0x9D, 0x07, 0x00, 0x00, 0x36, 0x02,
0x00, 0x00, 0x25, 0x10, 0xA5, 0x55, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0xEF, 0x04,
0x00, 0x00, 0x0E, 0x10, 0x80, 0x55, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x71, 0x04,
0x00, 0x00, 0x11, 0x10, 0x4A, 0x55, 0x00, 0x00, 0xCE, 0x08, 0x00, 0x00, 0xF2, 0x03,
0x00, 0x00, 0x0E, 0x10, 0x36, 0x55, 0x00, 0x00, 0xCF, 0x08, 0x00, 0x00, 0x74, 0x03,
0x00, 0x00, 0x09, 0x10, 0x8F, 0x4A, 0x00, 0x00, 0xB8, 0x07, 0x00, 0x00, 0x99, 0x02,
0x00, 0x00, 0x03, 0x10, 0x10, 0x4A, 0x00, 0x00, 0xAE, 0x07, 0x00, 0x00, 0x2A, 0x02,
0x00, 0x00, 0x23, 0x10, 0xB1, 0x54, 0x00, 0x00, 0xD4, 0x08, 0x00, 0x00, 0xD5, 0x04,
0x00, 0x00, 0x0E, 0x10, 0xA7, 0x54, 0x00, 0x00, 0xD6, 0x08, 0x00, 0x00, 0x5A, 0x04,
0x00, 0x00, 0x0E, 0x10, 0x91, 0x54, 0x00, 0x00, 0xD7, 0x08, 0x00, 0x00, 0xDE, 0x03,
0x00, 0x00, 0x0B, 0x10, 0x7E, 0x54, 0x00, 0x00, 0xD8, 0x08, 0x00, 0x00, 0x61, 0x03,
0x00, 0x00, 0x09, 0x10, 0x8F, 0x4A, 0x00, 0x00, 0xD1, 0x07, 0x00, 0x00, 0x8F, 0x02,
0x00, 0x00, 0x07, 0x10, 0x6E, 0x49, 0x00, 0x00, 0xB6, 0x07, 0x00, 0x00, 0x1B, 0x02,
0x00, 0x00, 0x1D, 0x10, 0xDD, 0x53, 0x00, 0x00, 0xDA, 0x08, 0x00, 0x00, 0xBD, 0x04,
0x00, 0x00, 0x0E, 0x10, 0xD9, 0x53, 0x00, 0x00, 0xDD, 0x08, 0x00, 0x00, 0x43, 0x04,
0x00, 0x00, 0x0B, 0x10, 0xC9, 0x53, 0x00, 0x00, 0xDE, 0x08, 0x00, 0x00, 0xC9, 0x03,
0x00, 0x00, 0x0B, 0x10, 0xDE, 0x53, 0x00, 0x00, 0xE4, 0x08, 0x00, 0x00, 0x4E, 0x03,
0x00, 0x00, 0x08, 0x10, 0x8D, 0x4B, 0x00, 0x00, 0x05, 0x08, 0x00, 0x00, 0x8C, 0x02,
0x00, 0x00, 0x09, 0x10, 0x1E, 0x49, 0x00, 0x00, 0xC6, 0x07, 0x00, 0x00, 0x0E, 0x02,
0x00, 0x00, 0x14, 0x10, 0x24, 0x53, 0x00, 0x00, 0xE1, 0x08, 0x00, 0x00, 0xA6, 0x04,
0x00, 0x00, 0x0E, 0x10, 0x21, 0x53, 0x00, 0x00, 0xE4, 0x08, 0x00, 0x00, 0x2D, 0x04,
0x00, 0x00, 0x0A, 0x10, 0x16, 0x53, 0x00, 0x00, 0xE6, 0x08, 0x00, 0x00, 0xB4, 0x03,
0x00, 0x00, 0x0B, 0x10, 0x32, 0x53, 0x00, 0x00, 0xEC, 0x08, 0x00, 0x00, 0x3B, 0x03,
0x00, 0x00, 0x08, 0x10, 0x1E, 0x4B, 0x00, 0x00, 0x11, 0x08, 0x00, 0x00, 0x7D, 0x02,
0x00, 0x00, 0x11, 0x10, 0x81, 0x48, 0x00, 0x00, 0xCC, 0x07, 0x00, 0x00, 0xFE, 0x01,
0x00, 0x00, 0x16, 0x10, 0x7E, 0x52, 0x00, 0x00, 0xEA, 0x08, 0x00, 0x00, 0x90, 0x04,
0x00, 0x00, 0x0B, 0x10, 0x6F, 0x52, 0x00, 0x00, 0xEB, 0x08, 0x00, 0x00, 0x17, 0x04,
0x00, 0x00, 0x0B, 0x10, 0x67, 0x52, 0x00, 0x00, 0xED, 0x08, 0x00, 0x00, 0x9F, 0x03,
0x00, 0x00, 0x0A, 0x10, 0x8C, 0x52, 0x00, 0x00, 0xF4, 0x08, 0x00, 0x00, 0x27, 0x03,
0x00, 0x00, 0x08, 0x10, 0xA9, 0x4A, 0x00, 0x00, 0x1C, 0x08, 0x00, 0x00, 0x6D, 0x02,
0x00, 0x00, 0x11, 0x10, 0x85, 0x48, 0x00, 0x00, 0xE3, 0x07, 0x00, 0x00, 0xF2, 0x01,
0x00, 0x00, 0x15, 0x10,
];
if packet_data.len() < size_of::<LidarFrame>() {
panic!("Packet too short: {}", packet_data.len());
}
let packet = parse_frame(&packet_data).unwrap();
let datetime = Utc.with_ymd_and_hms(1970, 1, 1, 0, 0, 0).unwrap();
let _rt: RefTime = (datetime, robot_clock.now());
let timestamp = packet.header.timestamp;
println!("Tov: {timestamp}");
}
}