Skip to main content

astrs_node_api/message/
geometry.rs

1//! `std/geometry/v1` — rigid-body quantities (§24.3).
2//!
3//! Six types, all `Struct` compositions of `Float64` scalars, all mirroring
4//! their `geometry_msgs` counterpart field for field so `astrs-ros2` bridges
5//! them by column name rather than by translation table.
6//!
7//! | Type | Layout |
8//! |---|---|
9//! | [`Vector3`] | `{x, y, z}` |
10//! | [`Quaternion`] | `{x, y, z, w}` — scalar last, the ROS 2 and Eigen order |
11//! | [`Pose`] | `{position: Vector3, orientation: Quaternion}` |
12//! | [`Transform`] | `{translation: Vector3, rotation: Quaternion}` |
13//! | [`Twist`] | `{linear: Vector3, angular: Vector3}` |
14//! | [`Accel`] | `{linear: Vector3, angular: Vector3}` |
15//!
16//! [`Twist`] and [`Accel`] are byte-identical: a velocity and an acceleration
17//! have the same shape, and only the URN says which one a port carries. That
18//! is deliberate — it is also why `astrs-data`'s reverse lookup refuses to
19//! guess a URN from a compound layout.
20//!
21//! # Examples
22//!
23//! ```
24//! use astrs_node_api::message::{AstrsMessage, Pose, Quaternion, Vector3};
25//!
26//! let pose = Pose::new(Vector3::new(1.0, 2.0, 3.0), Quaternion::identity());
27//! let batch = pose.to_record_batch()?;
28//! assert_eq!(Pose::from_record_batch(&batch)?, pose);
29//! assert!(pose.orientation.is_normalized(1e-9));
30//! # Ok::<(), astrs_data::DataError>(())
31//! ```
32
33use astrs_data::{AstrsMessage, DataType, Field, RecordBatch, Result};
34
35use super::{build, read, single_row_column};
36
37/// A three-component vector — `std/geometry/v1/Vector3`.
38#[derive(Debug, Clone, Copy, Default, PartialEq, PartialOrd)]
39pub struct Vector3 {
40    /// The x component.
41    pub x: f64,
42    /// The y component.
43    pub y: f64,
44    /// The z component.
45    pub z: f64,
46}
47
48impl Vector3 {
49    /// The zero vector.
50    pub const ZERO: Self = Self {
51        x: 0.0,
52        y: 0.0,
53        z: 0.0,
54    };
55
56    /// A vector from its components.
57    #[must_use]
58    pub const fn new(x: f64, y: f64, z: f64) -> Self {
59        Self { x, y, z }
60    }
61
62    /// The vector's Euclidean length.
63    #[must_use]
64    pub fn norm(self) -> f64 {
65        self.x.hypot(self.y).hypot(self.z)
66    }
67
68    /// The components as an array, in `xyz` order.
69    #[must_use]
70    pub const fn to_array(self) -> [f64; 3] {
71        [self.x, self.y, self.z]
72    }
73
74    /// A vector from an `xyz` array.
75    #[must_use]
76    pub const fn from_array(values: [f64; 3]) -> Self {
77        Self::new(values[0], values[1], values[2])
78    }
79
80    /// The columnar layout of this type, shared with every composite that
81    /// embeds it.
82    #[must_use]
83    pub fn layout() -> DataType {
84        DataType::strukt([
85            Field::required("x", DataType::Float64),
86            Field::required("y", DataType::Float64),
87            Field::required("z", DataType::Float64),
88        ])
89    }
90
91    /// Builds the child column for `values`.
92    ///
93    /// # Errors
94    ///
95    /// [`astrs_data::DataError`] when the columns cannot be assembled.
96    pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
97        let x: Vec<f64> = values.iter().map(|value| value.x).collect();
98        let y: Vec<f64> = values.iter().map(|value| value.y).collect();
99        let z: Vec<f64> = values.iter().map(|value| value.z).collect();
100        build::structure(vec![
101            ("x", build::primitive::<f64>(&x)),
102            ("y", build::primitive::<f64>(&y)),
103            ("z", build::primitive::<f64>(&z)),
104        ])
105    }
106
107    /// Reads one row of a `Vector3` column.
108    ///
109    /// # Errors
110    ///
111    /// [`astrs_data::DataError`] for a layout mismatch or an out-of-range row.
112    pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
113        let strukt = read::structure(column)?;
114        Ok(Self {
115            x: read::f64_at(read::field(strukt, "x")?, row)?,
116            y: read::f64_at(read::field(strukt, "y")?, row)?,
117            z: read::f64_at(read::field(strukt, "z")?, row)?,
118        })
119    }
120}
121
122impl From<[f64; 3]> for Vector3 {
123    fn from(values: [f64; 3]) -> Self {
124        Self::from_array(values)
125    }
126}
127
128impl AstrsMessage for Vector3 {
129    const URN: &'static str = "std/geometry/v1/Vector3";
130
131    fn data_type() -> DataType {
132        Self::layout()
133    }
134
135    fn to_record_batch(&self) -> Result<RecordBatch> {
136        Ok(RecordBatch::from_payload(Self::column(
137            core::slice::from_ref(self),
138        )?))
139    }
140
141    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
142        Self::at(single_row_column(batch)?, 0)
143    }
144}
145
146super::impl_from_payload!(Vector3);
147
148/// A rotation — `std/geometry/v1/Quaternion`, stored `xyzw` (scalar last).
149#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
150pub struct Quaternion {
151    /// The x component of the vector part.
152    pub x: f64,
153    /// The y component of the vector part.
154    pub y: f64,
155    /// The z component of the vector part.
156    pub z: f64,
157    /// The scalar part.
158    pub w: f64,
159}
160
161impl Quaternion {
162    /// The identity rotation.
163    pub const IDENTITY: Self = Self {
164        x: 0.0,
165        y: 0.0,
166        z: 0.0,
167        w: 1.0,
168    };
169
170    /// A quaternion from its components, in `xyzw` order.
171    #[must_use]
172    pub const fn new(x: f64, y: f64, z: f64, w: f64) -> Self {
173        Self { x, y, z, w }
174    }
175
176    /// The identity rotation.
177    #[must_use]
178    pub const fn identity() -> Self {
179        Self::IDENTITY
180    }
181
182    /// The quaternion's norm.
183    #[must_use]
184    pub fn norm(self) -> f64 {
185        self.x.hypot(self.y).hypot(self.z).hypot(self.w)
186    }
187
188    /// Whether the quaternion is a unit rotation within `tolerance`.
189    #[must_use]
190    pub fn is_normalized(self, tolerance: f64) -> bool {
191        (self.norm() - 1.0).abs() <= tolerance
192    }
193
194    /// The quaternion scaled to unit length, or [`Quaternion::IDENTITY`] when
195    /// it has no length to scale.
196    #[must_use]
197    pub fn normalized(self) -> Self {
198        let norm = self.norm();
199        if norm == 0.0 || !norm.is_finite() {
200            return Self::IDENTITY;
201        }
202        Self::new(self.x / norm, self.y / norm, self.z / norm, self.w / norm)
203    }
204
205    /// The components as an array, in `xyzw` order.
206    #[must_use]
207    pub const fn to_array(self) -> [f64; 4] {
208        [self.x, self.y, self.z, self.w]
209    }
210
211    /// The columnar layout of this type.
212    #[must_use]
213    pub fn layout() -> DataType {
214        DataType::strukt([
215            Field::required("x", DataType::Float64),
216            Field::required("y", DataType::Float64),
217            Field::required("z", DataType::Float64),
218            Field::required("w", DataType::Float64),
219        ])
220    }
221
222    /// Builds the child column for `values`.
223    ///
224    /// # Errors
225    ///
226    /// [`astrs_data::DataError`] when the columns cannot be assembled.
227    pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
228        let x: Vec<f64> = values.iter().map(|value| value.x).collect();
229        let y: Vec<f64> = values.iter().map(|value| value.y).collect();
230        let z: Vec<f64> = values.iter().map(|value| value.z).collect();
231        let w: Vec<f64> = values.iter().map(|value| value.w).collect();
232        build::structure(vec![
233            ("x", build::primitive::<f64>(&x)),
234            ("y", build::primitive::<f64>(&y)),
235            ("z", build::primitive::<f64>(&z)),
236            ("w", build::primitive::<f64>(&w)),
237        ])
238    }
239
240    /// Reads one row of a `Quaternion` column.
241    ///
242    /// # Errors
243    ///
244    /// [`astrs_data::DataError`] for a layout mismatch or an out-of-range row.
245    pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
246        let strukt = read::structure(column)?;
247        Ok(Self {
248            x: read::f64_at(read::field(strukt, "x")?, row)?,
249            y: read::f64_at(read::field(strukt, "y")?, row)?,
250            z: read::f64_at(read::field(strukt, "z")?, row)?,
251            w: read::f64_at(read::field(strukt, "w")?, row)?,
252        })
253    }
254}
255
256impl Default for Quaternion {
257    fn default() -> Self {
258        Self::IDENTITY
259    }
260}
261
262impl AstrsMessage for Quaternion {
263    const URN: &'static str = "std/geometry/v1/Quaternion";
264
265    fn data_type() -> DataType {
266        Self::layout()
267    }
268
269    fn to_record_batch(&self) -> Result<RecordBatch> {
270        Ok(RecordBatch::from_payload(Self::column(
271            core::slice::from_ref(self),
272        )?))
273    }
274
275    fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
276        Self::at(single_row_column(batch)?, 0)
277    }
278}
279
280super::impl_from_payload!(Quaternion);
281
282/// Writes a two-`Vector3`/`Quaternion` composite: `Pose`, `Transform`,
283/// `Twist` and `Accel` differ only in their field names and URN.
284macro_rules! composite {
285    (
286        $(#[$meta:meta])*
287        $name:ident, $urn:literal,
288        $first:ident : $first_ty:ty = $first_name:literal,
289        $second:ident : $second_ty:ty = $second_name:literal
290    ) => {
291        $(#[$meta])*
292        #[derive(Debug, Clone, Copy, Default, PartialEq, PartialOrd)]
293        pub struct $name {
294            #[doc = concat!("The `", $first_name, "` component.")]
295            pub $first: $first_ty,
296            #[doc = concat!("The `", $second_name, "` component.")]
297            pub $second: $second_ty,
298        }
299
300        impl $name {
301            /// A value from its two components.
302            #[must_use]
303            pub const fn new($first: $first_ty, $second: $second_ty) -> Self {
304                Self { $first, $second }
305            }
306
307            /// The columnar layout of this type.
308            #[must_use]
309            pub fn layout() -> DataType {
310                DataType::strukt([
311                    Field::required($first_name, <$first_ty>::layout()),
312                    Field::required($second_name, <$second_ty>::layout()),
313                ])
314            }
315
316            /// Builds the child column for `values`.
317            ///
318            /// # Errors
319            ///
320            /// [`astrs_data::DataError`] when the columns cannot be assembled.
321            pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
322                let first: Vec<$first_ty> = values.iter().map(|value| value.$first).collect();
323                let second: Vec<$second_ty> = values.iter().map(|value| value.$second).collect();
324                build::structure(vec![
325                    ($first_name, <$first_ty>::column(&first)?),
326                    ($second_name, <$second_ty>::column(&second)?),
327                ])
328            }
329
330            /// Reads one row of a column of this type.
331            ///
332            /// # Errors
333            ///
334            /// [`astrs_data::DataError`] for a layout mismatch or an
335            /// out-of-range row.
336            pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
337                let strukt = read::structure(column)?;
338                Ok(Self {
339                    $first: <$first_ty>::at(read::field(strukt, $first_name)?, row)?,
340                    $second: <$second_ty>::at(read::field(strukt, $second_name)?, row)?,
341                })
342            }
343        }
344
345        impl AstrsMessage for $name {
346            const URN: &'static str = $urn;
347
348            fn data_type() -> DataType {
349                Self::layout()
350            }
351
352            fn to_record_batch(&self) -> Result<RecordBatch> {
353                Ok(RecordBatch::from_payload(Self::column(
354                    core::slice::from_ref(self),
355                )?))
356            }
357
358            fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
359                Self::at(single_row_column(batch)?, 0)
360            }
361        }
362
363        super::impl_from_payload!($name);
364    };
365}
366
367composite!(
368    /// A position and an orientation — `std/geometry/v1/Pose`.
369    Pose,
370    "std/geometry/v1/Pose",
371    position: Vector3 = "position",
372    orientation: Quaternion = "orientation"
373);
374
375composite!(
376    /// A rigid-body transform — `std/geometry/v1/Transform`.
377    ///
378    /// The same shape as [`Pose`] under different names: a `Transform`
379    /// answers "how do I get from `frame` to `child_frame`", a `Pose`
380    /// answers "where is this, expressed in `frame`".
381    Transform,
382    "std/geometry/v1/Transform",
383    translation: Vector3 = "translation",
384    rotation: Quaternion = "rotation"
385);
386
387composite!(
388    /// A velocity — `std/geometry/v1/Twist`.
389    Twist,
390    "std/geometry/v1/Twist",
391    linear: Vector3 = "linear",
392    angular: Vector3 = "angular"
393);
394
395composite!(
396    /// An acceleration — `std/geometry/v1/Accel`.
397    ///
398    /// Byte-identical to [`Twist`]; only the URN distinguishes them.
399    Accel,
400    "std/geometry/v1/Accel",
401    linear: Vector3 = "linear",
402    angular: Vector3 = "angular"
403);
404
405#[cfg(test)]
406mod tests {
407    #![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
408
409    use super::*;
410    use crate::message::assert_registry_layout;
411
412    #[test]
413    fn every_geometry_type_conforms_to_the_registry() {
414        assert_registry_layout::<Vector3>().unwrap();
415        assert_registry_layout::<Quaternion>().unwrap();
416        assert_registry_layout::<Pose>().unwrap();
417        assert_registry_layout::<Transform>().unwrap();
418        assert_registry_layout::<Twist>().unwrap();
419        assert_registry_layout::<Accel>().unwrap();
420    }
421
422    #[test]
423    fn vectors_round_trip() {
424        let value = Vector3::new(1.5, -2.5, 3.5);
425        let batch = value.to_record_batch().unwrap();
426        assert_eq!(batch.num_rows(), 1);
427        assert_eq!(Vector3::from_record_batch(&batch).unwrap(), value);
428        assert_eq!(value.to_array(), [1.5, -2.5, 3.5]);
429        assert_eq!(Vector3::from([1.5, -2.5, 3.5]), value);
430        assert_eq!(Vector3::ZERO.norm(), 0.0);
431        assert_eq!(Vector3::new(3.0, 4.0, 0.0).norm(), 5.0);
432        assert_eq!(Vector3::default(), Vector3::ZERO);
433    }
434
435    #[test]
436    fn quaternions_round_trip_and_normalise() {
437        let half = core::f64::consts::FRAC_1_SQRT_2;
438        let value = Quaternion::new(0.0, 0.0, half, half);
439        let batch = value.to_record_batch().unwrap();
440        assert_eq!(Quaternion::from_record_batch(&batch).unwrap(), value);
441        assert!(value.is_normalized(1e-12));
442        assert_eq!(Quaternion::default(), Quaternion::IDENTITY);
443        assert_eq!(Quaternion::identity().to_array(), [0.0, 0.0, 0.0, 1.0]);
444
445        let scaled = Quaternion::new(0.0, 0.0, 0.0, 4.0);
446        assert!(!scaled.is_normalized(1e-9));
447        assert!(scaled.normalized().is_normalized(1e-12));
448
449        // A degenerate quaternion cannot be normalised; the identity is the
450        // only answer that stays a rotation.
451        let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
452        assert_eq!(zero.normalized(), Quaternion::IDENTITY);
453        let nan = Quaternion::new(f64::NAN, 0.0, 0.0, 0.0);
454        assert_eq!(nan.normalized(), Quaternion::IDENTITY);
455    }
456
457    #[test]
458    fn composites_round_trip() {
459        let pose = Pose::new(Vector3::new(1.0, 2.0, 3.0), Quaternion::identity());
460        assert_eq!(
461            Pose::from_record_batch(&pose.to_record_batch().unwrap()).unwrap(),
462            pose
463        );
464
465        let transform = Transform::new(Vector3::new(-1.0, 0.0, 0.5), Quaternion::identity());
466        assert_eq!(
467            Transform::from_record_batch(&transform.to_record_batch().unwrap()).unwrap(),
468            transform
469        );
470
471        let twist = Twist::new(Vector3::new(1.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.2));
472        assert_eq!(
473            Twist::from_record_batch(&twist.to_record_batch().unwrap()).unwrap(),
474            twist
475        );
476
477        let accel = Accel::new(Vector3::new(0.0, 0.0, -9.81), Vector3::ZERO);
478        assert_eq!(
479            Accel::from_record_batch(&accel.to_record_batch().unwrap()).unwrap(),
480            accel
481        );
482    }
483
484    #[test]
485    fn twist_and_accel_share_a_layout_but_not_a_urn() {
486        assert_eq!(Twist::data_type(), Accel::data_type());
487        assert_ne!(<Twist as AstrsMessage>::URN, <Accel as AstrsMessage>::URN);
488    }
489
490    #[test]
491    fn multi_row_columns_hold_every_value() {
492        let poses = [
493            Pose::new(Vector3::new(0.0, 0.0, 0.0), Quaternion::identity()),
494            Pose::new(Vector3::new(1.0, 1.0, 1.0), Quaternion::identity()),
495            Pose::new(Vector3::new(2.0, 4.0, 8.0), Quaternion::identity()),
496        ];
497        let column = Pose::column(&poses).unwrap();
498        assert_eq!(column.len(), 3);
499        for (row, expected) in poses.iter().enumerate() {
500            assert_eq!(&Pose::at(&column, row).unwrap(), expected);
501        }
502        assert!(Pose::at(&column, 3).is_err());
503    }
504
505    #[test]
506    fn a_wrong_layout_is_refused_rather_than_reinterpreted() {
507        let vector = Vector3::new(1.0, 2.0, 3.0).to_record_batch().unwrap();
508        assert!(Quaternion::from_record_batch(&vector).is_err());
509        assert!(Pose::from_record_batch(&vector).is_err());
510
511        let scalar = crate::message::Scalar::from(1.0_f64)
512            .to_record_batch()
513            .unwrap();
514        assert!(Vector3::from_record_batch(&scalar).is_err());
515    }
516}