1use 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
33pub const COVARIANCE_6X6: usize = 36;
35
36#[derive(Debug, Clone, PartialEq)]
38pub struct Odometry {
39 pub pose: Pose,
41 pub pose_covariance: [f64; COVARIANCE_6X6],
43 pub twist: Twist,
45 pub twist_covariance: [f64; COVARIANCE_6X6],
47}
48
49impl Odometry {
50 #[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 #[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#[derive(Debug, Clone, Copy, Default, PartialEq)]
117pub struct StampedPose {
118 pub stamp: Timestamp,
120 pub pose: Pose,
122}
123
124impl StampedPose {
125 #[must_use]
127 pub const fn new(stamp: Timestamp, pose: Pose) -> Self {
128 Self { stamp, pose }
129 }
130
131 #[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 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 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#[derive(Debug, Clone, Default, PartialEq)]
170pub struct Path {
171 pub poses: Vec<StampedPose>,
173}
174
175impl Path {
176 #[must_use]
178 pub const fn new(poses: Vec<StampedPose>) -> Self {
179 Self { poses }
180 }
181
182 #[must_use]
184 pub fn len(&self) -> usize {
185 self.poses.len()
186 }
187
188 #[must_use]
190 pub fn is_empty(&self) -> bool {
191 self.poses.is_empty()
192 }
193
194 #[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#[derive(Debug, Clone, Default, PartialEq)]
229pub struct OccupancyGrid {
230 pub resolution: f32,
232 pub width: u32,
234 pub height: u32,
236 pub origin: Pose,
238 pub cells: Vec<i8>,
241}
242
243impl OccupancyGrid {
244 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 #[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 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 #[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
351fn covariance_layout(n: i32) -> DataType {
353 DataType::fixed_size_list(Field::required("v", DataType::Float64), n * n)
354}
355
356fn 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}