lidar_utils/velodyne/
packet.rs

1//! Provides `C-packed` structs for Velodyne data packets.
2
3use super::consts::{AZIMUTH_COUNT_PER_REV, BLOCKS_PER_PACKET, CHANNELS_PER_BLOCK};
4
5use crate::common::*;
6
7pub use data_packet::*;
8pub use position_packet::*;
9
10mod data_packet {
11    use super::*;
12
13    /// Represents the block index in range from 0 to 31, or from 32 to 63.
14    #[repr(u16)]
15    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
16    pub enum BlockIdentifier {
17        Block0To31 = 0xeeff,
18        Block32To63 = 0xddff,
19    }
20
21    /// Represents the way the sensor measures the laser signal.
22    #[repr(u8)]
23    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
24    pub enum ReturnMode {
25        StrongestReturn = 0x37,
26        LastReturn = 0x38,
27        DualReturn = 0x39,
28    }
29
30    /// Represents the hardware model.
31    #[repr(u8)]
32    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
33    pub enum ProductID {
34        HDL32E = 0x21,
35        VLP16 = 0x22,
36        PuckLite = 0x23,
37        PuckHiRes = 0x24,
38        VLP32C = 0x28,
39        Velarray = 0x31,
40        VLS128 = 0xa1,
41    }
42
43    /// Represents a point of measurement.
44    #[repr(C, packed)]
45    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
46    pub struct Channel {
47        /// The raw distance of laser return.
48        pub distance: u16,
49        /// The intensity of laser return.
50        pub intensity: u8,
51    }
52
53    /// Represents a sequence of measurements with meta data.
54    #[repr(C, packed)]
55    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
56    pub struct Block {
57        /// Represents the block that the firing belongs to.
58        pub block_identifier: BlockIdentifier,
59        /// Encoder count of rotation motor ranging from 0 to 36000 (inclusive).
60        pub azimuth_count: u16,
61        /// Array of channels.
62        pub channels: [Channel; CHANNELS_PER_BLOCK],
63    }
64
65    impl Block {
66        pub fn azimuth_angle_radian(&self) -> f64 {
67            2.0 * std::f64::consts::PI * self.azimuth_count as f64
68                / (AZIMUTH_COUNT_PER_REV - 1) as f64
69        }
70
71        pub fn azimuth_angle_degree(&self) -> f64 {
72            360.0 * self.azimuth_count as f64 / (AZIMUTH_COUNT_PER_REV - 1) as f64
73        }
74
75        pub fn azimuth_angle(&self) -> Angle {
76            Angle::new::<radian>(self.azimuth_angle_radian())
77        }
78    }
79
80    /// Represents the data packet from Velodyne sensor.
81    #[repr(C, packed)]
82    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
83    pub struct DataPacket {
84        /// Sensor data.
85        pub blocks: [Block; BLOCKS_PER_PACKET],
86        /// Timestamp in microseconds.
87        pub timestamp: u32,
88        /// Indicates single return mode or dual return mode.
89        pub return_mode: ReturnMode,
90        /// Sensor model.
91        pub product_id: ProductID,
92    }
93
94    impl DataPacket {
95        /// Construct packet from [pcap::Packet](pcap::Packet).
96        #[cfg(feature = "pcap")]
97        pub fn from_pcap(packet: &pcap::Packet) -> Result<Self> {
98            let packet_header_size = 42;
99
100            let body_size = packet.header.len as usize - packet_header_size;
101            ensure!(
102                body_size == mem::size_of::<Self>(),
103                "Input pcap packet is not a valid Velodyne Lidar packet",
104            );
105
106            let mut buffer = Box::new([0u8; mem::size_of::<Self>()]);
107            buffer.copy_from_slice(&packet.data[packet_header_size..]);
108            Ok(Self::from_buffer(*buffer))
109        }
110
111        /// Construct packet from binary buffer.
112        pub fn from_buffer(buffer: [u8; mem::size_of::<Self>()]) -> Self {
113            unsafe { mem::transmute::<_, Self>(buffer) }
114        }
115
116        /// Construct packet from slice of bytes. Fail if the slice size is not correct.
117        pub fn from_slice(buffer: &[u8]) -> Result<&Self> {
118            ensure!(
119                buffer.len() == mem::size_of::<Self>(),
120                "Requre the slice length to be {}, but get {}",
121                mem::size_of::<Self>(),
122                buffer.len(),
123            );
124            let packet = unsafe { &*(buffer.as_ptr() as *const Self) };
125            Ok(packet)
126        }
127
128        /// Construct [NaiveDateTime](chrono::NaiveDateTime) from packet timestamp.
129        pub fn datetime(&self) -> NaiveDateTime {
130            let secs = self.timestamp / 1_000_000;
131            let nsecs = (self.timestamp % 1_000_000) * 1000;
132            NaiveDateTime::from_timestamp(secs as i64, nsecs as u32)
133        }
134
135        pub fn time(&self) -> Time {
136            Time::new::<microsecond>(self.timestamp as f64)
137        }
138    }
139}
140
141mod position_packet {
142    use super::*;
143
144    #[repr(C, packed)]
145    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
146    pub struct PositionPacket {
147        pub reserved_head: [u8; 187],
148        pub top_board_temperature: u8,
149        pub bottom_board_temperature: u8,
150        pub last_adc_calibration_temperature: u8,
151        pub last_adc_calibration_temperature_change: u16,
152        pub seconds_since_last_adc_calibration: u32,
153        pub last_adc_calibration_reason: LastAdcCalibrationReason,
154        pub adc_calibration_bitmask: u8,
155        pub toh: u32,
156        pub pps_status: PpsStatus,
157        pub thermal_status: ThermalStatus,
158        pub last_shutdown_temperature: u8,
159        pub temperature_of_unit_at_power_up: u8,
160        pub nmea: [u8; 128],
161        pub reserved_tail: [u8; 178],
162    }
163
164    impl PositionPacket {
165        /// Construct packet from [pcap::Packet](pcap::Packet).
166        #[cfg(feature = "pcap")]
167        pub fn from_pcap(packet: &pcap::Packet) -> Result<Self> {
168            let packet_header_size = 42;
169
170            let body_size = packet.header.len as usize - packet_header_size;
171            ensure!(
172                body_size == mem::size_of::<Self>(),
173                "Input pcap packet is not a valid Velodyne Lidar packet",
174            );
175
176            let mut buffer = Box::new([0u8; mem::size_of::<Self>()]);
177            buffer.copy_from_slice(&packet.data[packet_header_size..]);
178            Ok(Self::from_buffer(*buffer))
179        }
180
181        /// Construct packet from binary buffer.
182        pub fn from_buffer(buffer: [u8; mem::size_of::<Self>()]) -> Self {
183            unsafe { mem::transmute::<_, Self>(buffer) }
184        }
185
186        /// Construct packet from slice of bytes. Fail if the slice size is not correct.
187        pub fn from_slice(buffer: &[u8]) -> Result<&Self> {
188            ensure!(
189                buffer.len() == mem::size_of::<Self>(),
190                "Requre the slice length to be {}, but get {}",
191                mem::size_of::<Self>(),
192                buffer.len(),
193            );
194            let packet = unsafe { &*(buffer.as_ptr() as *const Self) };
195            Ok(packet)
196        }
197
198        pub fn calibration_in_progress(&self) -> bool {
199            self.adc_calibration_bitmask & 0b0001 != 0
200        }
201
202        pub fn meet_delta_temperature(&self) -> bool {
203            self.adc_calibration_bitmask & 0b0010 != 0
204        }
205
206        pub fn meet_periodic_elapsed_time_limit(&self) -> bool {
207            self.adc_calibration_bitmask & 0b0100 != 0
208        }
209    }
210
211    #[repr(u8)]
212    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
213    pub enum LastAdcCalibrationReason {
214        NoCalibration = 0,
215        PowerOn = 1,
216        Manual = 2,
217        DeltaTemperature = 3,
218        Periodic = 4,
219    }
220
221    #[repr(u8)]
222    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
223    pub enum PpsStatus {
224        Abscent = 0,
225        Synchronizing = 1,
226        Locked = 2,
227        Error = 3,
228    }
229
230    #[repr(u8)]
231    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
232    pub enum ThermalStatus {
233        Ok = 0,
234        ThermalShutdown = 1,
235    }
236}
237
238#[cfg(test)]
239mod tests {
240    use super::*;
241
242    #[test]
243    fn velodyne_packet_size_test() {
244        assert_eq!(mem::size_of::<DataPacket>(), 1206);
245        assert_eq!(mem::size_of::<PositionPacket>(), 512);
246    }
247}
lidar_utils/velodyne/packet.rs

lidar_utils/velodyne/
packet.rs
