Skip to main content

astrs_node_api/message/
nav.rs

1//! `std/nav/v1` — navigation products built on `std/geometry/v1` (§24.3).
2//!
3//! | Type | Layout |
4//! |---|---|
5//! | [`Odometry`] | `{pose: Pose, pose_covariance: Float64[36], twist: Twist, twist_covariance: Float64[36]}` |
6//! | [`Path`] | `List<Struct{stamp: Timestamp, pose: Pose}>` — a *top-level list*, not a struct wrapping one |
7//! | [`OccupancyGrid`] | `{resolution: Float32, width: UInt32, height: UInt32, origin: Pose, cells: List<Int8>}` |
8//!
9//! [`Path`] is the one std type whose payload column is not a `Struct`: a
10//! path *is* its sequence, so the layout says so. That makes its
11//! encode/decode read a little differently from every other type here, which
12//! is a fair price for not inventing a wrapper the spec does not have.
13//!
14//! # Examples
15//!
16//! ```
17//! use astrs_node_api::message::{AstrsMessage, Path, Pose, StampedPose, Timestamp, Vector3};
18//!
19//! let path = Path::new(vec![StampedPose::new(
20//!     Timestamp::from_nanos(1),
21//!     Pose::new(Vector3::new(1.0, 0.0, 0.0), Default::default()),
22//! )]);
23//! assert_eq!(Path::from_record_batch(&path.to_record_batch()?)?.len(), 1);
24//! # Ok::<(), astrs_data::DataError>(())
25//! ```
26
27use astrs_data::{ArrayRef, AstrsMessage, DataError, DataType, Field, RecordBatch, Result};
28
29use super::geometry::{Pose, Twist};
30use super::scalar::Timestamp;
31use super::{build, read, single_row_column};
32
33/// The width of a 6x6 covariance matrix, flattened row-major.
34pub const COVARIANCE_6X6: usize = 36;
35
36/// A pose and a velocity with covariances — `std/nav/v1/Odometry`.
37#[derive(Debug, Clone, PartialEq)]
38pub struct Odometry {
39    /// The estimated pose.
40    pub pose: Pose,
41    /// Its 6x6 covariance, row-major.
42    pub pose_covariance: [f64; COVARIANCE_6X6],
43    /// The estimated velocity.
44    pub twist: Twist,
45    /// Its 6x6 covariance, row-major.
46    pub twist_covariance: [f64; COVARIANCE_6X6],
47}
48
49impl Odometry {
50    /// An estimate with unknown (all-zero) covariances.
51    #[must_use]
52    pub const fn from_estimate(pose: Pose, twist: Twist) -> Self {
53        Self {
54            pose,
55            pose_covariance: [0.0; COVARIANCE_6X6],
56            twist,
57            twist_covariance: [0.0; COVARIANCE_6X6],
58        }
59    }
60
61    /// The columnar layout of this type.
62    #[must_use]
63    pub fn layout() -> DataType {
64        DataType::strukt([
65            Field::required("pose", Pose::layout()),
66            Field::required("pose_covariance", covariance_layout(6)),
67            Field::required("twist", Twist::layout()),
68            Field::required("twist_covariance", covariance_layout(6)),
69        ])
70    }
71}
72
73impl Default for Odometry {
74    fn default() -> Self {
75        Self::from_estimate(Pose::default(), Twist::default())
76    }
77}
78
79impl AstrsMessage for Odometry {
80    const URN: &'static str = "std/nav/v1/Odometry";
81
82    fn data_type() -> DataType {
83        Self::layout()
84    }
85
86    fn to_record_batch(&self) -> Result<RecordBatch> {
87        let column = build::structure(vec![
88            ("pose", Pose::column(core::slice::from_ref(&self.pose))?),
89            (
90                "pose_covariance",
91                build::fixed_tuples::<f64>("v", 36, &self.pose_covariance)?,
92            ),
93            ("twist", Twist::column(core::slice::from_ref(&self.twist))?),
94            (
95                "twist_covariance",
96                build::fixed_tuples::<f64>("v", 36, &self.twist_covariance)?,
97            ),
98        ])?;
99        Ok(RecordBatch::from_payload(column))
100    }
101
102    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
103        let column = single_row_column(batch)?;
104        Ok(Self {
105            pose: Pose::at(read::child(column, "pose")?, 0)?,
106            pose_covariance: covariance_at(read::child(column, "pose_covariance")?, "pose")?,
107            twist: Twist::at(read::child(column, "twist")?, 0)?,
108            twist_covariance: covariance_at(read::child(column, "twist_covariance")?, "twist")?,
109        })
110    }
111}
112
113super::impl_from_payload!(Odometry);
114
115/// One entry of a [`Path`]: a pose and the time it holds for.
116#[derive(Debug, Clone, Copy, Default, PartialEq)]
117pub struct StampedPose {
118    /// When the pose holds.
119    pub stamp: Timestamp,
120    /// The pose.
121    pub pose: Pose,
122}
123
124impl StampedPose {
125    /// An entry from its parts.
126    #[must_use]
127    pub const fn new(stamp: Timestamp, pose: Pose) -> Self {
128        Self { stamp, pose }
129    }
130
131    /// The columnar layout of one entry.
132    #[must_use]
133    pub fn layout() -> DataType {
134        DataType::strukt([
135            Field::required("stamp", DataType::Timestamp),
136            Field::required("pose", Pose::layout()),
137        ])
138    }
139
140    /// Builds the child column for `values`.
141    ///
142    /// # Errors
143    ///
144    /// [`DataError`] when the columns cannot be assembled.
145    pub fn column(values: &[Self]) -> Result<ArrayRef> {
146        let stamps: Vec<i64> = values.iter().map(|value| value.stamp.nanos).collect();
147        let poses: Vec<Pose> = values.iter().map(|value| value.pose).collect();
148        build::structure(vec![
149            ("stamp", build::timestamps(&stamps)),
150            ("pose", Pose::column(&poses)?),
151        ])
152    }
153
154    /// Reads one row of an entry column.
155    ///
156    /// # Errors
157    ///
158    /// [`DataError`] for a layout mismatch or an out-of-range row.
159    pub fn at(column: &ArrayRef, row: usize) -> Result<Self> {
160        let strukt = read::structure(column)?;
161        Ok(Self {
162            stamp: Timestamp::from_nanos(read::timestamp_at(read::field(strukt, "stamp")?, row)?),
163            pose: Pose::at(read::field(strukt, "pose")?, row)?,
164        })
165    }
166}
167
168/// An ordered pose sequence — `std/nav/v1/Path`.
169#[derive(Debug, Clone, Default, PartialEq)]
170pub struct Path {
171    /// The poses, in order.
172    pub poses: Vec<StampedPose>,
173}
174
175impl Path {
176    /// A path from its poses.
177    #[must_use]
178    pub const fn new(poses: Vec<StampedPose>) -> Self {
179        Self { poses }
180    }
181
182    /// How many poses the path holds.
183    #[must_use]
184    pub fn len(&self) -> usize {
185        self.poses.len()
186    }
187
188    /// Whether the path is empty.
189    #[must_use]
190    pub fn is_empty(&self) -> bool {
191        self.poses.is_empty()
192    }
193
194    /// The columnar layout of this type — a top-level `List`, not a `Struct`.
195    #[must_use]
196    pub fn layout() -> DataType {
197        DataType::list(Field::required("pose", StampedPose::layout()))
198    }
199}
200
201impl AstrsMessage for Path {
202    const URN: &'static str = "std/nav/v1/Path";
203
204    fn data_type() -> DataType {
205        Self::layout()
206    }
207
208    fn to_record_batch(&self) -> Result<RecordBatch> {
209        let child = StampedPose::column(&self.poses)?;
210        let column =
211            build::nested_lists("pose", StampedPose::layout(), &[self.poses.len()], child)?;
212        Ok(RecordBatch::from_payload(column))
213    }
214
215    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
216        let column = single_row_column(batch)?;
217        let (entries, len) = read::list_window(column, 0)?;
218        let poses = (0..len)
219            .map(|row| StampedPose::at(&entries, row))
220            .collect::<Result<Vec<StampedPose>>>()?;
221        Ok(Self { poses })
222    }
223}
224
225super::impl_from_payload!(Path);
226
227/// A 2-D occupancy costmap — `std/nav/v1/OccupancyGrid`, row-major.
228#[derive(Debug, Clone, Default, PartialEq)]
229pub struct OccupancyGrid {
230    /// The size of one cell, in metres.
231    pub resolution: f32,
232    /// The grid's width in cells.
233    pub width: u32,
234    /// The grid's height in cells.
235    pub height: u32,
236    /// The pose of cell `(0, 0)`.
237    pub origin: Pose,
238    /// `width * height` occupancy values, row-major: `-1` unknown, `0..=100`
239    /// the occupancy probability as a percentage.
240    pub cells: Vec<i8>,
241}
242
243impl OccupancyGrid {
244    /// A grid whose cell count matches its dimensions.
245    ///
246    /// # Errors
247    ///
248    /// [`DataError::ChildLengthMismatch`] when `cells.len() != width *
249    /// height`.
250    pub fn new(
251        resolution: f32,
252        width: u32,
253        height: u32,
254        origin: Pose,
255        cells: Vec<i8>,
256    ) -> Result<Self> {
257        let value = Self {
258            resolution,
259            width,
260            height,
261            origin,
262            cells,
263        };
264        value.check_dimensions()?;
265        Ok(value)
266    }
267
268    /// The occupancy at `(x, y)`, or `None` outside the grid.
269    #[must_use]
270    pub fn cell_at(&self, x: u32, y: u32) -> Option<i8> {
271        if x >= self.width || y >= self.height {
272            return None;
273        }
274        let index = usize::try_from(y)
275            .ok()?
276            .checked_mul(usize::try_from(self.width).ok()?)?
277            + usize::try_from(x).ok()?;
278        self.cells.get(index).copied()
279    }
280
281    /// Checks that the cell count matches the declared dimensions.
282    ///
283    /// # Errors
284    ///
285    /// [`DataError::ChildLengthMismatch`].
286    pub fn check_dimensions(&self) -> Result<()> {
287        let expected =
288            usize::try_from(u64::from(self.width) * u64::from(self.height)).unwrap_or(usize::MAX);
289        if self.cells.len() == expected {
290            Ok(())
291        } else {
292            Err(DataError::ChildLengthMismatch {
293                expected,
294                actual: self.cells.len(),
295            })
296        }
297    }
298
299    /// The columnar layout of this type.
300    #[must_use]
301    pub fn layout() -> DataType {
302        DataType::strukt([
303            Field::required("resolution", DataType::Float32),
304            Field::required("width", DataType::UInt32),
305            Field::required("height", DataType::UInt32),
306            Field::required("origin", Pose::layout()),
307            Field::required(
308                "cells",
309                DataType::list(Field::required("cell", DataType::Int8)),
310            ),
311        ])
312    }
313}
314
315impl AstrsMessage for OccupancyGrid {
316    const URN: &'static str = "std/nav/v1/OccupancyGrid";
317
318    fn data_type() -> DataType {
319        Self::layout()
320    }
321
322    fn to_record_batch(&self) -> Result<RecordBatch> {
323        self.check_dimensions()?;
324        let column = build::structure(vec![
325            ("resolution", build::primitive::<f32>(&[self.resolution])),
326            ("width", build::primitive::<u32>(&[self.width])),
327            ("height", build::primitive::<u32>(&[self.height])),
328            ("origin", Pose::column(core::slice::from_ref(&self.origin))?),
329            (
330                "cells",
331                build::primitive_lists::<i8>("cell", &[&self.cells])?,
332            ),
333        ])?;
334        Ok(RecordBatch::from_payload(column))
335    }
336
337    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
338        let column = single_row_column(batch)?;
339        Self::new(
340            read::f32_at(read::child(column, "resolution")?, 0)?,
341            read::primitive_at::<u32>(read::child(column, "width")?, 0)?,
342            read::primitive_at::<u32>(read::child(column, "height")?, 0)?,
343            Pose::at(read::child(column, "origin")?, 0)?,
344            read::primitive_list_at::<i8>(read::child(column, "cells")?, 0)?,
345        )
346    }
347}
348
349super::impl_from_payload!(OccupancyGrid);
350
351/// The layout of an `n * n` row-major covariance matrix.
352fn covariance_layout(n: i32) -> DataType {
353    DataType::fixed_size_list(Field::required("v", DataType::Float64), n * n)
354}
355
356/// Reads a 6x6 covariance out of a `FixedSizeList<Float64, 36>` column.
357fn covariance_at(column: &ArrayRef, field: &str) -> Result<[f64; COVARIANCE_6X6]> {
358    let values = read::fixed_tuple_at::<f64>(column, 0, COVARIANCE_6X6)?;
359    <[f64; COVARIANCE_6X6]>::try_from(values.as_slice()).map_err(|_| {
360        DataError::MessageFixedArrayLength {
361            field: field.to_owned(),
362            expected: COVARIANCE_6X6,
363            actual: values.len(),
364        }
365    })
366}
367
368#[cfg(test)]
369mod tests {
370    #![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
371
372    use super::*;
373    use crate::message::assert_registry_layout;
374    use crate::message::geometry::{Quaternion, Vector3};
375
376    #[test]
377    fn every_nav_type_conforms_to_the_registry() {
378        assert_registry_layout::<Odometry>().unwrap();
379        assert_registry_layout::<Path>().unwrap();
380        assert_registry_layout::<OccupancyGrid>().unwrap();
381    }
382
383    #[test]
384    fn odometry_round_trips_with_its_covariances() {
385        let mut odom = Odometry::from_estimate(
386            Pose::new(Vector3::new(1.0, 2.0, 0.0), Quaternion::identity()),
387            Twist::new(Vector3::new(0.5, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.1)),
388        );
389        odom.pose_covariance[0] = 0.25;
390        odom.twist_covariance[35] = 0.5;
391        let batch = odom.to_record_batch().unwrap();
392        let decoded = Odometry::from_record_batch(&batch).unwrap();
393        assert_eq!(decoded, odom);
394        assert_eq!(decoded.pose_covariance[0], 0.25);
395        assert_eq!(decoded.twist_covariance[35], 0.5);
396        assert_eq!(Odometry::default().pose, Pose::default());
397    }
398
399    #[test]
400    fn paths_round_trip_as_a_top_level_list() {
401        let path = Path::new(vec![
402            StampedPose::new(
403                Timestamp::from_nanos(1),
404                Pose::new(Vector3::new(0.0, 0.0, 0.0), Quaternion::identity()),
405            ),
406            StampedPose::new(
407                Timestamp::from_nanos(2),
408                Pose::new(Vector3::new(1.0, 0.0, 0.0), Quaternion::identity()),
409            ),
410        ]);
411        let batch = path.to_record_batch().unwrap();
412        assert!(
413            matches!(Path::data_type(), DataType::List(_)),
414            "a path is its sequence"
415        );
416        let decoded = Path::from_record_batch(&batch).unwrap();
417        assert_eq!(decoded, path);
418        assert_eq!(decoded.len(), 2);
419        assert!(!decoded.is_empty());
420        assert_eq!(decoded.poses[1].stamp.nanos(), 2);
421    }
422
423    #[test]
424    fn an_empty_path_round_trips() {
425        let path = Path::default();
426        assert!(path.is_empty());
427        let batch = path.to_record_batch().unwrap();
428        assert_eq!(Path::from_record_batch(&batch).unwrap(), path);
429    }
430
431    #[test]
432    fn occupancy_grids_round_trip_and_check_their_dimensions() {
433        let grid =
434            OccupancyGrid::new(0.05, 3, 2, Pose::default(), vec![-1, 0, 50, 100, 0, -1]).unwrap();
435        let batch = grid.to_record_batch().unwrap();
436        assert_eq!(OccupancyGrid::from_record_batch(&batch).unwrap(), grid);
437        assert_eq!(grid.cell_at(0, 0), Some(-1));
438        assert_eq!(grid.cell_at(2, 1), Some(-1));
439        assert_eq!(grid.cell_at(1, 0), Some(0));
440        assert_eq!(grid.cell_at(3, 0), None);
441        assert_eq!(grid.cell_at(0, 2), None);
442
443        let error = OccupancyGrid::new(0.05, 3, 2, Pose::default(), vec![0, 0]).unwrap_err();
444        assert!(matches!(error, DataError::ChildLengthMismatch { .. }));
445    }
446
447    #[test]
448    fn a_wrong_layout_is_refused() {
449        let odom = Odometry::default().to_record_batch().unwrap();
450        assert!(Path::from_record_batch(&odom).is_err());
451        assert!(OccupancyGrid::from_record_batch(&odom).is_err());
452
453        let path = Path::default().to_record_batch().unwrap();
454        assert!(Odometry::from_record_batch(&path).is_err());
455    }
456
457    #[test]
458    fn stamped_poses_read_back_row_by_row() {
459        let entries = [
460            StampedPose::new(Timestamp::from_nanos(10), Pose::default()),
461            StampedPose::new(Timestamp::from_nanos(20), Pose::default()),
462        ];
463        let column = StampedPose::column(&entries).unwrap();
464        assert_eq!(StampedPose::at(&column, 1).unwrap().stamp.nanos(), 20);
465        assert!(StampedPose::at(&column, 2).is_err());
466    }
467}