Skip to main content

astrs_node_api/message/
sensor.rs

1//! `std/sensor/v1` — raw sensor frames (§24.3).
2//!
3//! | Type | Layout |
4//! |---|---|
5//! | [`LaserScan`] | seven `Float32` scalars plus `ranges`/`intensities` as `List<Float32>` |
6//! | [`Imu`] | orientation, angular velocity and linear acceleration, each with a `Float64[9]` covariance |
7//! | [`NavSatFix`] | a GNSS fix with a `Float64[9]` position covariance |
8//! | [`Range`] | a single-beam distance reading |
9//! | [`PointCloud`] | `Struct{<name>: List<Float32>, …}`, structure-of-arrays over `fields=` |
10//!
11//! [`PointCloud`]'s layout depends on its URN parameter, so it carries the
12//! field names as data and exposes [`PointCloud::urn`] rather than a `const
13//! URN`; the other four implement [`AstrsMessage`] directly.
14//!
15//! # Examples
16//!
17//! ```
18//! use astrs_node_api::message::{AstrsMessage, Imu, PointCloud, Vector3};
19//!
20//! let imu = Imu::from_readings(
21//!     Default::default(),
22//!     Vector3::new(0.0, 0.0, 0.1),
23//!     Vector3::new(0.0, 0.0, -9.81),
24//! );
25//! assert_eq!(Imu::from_record_batch(&imu.to_record_batch()?)?, imu);
26//!
27//! let cloud = PointCloud::from_columns(vec![
28//!     ("x".to_owned(), vec![1.0, 2.0]),
29//!     ("y".to_owned(), vec![0.0, 0.5]),
30//! ])?;
31//! assert_eq!(cloud.urn(), "std/sensor/v1/PointCloud[fields=x:y]");
32//! # Ok::<(), astrs_data::DataError>(())
33//! ```
34
35use astrs_data::{AstrsMessage, DataError, DataType, Field, RecordBatch, Result};
36
37use super::geometry::{Quaternion, Vector3};
38use super::{build, read, single_row_column};
39
40/// The width of a 3x3 covariance matrix, flattened row-major.
41pub const COVARIANCE_3X3: usize = 9;
42
43/// A planar range sweep — `std/sensor/v1/LaserScan`.
44#[derive(Debug, Clone, Default, PartialEq)]
45pub struct LaserScan {
46    /// The angle of the first beam, in radians.
47    pub angle_min: f32,
48    /// The angle of the last beam, in radians.
49    pub angle_max: f32,
50    /// The angular step between beams, in radians.
51    pub angle_increment: f32,
52    /// The time between beams, in seconds.
53    pub time_increment: f32,
54    /// The time the whole sweep took, in seconds.
55    pub scan_time: f32,
56    /// The smallest range the device can report, in metres.
57    pub range_min: f32,
58    /// The largest range the device can report, in metres.
59    pub range_max: f32,
60    /// One range per beam, in metres.
61    pub ranges: Vec<f32>,
62    /// One intensity per beam; empty when the device reports none.
63    pub intensities: Vec<f32>,
64}
65
66impl LaserScan {
67    /// The number of beams the sweep reports.
68    #[must_use]
69    pub fn beam_count(&self) -> usize {
70        self.ranges.len()
71    }
72
73    /// The angle of beam `index`, in radians.
74    #[must_use]
75    pub fn angle_of(&self, index: usize) -> Option<f32> {
76        if index >= self.ranges.len() {
77            return None;
78        }
79        // `index` is bounded by the beam count, which a scan of any plausible
80        // size keeps well inside `f32`'s exact-integer range.
81        let step = u32::try_from(index).unwrap_or(u32::MAX);
82        #[expect(
83            clippy::cast_precision_loss,
84            reason = "a beam index is far below f32's 2^24 exact-integer ceiling"
85        )]
86        Some(self.angle_min + self.angle_increment * (step as f32))
87    }
88
89    /// Whether the sweep carries an intensity per beam.
90    #[must_use]
91    pub fn has_intensities(&self) -> bool {
92        !self.intensities.is_empty() && self.intensities.len() == self.ranges.len()
93    }
94
95    /// The columnar layout of this type.
96    #[must_use]
97    pub fn layout() -> DataType {
98        DataType::strukt([
99            Field::required("angle_min", DataType::Float32),
100            Field::required("angle_max", DataType::Float32),
101            Field::required("angle_increment", DataType::Float32),
102            Field::required("time_increment", DataType::Float32),
103            Field::required("scan_time", DataType::Float32),
104            Field::required("range_min", DataType::Float32),
105            Field::required("range_max", DataType::Float32),
106            Field::required(
107                "ranges",
108                DataType::list(Field::required("range", DataType::Float32)),
109            ),
110            Field::required(
111                "intensities",
112                DataType::list(Field::required("intensity", DataType::Float32)),
113            ),
114        ])
115    }
116}
117
118impl AstrsMessage for LaserScan {
119    const URN: &'static str = "std/sensor/v1/LaserScan";
120
121    fn data_type() -> DataType {
122        Self::layout()
123    }
124
125    fn to_record_batch(&self) -> Result<RecordBatch> {
126        let column = build::structure(vec![
127            ("angle_min", build::primitive::<f32>(&[self.angle_min])),
128            ("angle_max", build::primitive::<f32>(&[self.angle_max])),
129            (
130                "angle_increment",
131                build::primitive::<f32>(&[self.angle_increment]),
132            ),
133            (
134                "time_increment",
135                build::primitive::<f32>(&[self.time_increment]),
136            ),
137            ("scan_time", build::primitive::<f32>(&[self.scan_time])),
138            ("range_min", build::primitive::<f32>(&[self.range_min])),
139            ("range_max", build::primitive::<f32>(&[self.range_max])),
140            (
141                "ranges",
142                build::primitive_lists::<f32>("range", &[&self.ranges])?,
143            ),
144            (
145                "intensities",
146                build::primitive_lists::<f32>("intensity", &[&self.intensities])?,
147            ),
148        ])?;
149        Ok(RecordBatch::from_payload(column))
150    }
151
152    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
153        let column = single_row_column(batch)?;
154        Ok(Self {
155            angle_min: read::f32_at(read::child(column, "angle_min")?, 0)?,
156            angle_max: read::f32_at(read::child(column, "angle_max")?, 0)?,
157            angle_increment: read::f32_at(read::child(column, "angle_increment")?, 0)?,
158            time_increment: read::f32_at(read::child(column, "time_increment")?, 0)?,
159            scan_time: read::f32_at(read::child(column, "scan_time")?, 0)?,
160            range_min: read::f32_at(read::child(column, "range_min")?, 0)?,
161            range_max: read::f32_at(read::child(column, "range_max")?, 0)?,
162            ranges: read::primitive_list_at::<f32>(read::child(column, "ranges")?, 0)?,
163            intensities: read::primitive_list_at::<f32>(read::child(column, "intensities")?, 0)?,
164        })
165    }
166}
167
168super::impl_from_payload!(LaserScan);
169
170/// An inertial reading — `std/sensor/v1/Imu`.
171#[derive(Debug, Clone, PartialEq)]
172pub struct Imu {
173    /// The estimated orientation.
174    pub orientation: Quaternion,
175    /// Its 3x3 covariance, row-major.
176    pub orientation_covariance: [f64; COVARIANCE_3X3],
177    /// The angular velocity, in radians per second.
178    pub angular_velocity: Vector3,
179    /// Its 3x3 covariance, row-major.
180    pub angular_velocity_covariance: [f64; COVARIANCE_3X3],
181    /// The linear acceleration, in metres per second squared.
182    pub linear_acceleration: Vector3,
183    /// Its 3x3 covariance, row-major.
184    pub linear_acceleration_covariance: [f64; COVARIANCE_3X3],
185}
186
187impl Imu {
188    /// A reading with unknown (all-zero) covariances, which is what
189    /// `sensor_msgs/Imu` means by an unreported one.
190    #[must_use]
191    pub const fn from_readings(
192        orientation: Quaternion,
193        angular_velocity: Vector3,
194        linear_acceleration: Vector3,
195    ) -> Self {
196        Self {
197            orientation,
198            orientation_covariance: [0.0; COVARIANCE_3X3],
199            angular_velocity,
200            angular_velocity_covariance: [0.0; COVARIANCE_3X3],
201            linear_acceleration,
202            linear_acceleration_covariance: [0.0; COVARIANCE_3X3],
203        }
204    }
205
206    /// The columnar layout of this type.
207    #[must_use]
208    pub fn layout() -> DataType {
209        DataType::strukt([
210            Field::required("orientation", Quaternion::layout()),
211            Field::required("orientation_covariance", covariance_layout(3)),
212            Field::required("angular_velocity", Vector3::layout()),
213            Field::required("angular_velocity_covariance", covariance_layout(3)),
214            Field::required("linear_acceleration", Vector3::layout()),
215            Field::required("linear_acceleration_covariance", covariance_layout(3)),
216        ])
217    }
218}
219
220impl Default for Imu {
221    fn default() -> Self {
222        Self::from_readings(Quaternion::IDENTITY, Vector3::ZERO, Vector3::ZERO)
223    }
224}
225
226impl AstrsMessage for Imu {
227    const URN: &'static str = "std/sensor/v1/Imu";
228
229    fn data_type() -> DataType {
230        Self::layout()
231    }
232
233    fn to_record_batch(&self) -> Result<RecordBatch> {
234        let column = build::structure(vec![
235            (
236                "orientation",
237                Quaternion::column(core::slice::from_ref(&self.orientation))?,
238            ),
239            (
240                "orientation_covariance",
241                build::fixed_tuples::<f64>("v", 9, &self.orientation_covariance)?,
242            ),
243            (
244                "angular_velocity",
245                Vector3::column(core::slice::from_ref(&self.angular_velocity))?,
246            ),
247            (
248                "angular_velocity_covariance",
249                build::fixed_tuples::<f64>("v", 9, &self.angular_velocity_covariance)?,
250            ),
251            (
252                "linear_acceleration",
253                Vector3::column(core::slice::from_ref(&self.linear_acceleration))?,
254            ),
255            (
256                "linear_acceleration_covariance",
257                build::fixed_tuples::<f64>("v", 9, &self.linear_acceleration_covariance)?,
258            ),
259        ])?;
260        Ok(RecordBatch::from_payload(column))
261    }
262
263    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
264        let column = single_row_column(batch)?;
265        Ok(Self {
266            orientation: Quaternion::at(read::child(column, "orientation")?, 0)?,
267            orientation_covariance: covariance_at(
268                read::child(column, "orientation_covariance")?,
269                "orientation_covariance",
270            )?,
271            angular_velocity: Vector3::at(read::child(column, "angular_velocity")?, 0)?,
272            angular_velocity_covariance: covariance_at(
273                read::child(column, "angular_velocity_covariance")?,
274                "angular_velocity_covariance",
275            )?,
276            linear_acceleration: Vector3::at(read::child(column, "linear_acceleration")?, 0)?,
277            linear_acceleration_covariance: covariance_at(
278                read::child(column, "linear_acceleration_covariance")?,
279                "linear_acceleration_covariance",
280            )?,
281        })
282    }
283}
284
285super::impl_from_payload!(Imu);
286
287/// A GNSS fix — `std/sensor/v1/NavSatFix`.
288#[derive(Debug, Clone, PartialEq)]
289pub struct NavSatFix {
290    /// The fix status, matching `sensor_msgs/NavSatStatus::status`.
291    pub status: i8,
292    /// The constellations used, matching `sensor_msgs/NavSatStatus::service`.
293    pub service: u16,
294    /// Latitude in degrees.
295    pub latitude: f64,
296    /// Longitude in degrees.
297    pub longitude: f64,
298    /// Altitude in metres above the WGS 84 ellipsoid.
299    pub altitude: f64,
300    /// The 3x3 position covariance, row-major.
301    pub position_covariance: [f64; COVARIANCE_3X3],
302    /// How the covariance was obtained, matching `sensor_msgs/NavSatFix`.
303    pub position_covariance_type: u8,
304}
305
306impl NavSatFix {
307    /// A fix at a coordinate, with no covariance reported.
308    #[must_use]
309    pub const fn at_coordinate(latitude: f64, longitude: f64, altitude: f64) -> Self {
310        Self {
311            status: 0,
312            service: 1,
313            latitude,
314            longitude,
315            altitude,
316            position_covariance: [0.0; COVARIANCE_3X3],
317            position_covariance_type: 0,
318        }
319    }
320
321    /// The columnar layout of this type.
322    #[must_use]
323    pub fn layout() -> DataType {
324        DataType::strukt([
325            Field::required("status", DataType::Int8),
326            Field::required("service", DataType::UInt16),
327            Field::required("latitude", DataType::Float64),
328            Field::required("longitude", DataType::Float64),
329            Field::required("altitude", DataType::Float64),
330            Field::required("position_covariance", covariance_layout(3)),
331            Field::required("position_covariance_type", DataType::UInt8),
332        ])
333    }
334}
335
336impl Default for NavSatFix {
337    fn default() -> Self {
338        Self::at_coordinate(0.0, 0.0, 0.0)
339    }
340}
341
342impl AstrsMessage for NavSatFix {
343    const URN: &'static str = "std/sensor/v1/NavSatFix";
344
345    fn data_type() -> DataType {
346        Self::layout()
347    }
348
349    fn to_record_batch(&self) -> Result<RecordBatch> {
350        let column = build::structure(vec![
351            ("status", build::primitive::<i8>(&[self.status])),
352            ("service", build::primitive::<u16>(&[self.service])),
353            ("latitude", build::primitive::<f64>(&[self.latitude])),
354            ("longitude", build::primitive::<f64>(&[self.longitude])),
355            ("altitude", build::primitive::<f64>(&[self.altitude])),
356            (
357                "position_covariance",
358                build::fixed_tuples::<f64>("v", 9, &self.position_covariance)?,
359            ),
360            (
361                "position_covariance_type",
362                build::primitive::<u8>(&[self.position_covariance_type]),
363            ),
364        ])?;
365        Ok(RecordBatch::from_payload(column))
366    }
367
368    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
369        let column = single_row_column(batch)?;
370        Ok(Self {
371            status: read::primitive_at::<i8>(read::child(column, "status")?, 0)?,
372            service: read::primitive_at::<u16>(read::child(column, "service")?, 0)?,
373            latitude: read::f64_at(read::child(column, "latitude")?, 0)?,
374            longitude: read::f64_at(read::child(column, "longitude")?, 0)?,
375            altitude: read::f64_at(read::child(column, "altitude")?, 0)?,
376            position_covariance: covariance_at(
377                read::child(column, "position_covariance")?,
378                "position_covariance",
379            )?,
380            position_covariance_type: read::primitive_at::<u8>(
381                read::child(column, "position_covariance_type")?,
382                0,
383            )?,
384        })
385    }
386}
387
388super::impl_from_payload!(NavSatFix);
389
390/// A single-beam distance reading — `std/sensor/v1/Range`.
391#[derive(Debug, Clone, Copy, Default, PartialEq)]
392pub struct Range {
393    /// The radiation type, matching `sensor_msgs/Range` (`0` ultrasound,
394    /// `1` infrared).
395    pub radiation_type: u8,
396    /// The beam's field of view, in radians.
397    pub field_of_view: f32,
398    /// The smallest range the device can report, in metres.
399    pub min_range: f32,
400    /// The largest range the device can report, in metres.
401    pub max_range: f32,
402    /// The reading, in metres.
403    pub range: f32,
404}
405
406impl Range {
407    /// Whether the reading falls inside the device's reportable window.
408    #[must_use]
409    pub fn is_in_range(&self) -> bool {
410        self.range >= self.min_range && self.range <= self.max_range
411    }
412
413    /// The columnar layout of this type.
414    #[must_use]
415    pub fn layout() -> DataType {
416        DataType::strukt([
417            Field::required("radiation_type", DataType::UInt8),
418            Field::required("field_of_view", DataType::Float32),
419            Field::required("min_range", DataType::Float32),
420            Field::required("max_range", DataType::Float32),
421            Field::required("range", DataType::Float32),
422        ])
423    }
424}
425
426impl AstrsMessage for Range {
427    const URN: &'static str = "std/sensor/v1/Range";
428
429    fn data_type() -> DataType {
430        Self::layout()
431    }
432
433    fn to_record_batch(&self) -> Result<RecordBatch> {
434        let column = build::structure(vec![
435            (
436                "radiation_type",
437                build::primitive::<u8>(&[self.radiation_type]),
438            ),
439            (
440                "field_of_view",
441                build::primitive::<f32>(&[self.field_of_view]),
442            ),
443            ("min_range", build::primitive::<f32>(&[self.min_range])),
444            ("max_range", build::primitive::<f32>(&[self.max_range])),
445            ("range", build::primitive::<f32>(&[self.range])),
446        ])?;
447        Ok(RecordBatch::from_payload(column))
448    }
449
450    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
451        let column = single_row_column(batch)?;
452        Ok(Self {
453            radiation_type: read::primitive_at::<u8>(read::child(column, "radiation_type")?, 0)?,
454            field_of_view: read::f32_at(read::child(column, "field_of_view")?, 0)?,
455            min_range: read::f32_at(read::child(column, "min_range")?, 0)?,
456            max_range: read::f32_at(read::child(column, "max_range")?, 0)?,
457            range: read::f32_at(read::child(column, "range")?, 0)?,
458        })
459    }
460}
461
462super::impl_from_payload!(Range);
463
464/// A point set with named per-point fields —
465/// `std/sensor/v1/PointCloud[fields=…]`, structure-of-arrays.
466///
467/// Deliberately not `sensor_msgs/PointCloud2`'s packed byte blob: each field
468/// keeps its own typed, SIMD-ready buffer, so nothing has to unpack a
469/// row-major blob to read `z`.
470///
471/// The layout depends on the URN's `fields` parameter, which a `const URN`
472/// cannot express — this type therefore carries the names and renders its own
473/// [`PointCloud::urn`] rather than implementing [`AstrsMessage`].
474#[derive(Debug, Clone, Default, PartialEq)]
475pub struct PointCloud {
476    /// One `(name, values)` pair per field, in declaration order.
477    columns: Vec<(String, Vec<f32>)>,
478}
479
480impl PointCloud {
481    /// A cloud from its named columns.
482    ///
483    /// # Errors
484    ///
485    /// [`DataError::DuplicateFieldName`] when a name repeats, and
486    /// [`DataError::ColumnLengthMismatch`] when the columns disagree on how
487    /// many points they describe.
488    pub fn from_columns(columns: Vec<(String, Vec<f32>)>) -> Result<Self> {
489        let value = Self { columns };
490        value.check()?;
491        Ok(value)
492    }
493
494    /// An `x:y:z` cloud from three same-length coordinate columns.
495    ///
496    /// # Errors
497    ///
498    /// As [`PointCloud::from_columns`].
499    pub fn xyz(x: Vec<f32>, y: Vec<f32>, z: Vec<f32>) -> Result<Self> {
500        Self::from_columns(vec![
501            ("x".to_owned(), x),
502            ("y".to_owned(), y),
503            ("z".to_owned(), z),
504        ])
505    }
506
507    /// The field names, in declaration order.
508    #[must_use]
509    pub fn field_names(&self) -> Vec<&str> {
510        self.columns.iter().map(|(name, _)| name.as_str()).collect()
511    }
512
513    /// One field's values.
514    #[must_use]
515    pub fn column(&self, name: &str) -> Option<&[f32]> {
516        self.columns
517            .iter()
518            .find(|(field, _)| field == name)
519            .map(|(_, values)| values.as_slice())
520    }
521
522    /// How many points the cloud holds.
523    #[must_use]
524    pub fn len(&self) -> usize {
525        self.columns.first().map_or(0, |(_, values)| values.len())
526    }
527
528    /// Whether the cloud holds no points.
529    #[must_use]
530    pub fn is_empty(&self) -> bool {
531        self.len() == 0
532    }
533
534    /// The parameterised URN a port carrying this cloud declares.
535    #[must_use]
536    pub fn urn(&self) -> String {
537        format!(
538            "std/sensor/v1/PointCloud[fields={}]",
539            self.field_names().join(":")
540        )
541    }
542
543    /// The columnar layout this cloud's fields produce.
544    #[must_use]
545    pub fn layout(&self) -> DataType {
546        DataType::strukt(self.columns.iter().map(|(name, _)| {
547            Field::required(
548                name,
549                DataType::list(Field::required("v", DataType::Float32)),
550            )
551        }))
552    }
553
554    /// Checks the invariants a well-formed cloud holds.
555    ///
556    /// # Errors
557    ///
558    /// As [`PointCloud::from_columns`].
559    pub fn check(&self) -> Result<()> {
560        let mut seen: Vec<&str> = Vec::with_capacity(self.columns.len());
561        let expected = self.len();
562        for (index, (name, values)) in self.columns.iter().enumerate() {
563            if seen.contains(&name.as_str()) {
564                return Err(DataError::DuplicateFieldName { name: name.clone() });
565            }
566            seen.push(name);
567            if values.len() != expected {
568                return Err(DataError::ColumnLengthMismatch {
569                    index,
570                    name: name.clone(),
571                    expected,
572                    actual: values.len(),
573                });
574            }
575        }
576        Ok(())
577    }
578
579    /// Encodes the cloud as a one-row payload batch.
580    ///
581    /// The inherent twin of [`AstrsMessage::to_record_batch`]; see the type's
582    /// documentation for why the trait itself does not apply.
583    ///
584    /// # Errors
585    ///
586    /// [`DataError`] when the cloud is malformed or the columns cannot be
587    /// assembled.
588    pub fn to_record_batch(&self) -> Result<RecordBatch> {
589        self.check()?;
590        let mut children: Vec<(&str, astrs_data::ArrayRef)> =
591            Vec::with_capacity(self.columns.len());
592        for (name, values) in &self.columns {
593            children.push((
594                name.as_str(),
595                build::primitive_lists::<f32>("v", &[values])?,
596            ));
597        }
598        Ok(RecordBatch::from_payload(build::structure(children)?))
599    }
600
601    /// Decodes a cloud from a one-row payload batch.
602    ///
603    /// # Errors
604    ///
605    /// [`DataError`] when the batch is not a `PointCloud` layout.
606    pub fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
607        let column = single_row_column(batch)?;
608        let strukt = read::structure(column)?;
609        let mut columns = Vec::with_capacity(strukt.fields().len());
610        for field in strukt.fields() {
611            let name = field.name().to_owned();
612            let child = read::field(strukt, &name)?;
613            columns.push((name, read::primitive_list_at::<f32>(child, 0)?));
614        }
615        Self::from_columns(columns)
616    }
617}
618
619impl super::FromPayload for PointCloud {
620    fn from_batch(batch: &RecordBatch) -> Result<Self> {
621        Self::from_record_batch(batch)
622    }
623}
624
625/// The layout of an `n * n` row-major covariance matrix.
626#[must_use]
627fn covariance_layout(n: i32) -> DataType {
628    DataType::fixed_size_list(Field::required("v", DataType::Float64), n * n)
629}
630
631/// Reads a 3x3 covariance out of a `FixedSizeList<Float64, 9>` column.
632fn covariance_at(column: &astrs_data::ArrayRef, field: &str) -> Result<[f64; COVARIANCE_3X3]> {
633    let values = read::fixed_tuple_at::<f64>(column, 0, COVARIANCE_3X3)?;
634    <[f64; COVARIANCE_3X3]>::try_from(values.as_slice()).map_err(|_| {
635        DataError::MessageFixedArrayLength {
636            field: field.to_owned(),
637            expected: COVARIANCE_3X3,
638            actual: values.len(),
639        }
640    })
641}
642
643#[cfg(test)]
644mod tests {
645    #![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
646
647    use super::*;
648    use crate::message::{assert_layout_matches, assert_registry_layout};
649
650    #[test]
651    fn every_fixed_sensor_type_conforms_to_the_registry() {
652        assert_registry_layout::<LaserScan>().unwrap();
653        assert_registry_layout::<Imu>().unwrap();
654        assert_registry_layout::<NavSatFix>().unwrap();
655        assert_registry_layout::<Range>().unwrap();
656    }
657
658    #[test]
659    fn laser_scans_round_trip() {
660        let scan = LaserScan {
661            angle_min: -1.5,
662            angle_max: 1.5,
663            angle_increment: 0.5,
664            time_increment: 0.001,
665            scan_time: 0.1,
666            range_min: 0.05,
667            range_max: 30.0,
668            ranges: vec![1.0, 2.0, 3.0],
669            intensities: vec![10.0, 20.0, 30.0],
670        };
671        let batch = scan.to_record_batch().unwrap();
672        assert_eq!(LaserScan::from_record_batch(&batch).unwrap(), scan);
673        assert_eq!(scan.beam_count(), 3);
674        assert!(scan.has_intensities());
675        assert_eq!(scan.angle_of(0), Some(-1.5));
676        assert_eq!(scan.angle_of(2), Some(-0.5));
677        assert_eq!(scan.angle_of(3), None);
678
679        let bare = LaserScan {
680            intensities: Vec::new(),
681            ..scan
682        };
683        assert!(!bare.has_intensities());
684        assert_eq!(
685            LaserScan::from_record_batch(&bare.to_record_batch().unwrap()).unwrap(),
686            bare
687        );
688        assert_eq!(LaserScan::default().beam_count(), 0);
689    }
690
691    #[test]
692    fn imu_readings_round_trip_with_their_covariances() {
693        let mut imu = Imu::from_readings(
694            Quaternion::identity(),
695            Vector3::new(0.0, 0.0, 0.1),
696            Vector3::new(0.0, 0.0, -9.81),
697        );
698        imu.orientation_covariance = [1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0];
699        let batch = imu.to_record_batch().unwrap();
700        let decoded = Imu::from_record_batch(&batch).unwrap();
701        assert_eq!(decoded, imu);
702        assert_eq!(decoded.orientation_covariance[4], 2.0);
703        assert_eq!(Imu::default().linear_acceleration, Vector3::ZERO);
704    }
705
706    #[test]
707    fn gnss_fixes_round_trip() {
708        let fix = NavSatFix::at_coordinate(35.681_2, 139.767_1, 40.0);
709        let batch = fix.to_record_batch().unwrap();
710        assert_eq!(NavSatFix::from_record_batch(&batch).unwrap(), fix);
711        assert_eq!(NavSatFix::default().latitude, 0.0);
712    }
713
714    #[test]
715    fn ranges_round_trip_and_report_their_window() {
716        let reading = Range {
717            radiation_type: 1,
718            field_of_view: 0.26,
719            min_range: 0.1,
720            max_range: 4.0,
721            range: 1.2,
722        };
723        let batch = reading.to_record_batch().unwrap();
724        assert_eq!(Range::from_record_batch(&batch).unwrap(), reading);
725        assert!(reading.is_in_range());
726        assert!(
727            !Range {
728                range: 9.0,
729                ..reading
730            }
731            .is_in_range()
732        );
733    }
734
735    #[test]
736    fn point_clouds_round_trip_and_render_their_urn() {
737        let cloud = PointCloud::xyz(vec![1.0, 2.0], vec![0.0, 0.5], vec![-1.0, -2.0]).unwrap();
738        assert_eq!(cloud.urn(), "std/sensor/v1/PointCloud[fields=x:y:z]");
739        assert_layout_matches(&cloud.urn(), &cloud.layout()).unwrap();
740
741        let batch = cloud.to_record_batch().unwrap();
742        let decoded = PointCloud::from_record_batch(&batch).unwrap();
743        assert_eq!(decoded, cloud);
744        assert_eq!(decoded.len(), 2);
745        assert!(!decoded.is_empty());
746        assert_eq!(decoded.field_names(), vec!["x", "y", "z"]);
747        assert_eq!(decoded.column("y"), Some(&[0.0, 0.5][..]));
748        assert_eq!(decoded.column("intensity"), None);
749    }
750
751    #[test]
752    fn a_malformed_point_cloud_is_refused() {
753        let error = PointCloud::from_columns(vec![
754            ("x".to_owned(), vec![1.0, 2.0]),
755            ("y".to_owned(), vec![1.0]),
756        ])
757        .unwrap_err();
758        assert!(matches!(error, DataError::ColumnLengthMismatch { .. }));
759
760        let error = PointCloud::from_columns(vec![
761            ("x".to_owned(), vec![1.0]),
762            ("x".to_owned(), vec![2.0]),
763        ])
764        .unwrap_err();
765        assert!(matches!(error, DataError::DuplicateFieldName { .. }));
766
767        assert!(PointCloud::default().is_empty());
768        assert_eq!(PointCloud::default().len(), 0);
769    }
770
771    #[test]
772    fn a_wrong_layout_is_refused() {
773        let range = Range::default().to_record_batch().unwrap();
774        assert!(Imu::from_record_batch(&range).is_err());
775        assert!(LaserScan::from_record_batch(&range).is_err());
776        assert!(NavSatFix::from_record_batch(&range).is_err());
777
778        let imu = Imu::default().to_record_batch().unwrap();
779        assert!(Range::from_record_batch(&imu).is_err());
780        assert!(PointCloud::from_record_batch(&imu).is_err());
781    }
782}