lidar_utils/ouster/
packet.rs

1//! Provides a set of _C-packed_ structs for Ouster packets.
2
3use super::consts::{COLUMNS_PER_PACKET, ENCODER_TICKS_PER_REV, PIXELS_PER_COLUMN};
4use crate::common::*;
5
6/// Represents a point of signal measurement.
7#[repr(C, packed)]
8#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
9pub struct Pixel {
10    /// The least significant 20 bits form distance in millimeters.
11    pub raw_distance: u32,
12    pub reflectivity: u16,
13    pub signal_photons: u16,
14    pub noise_photons: u16,
15    _pad: u16,
16}
17
18impl Pixel {
19    /// Extract distance in millimeters from raw_distance field.
20    pub fn distance_millimeter(&self) -> u32 {
21        self.raw_distance & 0x000fffff
22    }
23
24    pub fn distance(&self) -> Length {
25        Length::new::<millimeter>(self.distance_millimeter() as f64)
26    }
27}
28
29/// Represents a list of [Pixel]s along with meta data.
30#[repr(C, packed)]
31#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
32pub struct Column {
33    /// Unix timestamp in nanoseconds.
34    pub timestamp: u64,
35    /// The column index.
36    pub measurement_id: u16,
37    /// The frame index.
38    pub frame_id: u16,
39    /// Clockwise encoder count of rotation motor ranging from 0 to [ENCODER_TICKS_PER_REV] (exclusive).
40    pub encoder_ticks: u32,
41    /// Array of pixels.
42    pub pixels: [Pixel; PIXELS_PER_COLUMN],
43    /// Packet validility mark. True if value is 0xffffffff.
44    pub raw_valid: u32,
45}
46
47impl Column {
48    /// Construct [NaiveDateTime](chrono::NaiveDateTime) object from column timestamp.
49    pub fn datetime(&self) -> NaiveDateTime {
50        let secs = self.timestamp / 1_000_000_000;
51        let nsecs = self.timestamp % 1_000_000_000;
52        NaiveDateTime::from_timestamp(secs as i64, nsecs as u32)
53    }
54
55    pub fn time(&self) -> Time {
56        Time::new::<nanosecond>(self.timestamp as f64)
57    }
58
59    /// Compute azimuth angle in degrees from encoder ticks.
60    pub fn azimuth_angle_degrees(&self) -> f64 {
61        360.0 * self.encoder_ticks as f64 / ENCODER_TICKS_PER_REV as f64
62    }
63
64    /// Compute azimuth angle in radians from encoder ticks.
65    pub fn azimuth_angle_radians(&self) -> f64 {
66        2.0 * std::f64::consts::PI * self.encoder_ticks as f64 / ENCODER_TICKS_PER_REV as f64
67    }
68
69    pub fn azimuth_angle(&self) -> Angle {
70        Angle::new::<radian>(self.azimuth_angle_radians())
71    }
72
73    /// Return if this packet is marked valid.
74    pub fn valid(&self) -> bool {
75        self.raw_valid == 0xffffffff
76    }
77}
78
79/// Represents a data packet from Ouster sensor.
80#[repr(C, packed)]
81#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
82pub struct Packet {
83    pub columns: [Column; COLUMNS_PER_PACKET],
84}
85
86impl Packet {
87    /// Construct packet from [pcap's Packet](pcap::Packet).
88    #[cfg(feature = "pcap")]
89    pub fn from_pcap(packet: &pcap::Packet) -> Result<Packet> {
90        let packet_header_size = 42;
91
92        let body_size = packet.header.len as usize - packet_header_size;
93        ensure!(
94            body_size == mem::size_of::<Packet>(),
95            "Input pcap packet is not a valid Ouster Lidar packet",
96        );
97
98        let mut buffer = Box::new([0u8; mem::size_of::<Packet>()]);
99        buffer.copy_from_slice(&packet.data[packet_header_size..]);
100        Ok(Self::from_buffer(*buffer))
101    }
102
103    /// Construct packet from binary buffer.
104    pub fn from_buffer(buffer: [u8; mem::size_of::<Packet>()]) -> Packet {
105        unsafe { std::mem::transmute::<_, Packet>(buffer) }
106    }
107
108    /// Construct packet from slice of bytes. Error if the slice size is not correct.
109    pub fn from_slice(buffer: &[u8]) -> Result<&Packet> {
110        ensure!(
111            buffer.len() == mem::size_of::<Packet>(),
112            "Requre the slice length to be {}, but get {}",
113            mem::size_of::<Packet>(),
114            buffer.len(),
115        );
116        let packet = unsafe { &*(buffer.as_ptr() as *const Packet) };
117        Ok(packet)
118    }
119}
120
121impl AsRef<Packet> for Packet {
122    fn as_ref(&self) -> &Packet {
123        self
124    }
125}
lidar_utils/ouster/packet.rs

lidar_utils/ouster/
packet.rs
