From 54d872c34dcce58e7301444a2238dc9eb6a2b2f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Mon, 9 Nov 2026 17:20:10 +0900 Subject: [PATCH 1/6] feat(autoware): add PointCloud struct --- applications/autoware/common_types/README.md | 176 +++++ applications/autoware/common_types/src/lib.rs | 1 + .../autoware/common_types/src/point.rs | 295 +++++++++ .../common_types/src/point/accessor.rs | 498 ++++++++++++++ .../autoware/common_types/src/point/cloud.rs | 618 ++++++++++++++++++ .../autoware/common_types/src/point/field.rs | 587 +++++++++++++++++ .../common_types/src/point/selection.rs | 517 +++++++++++++++ .../common_types/src/point/test_support.rs | 53 ++ 8 files changed, 2745 insertions(+) create mode 100644 applications/autoware/common_types/README.md create mode 100644 applications/autoware/common_types/src/point.rs create mode 100644 applications/autoware/common_types/src/point/accessor.rs create mode 100644 applications/autoware/common_types/src/point/cloud.rs create mode 100644 applications/autoware/common_types/src/point/field.rs create mode 100644 applications/autoware/common_types/src/point/selection.rs create mode 100644 applications/autoware/common_types/src/point/test_support.rs diff --git a/applications/autoware/common_types/README.md b/applications/autoware/common_types/README.md new file mode 100644 index 000000000..0a71b213f --- /dev/null +++ b/applications/autoware/common_types/README.md @@ -0,0 +1,176 @@ +# Autoware common types + +This `no_std` crate defines shared Autoware message, geometry, transform, and +point-cloud access types. It uses `alloc`. + +## Data types + +### Messages and geometry + +| Type | Purpose | +| --- | --- | +| `Header` | A `frame_id` and `timestamp` attached to a message. | +| `PointCloud` | A public, ROS-style binary point-cloud message: header, dimensions, field descriptors, byte order, point and row strides, data, and density flag. Its public fields are not validated on construction. | +| `PointField` | A message descriptor for one named field: its byte offset, `FieldDatatype`, and number of consecutive values per point. Offsets and counts are `u32`. | +| `FieldDatatype` | The runtime encoding of a field: signed or unsigned 8-, 16-, or 32-bit integers, `f32`, or `f64`. | +| `PointFieldValue` | A sealed trait implemented by those eight Rust scalar types. It provides their datatype, size, and native-endian encoding and decoding. | +| `Point32` | A `Vector3` used as a polygon vertex. | +| `Polygon` / `PolygonStamped` | A sequence of 3D vertices, optionally paired with a `Header`. | +| `Transform` | A rigid transform with `f64` translation and a unit quaternion. `try_new` validates finite inputs and a near-unit quaternion, then normalizes it. | +| `TransformStamped` | A transform accompanied by a header and child frame ID. | +| `TransformListener` / `MockTransformListener` | An interface for transform lookup and an in-memory implementation for tests or other callers. | + +`nalgebra::Quaternion::new` uses `(w, x, y, z)` order, unlike ROS's +`(x, y, z, w)` convention. `Transform::try_from_ros_xyzw` is a deprecated +compatibility helper; prefer explicit conversion at the I/O boundary. + +### Point-cloud access + +Point-cloud messages might not be well-formed and their fields could be corrupted. +To ensure a safe point-cloud manipulation, we provide **accessors** that uses Rust's typesystem to ensure a correct behavior. + + +The accessor types borrow a message and validate its layout before access. +They do not change message metadata while borrowed. + +| Type | Purpose | +| --- | --- | +| `PointCloudAccessor` | Owns resolved field metadata and cached dimensions and strides while borrowing a cloud. `PointCloudReader` and `PointCloudWriter` are aliases for shared and exclusive borrows. | +| `CheckedField` | Validated field layout without a statically selected Rust value type. | +| `PointAccessor<'a, D>` | A borrowed point and its checked metadata. `PointReader` and `PointWriter` expose shared or mutable bytes and named field access. | +| `FieldAccessor` | A borrowed field's complete byte slice and checked metadata. | +| `PointCloudError` / `PointFieldError` | Cloud layout and access errors; field-specific errors are propagated through `PointCloudError::InvalidField`. | + +```mermaid +classDiagram + class Header + class PointCloud + class PointField + class FieldDatatype + class PointCloudAccessor + class CheckedField + class PointAccessor + class FieldAccessor + + PointCloud *-- Header + PointCloud *-- PointField + PointField --> FieldDatatype + PointCloudAccessor --> PointCloud : borrows + PointCloudAccessor *-- CheckedField : caches + PointCloudAccessor --> PointAccessor : point access + PointAccessor --> FieldAccessor : field access + FieldAccessor --> CheckedField +``` + +#### Layout and selection rules + +Cloud construction requires native byte order, a nonzero `point_step`, +`row_step >= width * point_step`, and exactly `row_step * height` data bytes. +Row padding is allowed. Selected field ranges must fit inside a point; field +overlaps and zero-count fields are allowed. A point is accessed by row-major +index or by `(row, column)`; its bytes exclude row padding. + +* `PointCloudReader::try_new(&cloud, None)` resolves every field descriptor. +* `Some(&[(name, datatype), ...])` resolves only the requested fields, in + request order. The first descriptor with each name is used; a missing name + or datatype mismatch is an error. Unselected descriptors are not validated. +* `Some(&[])` resolves no fields but still validates the cloud layout and + permits access to raw point bytes. + +### Typed point-cloud access + +Typed access uses the same validated layouts while retaining the selected Rust +types. A raw field can also be converted individually with `into_typed::()`, +which checks its datatype. + +| Type | Purpose | +| --- | --- | +| `Field` / `FieldSelection` | A named typed field request and the sealed tuple-selection interface. Tuples contain one to eight fields and can mix scalar types. Every selected field retains all of its values. | +| `TypedCheckedField` | Checked field metadata plus a type witness for `T`; used internally by typed selections. It dereferences to `CheckedField`. | +| `TypedPointCloudAccessor` | A cloud accessor with typed selection metadata `S`. `TypedPointCloudReader` and `TypedPointCloudWriter` are shared and exclusive aliases. | +| `TypedPointReader` / `TypedPointWriter` | A point with typed tuple access. A writer supports typed reads and ordered tuple writes. | +| `TypedFieldAccessor` | A typed field view supporting scalar reads, iteration, and, for mutable bytes, writes. Values are decoded from native-endian bytes rather than borrowed as aligned `T` references. | +| `PointFieldValueMut` | A mutable proxy for one possibly unaligned value, returned when iterating over a mutable typed field. | + +```mermaid +classDiagram + class Field + class FieldSelection + class PointFieldValue + class PointCloudAccessor + class TypedPointCloudAccessor + class CheckedField + class TypedCheckedField + class PointAccessor + class TypedPointReader + class TypedPointWriter + class FieldAccessor + class TypedFieldAccessor + class PointFieldValueMut + + Field --> PointFieldValue : selects scalar type + FieldSelection --> Field : tuple of requests + TypedPointCloudAccessor *-- PointCloudAccessor + TypedPointCloudAccessor --> FieldSelection + TypedPointCloudAccessor *-- TypedCheckedField : selected metadata + TypedCheckedField --> CheckedField : dereferences + TypedPointCloudAccessor --> TypedPointReader + TypedPointCloudAccessor --> TypedPointWriter + TypedPointReader *-- PointAccessor + TypedPointWriter *-- PointAccessor + TypedPointReader --> TypedFieldAccessor : typed fields + TypedPointWriter --> TypedFieldAccessor : typed reads + FieldAccessor --> TypedFieldAccessor : into_typed + TypedFieldAccessor *-- FieldAccessor + TypedFieldAccessor --> PointFieldValueMut : mutable values +``` + +#### Typed selection and writes + +* `try_new_typed` takes a tuple of `Field::::new(name)` requests and checks + the names and types at construction. Point access then returns typed handles + without repeating name or datatype lookups. Bounds and byte ranges are still + checked when accessing values. + +For tuple writes, `write_values(index, values)` writes the same element index +in each selected field; `write_fields(slices)` writes all values and requires +each slice length to match its field count. Both validate all requested writes +before mutation. Writes occur left to right, so a later tuple element wins if +fields overlap or a field is selected more than once. + +```rust +use common_types::point::{Field, PointCloud, PointCloudError, PointCloudReader, PointCloudWriter}; + +fn read_xyz(cloud: &PointCloud) -> Result<(f32, f32, f32), PointCloudError> { + let reader = PointCloudReader::try_new_typed( + cloud, + (Field::::new("x"), Field::::new("y"), Field::::new("z")), + )?; + let (x, y, z) = reader.point(0)?.fields(); + Ok((x.value(0)?, y.value(0)?, z.value(0)?)) +} + +fn write_xyz(cloud: &mut PointCloud) -> Result<(), PointCloudError> { + let mut writer = PointCloudWriter::try_new_typed( + cloud, + (Field::::new("x"), Field::::new("y"), Field::::new("z")), + )?; + writer.point_mut(0)?.write_values(0, (1.0, 2.0, 3.0)) +} +``` + +## Validation + +From the repository root: + +```sh +cargo build -p common_types --all-targets --all-features +cargo test -p common_types --all-features +cargo clippy -p common_types --all-targets --all-features -- -D warnings +``` + +Tests cover layout and typed access, ordered overlapping writes, empty clouds, +transforms, metadata, extra values, unaligned fields, and padded input rows. + +See the [crop box filter](../crop_box_filter/README.md) for a consumer of these +types. diff --git a/applications/autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs index 08779e99c..737a518f7 100644 --- a/applications/autoware/common_types/src/lib.rs +++ b/applications/autoware/common_types/src/lib.rs @@ -17,6 +17,7 @@ extern crate alloc; pub mod geometry; +pub mod point; pub mod transform; #[derive(Debug, Clone)] diff --git a/applications/autoware/common_types/src/point.rs b/applications/autoware/common_types/src/point.rs new file mode 100644 index 000000000..48ab30571 --- /dev/null +++ b/applications/autoware/common_types/src/point.rs @@ -0,0 +1,295 @@ +mod accessor; +mod cloud; +mod field; +mod selection; +#[cfg(test)] +mod test_support; + +pub use accessor::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}; +pub use cloud::{ + PointCloudAccessor, PointCloudReader, PointCloudWriter, TypedPointCloudAccessor, + TypedPointCloudReader, TypedPointCloudWriter, +}; +pub use field::{ + CheckedField, FieldAccessor, PointFieldValueMut, TypedCheckedField, TypedFieldAccessor, +}; +pub use selection::{Field, FieldSelection}; + +use crate::Header; +use alloc::vec::Vec; + +/// Represents the datatype of a point field in a point cloud. +#[repr(u8)] +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum FieldDatatype { + Int8 = 1, + Uint8 = 2, + Int16 = 3, + Uint16 = 4, + Int32 = 5, + Uint32 = 6, + Float32 = 7, + Float64 = 8, +} + +impl FieldDatatype { + pub const fn size(self) -> u32 { + match self { + Self::Int8 | Self::Uint8 => 1, + Self::Int16 | Self::Uint16 => 2, + Self::Int32 | Self::Uint32 | Self::Float32 => 4, + Self::Float64 => 8, + } + } +} + +/// Reads and writes scalar values stored in a [`PointField`]. +pub trait PointFieldValue: Copy + sealed::Sealed { + /// The matching `sensor_msgs::msg::PointField` datatype. + const DATATYPE: FieldDatatype; + /// Size of the value in bytes. + const SIZE: usize; + /// Encoded native-endian bytes. + type Bytes: AsRef<[u8]>; + + /// Decodes a native-endian value; returns an error if `bytes.len() != SIZE`. + /// + /// # Errors + /// + /// Returns [`PointFieldError::InvalidLength`] for a non-scalar byte length. + fn from_ne_slice(bytes: &[u8]) -> Result; + fn to_ne_bytes(self) -> Self::Bytes; +} + +macro_rules! impl_point_field_value { + ($($ty:ty => $datatype:ident),* $(,)?) => {$( + impl sealed::Sealed for $ty {} + + impl PointFieldValue for $ty { + const DATATYPE: FieldDatatype = FieldDatatype::$datatype; + const SIZE: usize = core::mem::size_of::<$ty>(); + type Bytes = [u8; core::mem::size_of::<$ty>()]; + + fn from_ne_slice(bytes: &[u8]) -> Result { + bytes.try_into().map(<$ty>::from_ne_bytes).map_err(|_| PointFieldError::InvalidLength) + } + + fn to_ne_bytes(self) -> Self::Bytes { + <$ty>::to_ne_bytes(self) + } + } + )*}; +} + +impl_point_field_value! { + i8 => Int8, + u8 => Uint8, + i16 => Int16, + u16 => Uint16, + i32 => Int32, + u32 => Uint32, + f32 => Float32, + f64 => Float64, +} + +/// Describes a single field (channel) in a point cloud. +/// +/// # Note +/// +/// The `PointField` struct may contain an invalid layout, such as overlapping fields or incorrect offsets. +/// Use [`CheckedField`] for validated metadata without a statically +/// selected Rust value type, or [`TypedCheckedField`] for typed metadata proof. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct PointField { + /// Common `PointField` names are x, y, z, intensity, rgb, rgba. + pub name: &'static str, + /// Offset from the start of the point struct. + pub offset: u32, + /// Datatype of the field. + pub datatype: FieldDatatype, + /// How many elements in the field. + pub count: u32, +} + +impl PointField { + /// Creates a new `PointField` with the given name, offset, datatype, and count. + pub const fn new(name: &'static str, offset: u32, datatype: FieldDatatype, count: u32) -> Self { + Self { + name, + offset, + datatype, + count, + } + } +} + +/// This message holds a collection of N-dimensional points, which may contain additional +/// information such as normals, intensity, etc. The point data is stored as a binary blob, +/// its layout described by the contents of the "fields" array. +/// +/// # Note +/// +/// This is a plain message: all fields are public and may contain an invalid layout +/// or either byte order. Validated access requires native-endian data. +/// Borrow it through [`PointCloudReader`] or [`PointCloudWriter`] before access. +pub struct PointCloud { + /// Time of sensor data acquisition, and the coordinate frame ID (for 3D points). + pub header: Header, + /// Height of the point cloud. If the cloud is unordered, height is 1. + pub height: u32, + /// Width of the point cloud. If the cloud is unordered, width is the length of the point cloud. + pub width: u32, + /// Describes the channels and their layout in the binary data blob. + pub fields: Vec, + /// Is the data big-endian? + /// + /// Readers and writers require this to equal `cfg!(target_endian = "big")`, + /// even for empty clouds or selections; they do not swap bytes. + pub is_bigendian: bool, + /// Length of a point in bytes. + pub point_step: u32, + /// Length of a row in bytes. + pub row_step: u32, + /// Actual point data, size is `row_step * height` bytes. + pub data: Vec, + /// True if there are no invalid points. + pub is_dense: bool, +} + +mod sealed { + pub trait Sealed {} +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn point_field_value_encodes_and_decodes_values() { + let encoded = ::to_ne_bytes(3.5); + assert_eq!( + ::from_ne_slice(encoded.as_ref()).unwrap(), + 3.5 + ); + assert_eq!(::DATATYPE, FieldDatatype::Float32); + assert_eq!(::SIZE, 4); + } +} + +/// Errors that can occur when working with point fields. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[non_exhaustive] +pub enum PointFieldError { + InvalidLength, + FieldNotFound(&'static str), + DatatypeMismatch { + name: &'static str, + expected: FieldDatatype, + actual: FieldDatatype, + }, + FieldOutOfBounds { + name: &'static str, + offset_start: usize, + offset_end: usize, + point_step: usize, + }, + ValueOutOfBounds { + name: &'static str, + index: usize, + count: usize, + }, + Overflow, +} + +impl core::fmt::Display for PointFieldError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::InvalidLength => write!(f, "Invalid length for point field"), + Self::FieldNotFound(name) => write!(f, "Field {name} not found"), + Self::DatatypeMismatch { + name, + expected, + actual, + } => write!( + f, + "Field {name} has datatype {actual:?}, expected {expected:?}" + ), + Self::FieldOutOfBounds { + name, + offset_start, + offset_end, + point_step, + } => write!( + f, + "Field {name} at offset range {offset_start}-{offset_end} exceeds the point step {point_step}" + ), + Self::ValueOutOfBounds { name, index, count } => write!( + f, + "Value {index} of field {name} is outside its {count} values" + ), + Self::Overflow => write!(f, "Point field offset overflows usize"), + } + } +} + +impl core::error::Error for PointFieldError {} + +/// Errors that can occur when constructing point clouds or accessing their points. +#[derive(Debug, Clone, PartialEq, Eq)] +#[non_exhaustive] +pub enum PointCloudError { + UnsupportedEndianness, + ZeroPointStep, + InvalidRowStep { step: usize, min: usize }, + InvalidField(PointFieldError), + InvalidDataSize { size: usize, expected: usize }, + RowOutOfBounds { index: usize, height: usize }, + PointOutOfBounds { index: usize, count: usize }, + ColumnOutOfBounds { index: usize, width: usize }, + InvalidData, + Overflow, +} + +impl core::fmt::Display for PointCloudError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::UnsupportedEndianness => write!(f, "Point cloud endianness is not native"), + Self::ZeroPointStep => write!(f, "Point step is zero"), + Self::InvalidRowStep { step, min } => write!( + f, + "Row step {step} is smaller than the minimum row size {min}" + ), + Self::InvalidField(error) => write!(f, "Invalid point field: {error}"), + Self::InvalidDataSize { size, expected } => write!( + f, + "Data size {size} does not match expected size {expected}", + ), + Self::Overflow => write!(f, "Point cloud size overflows usize"), + Self::RowOutOfBounds { index, height } => { + write!(f, "Row {index} is outside cloud height {height}") + } + Self::PointOutOfBounds { index, count } => { + write!(f, "Point {index} is outside cloud of {count} points") + } + Self::ColumnOutOfBounds { index, width } => { + write!(f, "Column {index} is outside cloud width {width}") + } + Self::InvalidData => write!(f, "Point cloud data does not match its layout"), + } + } +} + +impl core::error::Error for PointCloudError { + fn source(&self) -> Option<&(dyn core::error::Error + 'static)> { + match self { + Self::InvalidField(error) => Some(error), + _ => None, + } + } +} + +impl From for PointCloudError { + fn from(error: PointFieldError) -> Self { + Self::InvalidField(error) + } +} diff --git a/applications/autoware/common_types/src/point/accessor.rs b/applications/autoware/common_types/src/point/accessor.rs new file mode 100644 index 000000000..e4556f663 --- /dev/null +++ b/applications/autoware/common_types/src/point/accessor.rs @@ -0,0 +1,498 @@ +use super::{ + field::{CheckedField, FieldAccessor}, + selection::FieldSelection, + PointCloudError, PointFieldError, +}; + +/// Read-only access to one point, retaining every field and value. +pub type PointReader<'a> = PointAccessor<'a, &'a [u8]>; + +/// Writable access to one point, retaining every field and value. +pub type PointWriter<'a> = PointAccessor<'a, &'a mut [u8]>; + +/// A borrowed point accessor with eagerly validated field byte ranges. +/// +/// Construction borrows the reader's metadata without allocating. Duplicate names resolve +/// to the first field. Overlapping fields are allowed; writes can affect both fields. +pub struct PointAccessor<'a, D> { + pub(super) bytes: D, + pub(super) fields: &'a [CheckedField], +} + +impl<'a, D: AsRef<[u8]>> PointAccessor<'a, D> { + /// Helper function to find a checked field info by name. + fn field_info(&self, name: &'static str) -> Result<&'a CheckedField, PointCloudError> { + self.fields + .iter() + .find(|field| field.name() == name) + .ok_or(PointFieldError::FieldNotFound(name).into()) + } + + /// Returns the packed bytes of this point, excluding any row padding. + #[inline] + pub fn bytes(&self) -> &[u8] { + self.bytes.as_ref() + } + + /// Borrows a field's complete raw bytes without copying. + pub fn field<'b>( + &'b self, + name: &'static str, + ) -> Result, PointCloudError> { + let resolved = self.field_info(name)?; + let bytes = self + .bytes() + .get(resolved.range()) + .ok_or(PointCloudError::InvalidData)?; + Ok(FieldAccessor { + field: resolved, + bytes, + }) + } +} + +impl<'a, D: AsRef<[u8]> + AsMut<[u8]>> PointAccessor<'a, D> { + /// Returns writable bytes for this point, excluding any row padding. + pub fn bytes_mut(&mut self) -> &mut [u8] { + self.bytes.as_mut() + } + + /// Exclusively borrows a resolved field's complete writable raw bytes. + pub fn field_mut<'b>( + &'b mut self, + name: &'static str, + ) -> Result, PointCloudError> { + let resolved = self.field_info(name)?; + let range = resolved.range(); + let bytes = self + .bytes + .as_mut() + .get_mut(range) + .ok_or(PointCloudError::InvalidData)?; + Ok(FieldAccessor { + field: resolved, + bytes, + }) + } +} + +/// One borrowed point with a typed read tuple and complete raw point access. +pub struct TypedPointReader<'a, S: FieldSelection> { + pub(super) raw: PointReader<'a>, + pub(super) fields: S::Fields<'a>, +} + +impl<'a, S: FieldSelection> TypedPointReader<'a, S> { + /// Returns typed handles in selection order, retaining every field value. + #[inline] + pub fn fields(&self) -> S::Fields<'a> { + self.fields + } + + /// Returns the raw bytes of this point, excluding any row padding. + #[inline] + pub fn bytes(&self) -> &[u8] { + self.raw.bytes() + } + + /// Borrows a field's complete raw bytes without copying. + #[inline] + pub fn field(&self, name: &'static str) -> Result, PointCloudError> { + self.raw.field(name) + } +} + +/// One mutably borrowed point with typed reads and validated tuple writes. +pub struct TypedPointWriter<'a, S: FieldSelection> { + pub(super) raw: PointWriter<'a>, + pub(super) checked: &'a S::Checked, +} + +impl TypedPointWriter<'_, S> { + /// Returns typed read handles in the original selection order. + #[inline] + pub fn fields(&self) -> Result, PointCloudError> { + S::access(self.checked, self.raw.bytes) + } + + #[inline] + /// Returns the raw bytes of this point, excluding any row padding. + pub fn bytes(&self) -> &[u8] { + self.raw.bytes() + } + + /// Writes the selected value at the same element index in every field. + /// + /// All selected indices and byte ranges are validated before the first + /// write. Overlapping and repeated selections are applied left to right, + /// so the last tuple element affecting a byte wins. + #[inline] + pub fn write_values( + &mut self, + element_index: usize, + values: S::Values, + ) -> Result<(), PointCloudError> { + S::write_values(self.checked, self.raw.bytes, element_index, values) + } + + /// Writes each selected field from a slice containing all values for this point. + /// + /// Each slice length must equal that field's `count()`. All lengths and byte + /// ranges are validated before mutation. Complete fields are written in tuple + /// selection order, so overlapping or repeated selections are last-wins. + /// + /// ``` + /// use common_types::point::{ + /// Field, PointCloud, PointCloudError, PointCloudWriter, + /// }; + /// + /// fn set_first_point(cloud: &mut PointCloud) -> Result<(), PointCloudError> { + /// let mut writer = PointCloudWriter::try_new_typed( + /// cloud, + /// (Field::::new("bytes"), Field::::new("words")), + /// )?; + /// let byte_values: &[u8] = &[1, 2]; + /// let word_values: &[u16] = &[0x1234]; + /// writer.point_mut(0)?.write_fields((byte_values, word_values))?; + /// Ok(()) + /// } + /// ``` + pub fn write_fields(&mut self, values: S::Slices<'_>) -> Result<(), PointCloudError> { + S::write_fields(self.checked, &values, self.raw.bytes) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::point::selection::typed_selection; + use crate::point::test_support::{message, projected_cloud}; + use crate::point::*; + use alloc::{vec, vec::Vec}; + + #[test] + fn malformed_field_ranges_fail_without_mutating_point_bytes() { + let descriptor = PointField::new("values", 1, FieldDatatype::Uint16, 2); + let resolved = [CheckedField::try_new(descriptor, 5).unwrap()]; + let mut bytes = [0xAA; 4]; + { + let mut point = PointAccessor { + bytes: &mut bytes[..], + fields: &resolved, + }; + assert_eq!( + point.field("values").err(), + Some(PointCloudError::InvalidData) + ); + assert_eq!( + point.field_mut("values").err(), + Some(PointCloudError::InvalidData) + ); + assert_eq!( + point + .field_mut("values") + .and_then(|field| field.into_typed::()) + .and_then(|mut field| field.write_value(0, 42)), + Err(PointCloudError::InvalidData) + ); + } + assert_eq!(bytes, [0xAA; 4]); + } + #[test] + fn point_handles_reuse_resolved_metadata() { + let mut cloud = projected_cloud(); + let reader = PointCloudWriter::try_new( + &mut cloud, + Some(&[ + ("label", FieldDatatype::Uint8), + ("vector", FieldDatatype::Uint16), + ]), + ) + .unwrap(); + let resolved = reader.fields().nth(1).unwrap(); + for index in 0..reader.point_count() { + let point = reader.point(index).unwrap(); + assert!(core::ptr::eq(point.field_info("vector").unwrap(), resolved)); + let raw = point.field("vector").unwrap(); + assert!(core::ptr::eq(&*raw, resolved)); + assert!(core::ptr::eq( + raw.into_typed::().unwrap().raw.field, + resolved + )); + } + } + #[test] + fn typed_point_write_fields_prevalidates_malformed_final_range() { + let checked_fields = [ + CheckedField::try_new(PointField::new("first", 0, FieldDatatype::Uint8, 1), 4).unwrap(), + CheckedField::try_new(PointField::new("last", 2, FieldDatatype::Uint16, 1), 4).unwrap(), + ]; + let mut field_iter = checked_fields.iter(); + let checked = + <(Field, Field) as typed_selection::Selection>::resolve(&mut field_iter) + .unwrap(); + let mut bytes = [0xAA; 3]; + { + let raw = PointAccessor { + bytes: &mut bytes[..], + fields: &checked_fields, + }; + let mut writer: TypedPointWriter<'_, (Field, Field)> = TypedPointWriter { + raw, + checked: &checked, + }; + assert_eq!( + writer.write_fields((&[0x42], &[0x1234])), + Err(PointCloudError::InvalidData) + ); + } + assert_eq!(bytes, [0xAA; 3]); + } + #[test] + fn typed_point_write_fields_accepts_empty_whole_field_slices() { + let mut cloud = message( + 1, + 1, + 4, + 4, + vec![ + PointField::new("value", 0, FieldDatatype::Uint8, 1), + PointField::new("empty", 4, FieldDatatype::Uint16, 0), + ], + vec![0xAA; 4], + ); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + (Field::::new("value"), Field::::new("empty")), + ) + .unwrap(); + + writer + .point_mut(0) + .unwrap() + .write_fields((&[0x42], &[])) + .unwrap(); + + assert_eq!(writer.data(), &[0x42, 0xAA, 0xAA, 0xAA]); + } + #[test] + fn typed_point_write_fields_apply_complete_overlapping_fields_in_tuple_order() { + let mut cloud = message( + 1, + 1, + 3, + 3, + vec![ + PointField::new("first", 0, FieldDatatype::Uint8, 2), + PointField::new("shifted", 1, FieldDatatype::Uint8, 2), + ], + vec![0xAA; 3], + ); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + (Field::::new("first"), Field::::new("shifted")), + ) + .unwrap(); + + writer + .point_mut(0) + .unwrap() + .write_fields((&[1, 2], &[9, 8])) + .unwrap(); + + assert_eq!(writer.data(), &[1, 9, 8]); + } + #[test] + fn typed_point_write_fields_checks_lengths_and_orders_repeated_fields() { + let mut cloud = message( + 1, + 1, + 4, + 4, + vec![ + PointField::new("wide", 0, FieldDatatype::Uint8, 2), + PointField::new("alias", 1, FieldDatatype::Uint8, 1), + ], + vec![0xAA; 4], + ); + let original = cloud.data.clone(); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + ( + Field::::new("wide"), + Field::::new("alias"), + Field::::new("wide"), + ), + ) + .unwrap(); + assert_eq!( + writer + .point_mut(0) + .unwrap() + .write_fields((&[1, 2], &[9], &[3])), + Err(PointCloudError::InvalidData) + ); + assert_eq!(writer.data(), original); + writer + .point_mut(0) + .unwrap() + .write_fields((&[1, 2], &[9], &[3, 4])) + .unwrap(); + assert_eq!(writer.data(), &[3, 4, 0xAA, 0xAA]); + } + #[test] + fn typed_point_writer_writes_mixed_fields_without_touching_padding_or_neighbors() { + let mut cloud = message( + 2, + 2, + 8, + 18, + vec![ + PointField::new("byte_values", 1, FieldDatatype::Uint8, 2), + PointField::new("word_values", 3, FieldDatatype::Uint16, 2), + ], + vec![0xAA; 36], + ); + let original = cloud.data.clone(); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + ( + Field::::new("byte_values"), + Field::::new("word_values"), + ), + ) + .unwrap(); + writer + .point_at_mut(1, 1) + .unwrap() + .write_values(1, (0x42, 0x1234)) + .unwrap(); + let mut expected = original.clone(); + expected[28] = 0x42; + expected[31..33].copy_from_slice(&0x1234_u16.to_ne_bytes()); + assert_eq!(writer.data(), expected); + + assert_eq!( + writer + .point_mut(3) + .unwrap() + .write_fields((&[0x41, 0x42], &[0x1233])), + Err(PointCloudError::InvalidData) + ); + assert_eq!(writer.data(), expected); + writer + .point_mut(3) + .unwrap() + .write_fields((&[0x41, 0x42], &[0x1233, 0x1234])) + .unwrap(); + + { + let point = writer.point_mut(3).unwrap(); + let (byte_values, word_values) = point.fields().unwrap(); + assert_eq!( + byte_values.values().collect::, _>>(), + Ok(vec![0x41, 0x42]) + ); + assert_eq!( + word_values.values().collect::, _>>(), + Ok(vec![0x1233, 0x1234]) + ); + } + + let mut expected = original; + expected[27] = 0x41; + expected[28] = 0x42; + expected[29..31].copy_from_slice(&0x1233_u16.to_ne_bytes()); + expected[31..33].copy_from_slice(&0x1234_u16.to_ne_bytes()); + assert_eq!(writer.data(), expected); + assert_eq!(writer.data().get(16..18), Some(&[0xAA; 2][..])); + assert_eq!(writer.data().get(34..36), Some(&[0xAA; 2][..])); + } + #[test] + fn typed_writer_validates_every_value_before_mutating_and_orders_overlaps() { + let mut cloud = message( + 1, + 1, + 4, + 4, + vec![ + PointField::new("first", 0, FieldDatatype::Uint8, 2), + PointField::new("last", 2, FieldDatatype::Uint16, 1), + ], + vec![1, 2, 3, 4], + ); + let original = cloud.data.clone(); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + (Field::::new("first"), Field::::new("last")), + ) + .unwrap(); + assert!(matches!( + writer.point_mut(0).unwrap().write_values(1, (9, 0x1234)), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "last", + index: 1, + count: 1, + } + )) + )); + assert_eq!(writer.data(), original); + + let mut empty = message( + 1, + 1, + 3, + 3, + vec![ + PointField::new("first", 0, FieldDatatype::Uint8, 1), + PointField::new("empty", 1, FieldDatatype::Uint16, 0), + ], + vec![4, 5, 6], + ); + let original = empty.data.clone(); + let mut writer = PointCloudWriter::try_new_typed( + &mut empty, + (Field::::new("first"), Field::::new("empty")), + ) + .unwrap(); + assert!(matches!( + writer.point_mut(0).unwrap().write_values(0, (9, 10)), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "empty", + index: 0, + count: 0, + } + )) + )); + assert_eq!(writer.data(), original); + + let mut overlap = message( + 1, + 1, + 2, + 2, + vec![ + PointField::new("wide", 0, FieldDatatype::Uint16, 1), + PointField::new("byte", 1, FieldDatatype::Uint8, 1), + ], + vec![0, 0], + ); + let mut writer = PointCloudWriter::try_new_typed( + &mut overlap, + ( + Field::::new("wide"), + Field::::new("byte"), + Field::::new("wide"), + ), + ) + .unwrap(); + writer + .point_mut(0) + .unwrap() + .write_values(0, (0x1111, 0x22, 0x3344)) + .unwrap(); + assert_eq!(writer.data(), &0x3344_u16.to_ne_bytes()); + } +} diff --git a/applications/autoware/common_types/src/point/cloud.rs b/applications/autoware/common_types/src/point/cloud.rs new file mode 100644 index 000000000..365bf139e --- /dev/null +++ b/applications/autoware/common_types/src/point/cloud.rs @@ -0,0 +1,618 @@ +use alloc::vec::Vec; +use core::{ + borrow::{Borrow, BorrowMut}, + ops::{Deref, DerefMut}, +}; + +use super::{ + accessor::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}, + field::CheckedField, + selection::FieldSelection, + FieldDatatype, PointCloud, PointCloudError, PointFieldError, +}; + +/// Validated read-only borrowing of a public message. +pub type PointCloudReader<'a> = PointCloudAccessor<&'a PointCloud>; + +/// Validated exclusive borrowing; only point bytes, never metadata, are writable. +pub type PointCloudWriter<'a> = PointCloudAccessor<&'a mut PointCloud>; + +/// Owns resolved metadata for the selected fields. +/// +/// The message remains borrowed for the lifetime of this object. The reader +/// does not expose mutable message metadata, so cached validation cannot be +/// invalidated. Overlapping fields are permitted, with exclusive reborrows +/// for writes. Only native-endian messages are accepted; typed conversions use +/// native endianness, not typed refs, and never swap bytes. +/// +/// Pass `None` to resolve every field or `Some(&[(name, datatype), ...])` to +/// resolve only those fields and validate their types, in the requested order. +/// Points are accessed individually with +/// [`Self::point`] or [`Self::point_at`]: +/// +/// ``` +/// use common_types::point::{FieldDatatype, PointCloud, PointCloudError, PointCloudWriter}; +/// +/// fn update_first_normal(cloud: &mut PointCloud) -> Result<(), PointCloudError> { +/// let mut writer = PointCloudWriter::try_new( +/// cloud, Some(&[("normal", FieldDatatype::Float32)]) +/// )?; +/// let mut point = writer.point_mut(0)?; // get first point of the cloud +/// let mut field = point.field_mut("normal")?; // get the "normal" field +/// let mut field = field.into_typed::()?; // convert to typed field +/// field.write_value(0, 1.0_f32)?; // write the value 1.0 to the first element of the field +/// Ok(()) +/// } +/// ``` +pub struct PointCloudAccessor { + cloud: C, + point_step: usize, + row_step: usize, + width: usize, + height: usize, + point_count: usize, + fields: Vec, +} + +impl> PointCloudAccessor { + /// Borrows a cloud after validating its row layout, data size, and selected fields. + /// + /// Requires `is_bigendian == cfg!(target_endian = "big")`. Unsupported byte + /// order returns [`PointCloudError::UnsupportedEndianness`] before layout or + /// field validation, including for empty clouds and empty selections. + /// + /// # Field Selection + /// + /// - `None` validates and resolves every field. + /// - `Some(fields)` validates only the first descriptor matching each requested + /// name, including its expected datatype, in request order. + /// - `Some(&[])` resolves no fields while retaining access to complete point bytes. + /// - Missing names and datatype mismatches are errors; later descriptors with + /// the same name are not considered, even if their datatype matches. + pub fn try_new( + cloud: C, + selections: Option<&[(&'static str, FieldDatatype)]>, + ) -> Result { + let message = cloud.borrow(); + + if message.is_bigendian != cfg!(target_endian = "big") { + return Err(PointCloudError::UnsupportedEndianness); + } + + let width = message.width as usize; + let height = message.height as usize; + let point_step = message.point_step as usize; + let row_step = message.row_step as usize; + + // Check for valid point step + if point_step == 0 { + return Err(PointCloudError::ZeroPointStep); + } + + // Compute useful derived values (checking for overflow) + let point_count = width.checked_mul(height).ok_or(PointCloudError::Overflow)?; + let min_row_step = width + .checked_mul(point_step) + .ok_or(PointCloudError::Overflow)?; + let expected_size = row_step + .checked_mul(height) + .ok_or(PointCloudError::Overflow)?; + + // Check for valid row step (row padding allowed) + if min_row_step > row_step { + return Err(PointCloudError::InvalidRowStep { + step: row_step, + min: min_row_step, + }); + } + // Check for valid data size (must match exactly the expected size) + if message.data.len() != expected_size { + return Err(PointCloudError::InvalidDataSize { + size: message.data.len(), + expected: expected_size, + }); + } + + let mut fields = Vec::new(); + match selections { + Some(selections) => { + for &(name, expected) in selections { + let field = message + .fields + .iter() + .find(|field| field.name == name) + .copied() + .ok_or(PointFieldError::FieldNotFound(name))?; + if field.datatype != expected { + return Err(PointFieldError::DatatypeMismatch { + name, + expected, + actual: field.datatype, + } + .into()); + } + fields.push(CheckedField::try_new(field, message.point_step)?); + } + } + None => { + for &field in &message.fields { + fields.push(CheckedField::try_new(field, message.point_step)?); + } + } + } + Ok(Self { + cloud, + point_step, + row_step, + width, + height, + point_count, + fields, + }) + } + + /// Selects a typed tuple, validating only the selected fields. + pub fn try_new_typed( + cloud: C, + selection: S, + ) -> Result, PointCloudError> { + let requests = selection.requests(); + let raw = Self::try_new(cloud, Some(requests.as_ref()))?; + let checked = S::resolve(&mut raw.fields.iter())?; + Ok(TypedPointCloudAccessor { raw, checked }) + } + + pub fn fields(&self) -> impl ExactSizeIterator + '_ { + self.fields.iter() + } + + #[inline] + pub fn data(&self) -> &[u8] { + &self.cloud.borrow().data + } + + /// Returns the number of points, excluding row padding. + #[must_use] + pub const fn point_count(&self) -> usize { + self.point_count + } + + /// Helper function to calculate the starting byte offset of a point given its row and column. + fn point_start(&self, row: usize, col: usize) -> Result { + if row >= self.height { + return Err(PointCloudError::RowOutOfBounds { + index: row, + height: self.height, + }); + } + if col >= self.width { + return Err(PointCloudError::ColumnOutOfBounds { + index: col, + width: self.width, + }); + } + let row_offset = row * self.row_step; + let column_offset = col * self.point_step; + + Ok(row_offset + column_offset) + } + + fn point_coordinates(&self, index: usize) -> Result<(usize, usize), PointCloudError> { + let width = self.width; + let count = self.point_count; + if index >= count { + return Err(PointCloudError::PointOutOfBounds { index, count }); + } + let row = index / width; + let column = index % width; + Ok((row, column)) + } + + /// Borrows one point by its row-major index, reusing cached field layouts. + pub fn point(&self, index: usize) -> Result, PointCloudError> { + let (row, col) = self.point_coordinates(index)?; + self.point_at(row, col) + } + + /// Borrows one point by its zero-based row and column. + pub fn point_at(&self, row: usize, col: usize) -> Result, PointCloudError> { + let start = self.point_start(row, col)?; + let end = start + self.point_step; + let bytes = self + .cloud + .borrow() + .data + .get(start..end) + .ok_or(PointCloudError::InvalidData)?; + Ok(PointAccessor { + bytes, + fields: &self.fields, + }) + } +} + +impl> PointCloudAccessor { + /// Mutably borrows one point by its row-major index. + pub fn point_mut(&mut self, index: usize) -> Result, PointCloudError> { + let (row, col) = self.point_coordinates(index)?; + self.point_at_mut(row, col) + } + + /// Mutably borrows one point by its zero-based row and column. + pub fn point_at_mut( + &mut self, + row: usize, + col: usize, + ) -> Result, PointCloudError> { + let start = self.point_start(row, col)?; + let end = start + .checked_add(self.point_step) + .ok_or(PointCloudError::Overflow)?; + let bytes = self + .cloud + .borrow_mut() + .data + .get_mut(start..end) + .ok_or(PointCloudError::InvalidData)?; + Ok(PointAccessor { + bytes, + fields: &self.fields, + }) + } +} + +/// A read-only typed cloud accessor. +pub type TypedPointCloudReader<'a, S> = TypedPointCloudAccessor<&'a PointCloud, S>; + +/// An exclusively borrowed typed cloud accessor. +pub type TypedPointCloudWriter<'a, S> = TypedPointCloudAccessor<&'a mut PointCloud, S>; + +/// A cloud accessor with cached metadata retaining each selected Rust type. +/// +/// Name, datatype, and layout validation happen at construction, not per point. +/// Overlapping and repeated selections are allowed; tuple writes validate every +/// selected value before changing bytes and apply writes in selection order. +pub struct TypedPointCloudAccessor { + raw: PointCloudAccessor, + checked: S::Checked, +} + +impl, S: FieldSelection> TypedPointCloudAccessor { + pub fn point(&self, index: usize) -> Result, PointCloudError> { + let (row, col) = self.raw.point_coordinates(index)?; + self.point_at(row, col) + } + + pub fn point_at( + &self, + row: usize, + col: usize, + ) -> Result, PointCloudError> { + let raw = self.raw.point_at(row, col)?; + let fields = S::access(&self.checked, raw.bytes)?; + Ok(TypedPointReader { raw, fields }) + } +} + +impl, S: FieldSelection> TypedPointCloudAccessor { + pub fn point_mut(&mut self, index: usize) -> Result, PointCloudError> { + let raw = self.raw.point_mut(index)?; + Ok(TypedPointWriter { + raw, + checked: &self.checked, + }) + } + + pub fn point_at_mut( + &mut self, + row: usize, + col: usize, + ) -> Result, PointCloudError> { + let raw = self.raw.point_at_mut(row, col)?; + Ok(TypedPointWriter { + raw, + checked: &self.checked, + }) + } +} + +impl Deref for TypedPointCloudAccessor { + type Target = PointCloudAccessor; + + fn deref(&self) -> &Self::Target { + &self.raw + } +} + +impl DerefMut for TypedPointCloudAccessor { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.raw + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::point::test_support::{message, projected_cloud}; + use crate::point::*; + use alloc::{vec, vec::Vec}; + use core::borrow::Borrow; + + fn vector_at>(cloud: &PointCloudAccessor, index: usize) -> Vec { + cloud + .point(index) + .unwrap() + .field("vector") + .unwrap() + .into_typed::() + .unwrap() + .values() + .collect::, _>>() + .unwrap() + } + + #[test] + fn point_handles_reuse_cloud_metadata() { + let mut cloud = projected_cloud(); + let mut reader = PointCloudWriter::try_new( + &mut cloud, + Some(&[ + ("label", FieldDatatype::Uint8), + ("vector", FieldDatatype::Uint16), + ]), + ) + .unwrap(); + let metadata = reader.fields.as_ptr(); + for index in 0..reader.point_count() { + let point = reader.point(index).unwrap(); + assert_eq!(point.fields.as_ptr(), metadata); + } + let point = reader.point_mut(3).unwrap(); + assert_eq!(point.fields.as_ptr(), metadata); + } + + #[test] + fn typed_writer_reads_and_writes_points_across_padded_rows() { + let mut cloud = projected_cloud(); + let mut writer = PointCloudWriter::try_new_typed( + &mut cloud, + (Field::::new("label"), Field::::new("vector")), + ) + .unwrap(); + assert_eq!(writer.point_count(), 4); + assert_eq!(writer.point(3).unwrap().fields().1.value(1), Ok(112)); + assert_eq!(writer.point_at(1, 1).unwrap().fields().0.value(0), Ok(110)); + writer + .point_at_mut(1, 1) + .unwrap() + .write_values(0, (42, 500)) + .unwrap(); + let (label, vector) = writer.point(3).unwrap().fields(); + assert_eq!(label.value(0), Ok(42)); + assert_eq!( + vector.values().collect::, _>>().unwrap(), + vec![500, 112, 113] + ); + assert!(matches!( + writer.point(4), + Err(PointCloudError::PointOutOfBounds { index: 4, count: 4 }) + )); + assert!(matches!( + writer.point_at(2, 0), + Err(PointCloudError::RowOutOfBounds { + index: 2, + height: 2 + }) + )); + assert_eq!(writer.data().get(35..38), Some(&[0xCC; 3][..])); + } + + #[test] + fn tuple_reads_complete_unaligned_fields_in_order_across_padded_rows() { + let mut cloud = projected_cloud(); + // Invalid unselected metadata must not prevent typed selection. + cloud + .fields + .push(PointField::new("unused", 8, FieldDatatype::Float64, 1)); + let original = cloud.data.clone(); + let reader = PointCloudReader::try_new_typed( + &cloud, + (Field::::new("label"), Field::::new("vector")), + ) + .unwrap(); + assert_eq!(reader.point_count(), 4); + assert_eq!(reader.data(), original); + assert_eq!( + reader.fields().map(CheckedField::name).collect::>(), + vec!["label", "vector"] + ); + for (index, base) in [0_u16, 10, 100, 110].into_iter().enumerate() { + let point = reader.point(index).unwrap(); + let (label, vector) = point.fields(); + assert_eq!(label.value(0), Ok(u8::try_from(base).unwrap())); + assert_eq!(vector.count(), 3); + assert_eq!(vector.bytes(), point.field("vector").unwrap().bytes()); + assert_eq!( + vector.values().collect::, _>>().unwrap(), + vec![base + 1, base + 2, base + 3] + ); + let (_, at_vector) = reader.point_at(index / 2, index % 2).unwrap().fields(); + assert_eq!(at_vector.value(2), Ok(base + 3)); + assert_eq!(point.bytes().len(), 8); + assert_eq!( + vector.value(3), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "vector", + index: 3, + count: 3, + } + )) + ); + } + assert!(matches!( + reader.point(4), + Err(PointCloudError::PointOutOfBounds { .. }) + )); + assert!(matches!( + reader.point_at(2, 0), + Err(PointCloudError::RowOutOfBounds { .. }) + )); + assert!(matches!( + reader.point_at(0, 2), + Err(PointCloudError::ColumnOutOfBounds { .. }) + )); + assert_eq!(cloud.data, original); + } + + #[test] + fn construction_requires_native_endianness_for_readers_and_writers() { + let mut empty = projected_cloud(); + empty.height = 0; + empty.data.clear(); + for mut cloud in [ + projected_cloud(), + empty, + message(0, 1, 8, 0, vec![], vec![]), + ] { + let fields: &[(&'static str, FieldDatatype)] = if cloud.fields.is_empty() { + &[] + } else { + &[("vector", FieldDatatype::Uint16)] + }; + for selection in [None, Some(fields), Some(&[][..])] { + assert!(PointCloudReader::try_new(&cloud, selection).is_ok()); + assert!(PointCloudWriter::try_new(&mut cloud, selection).is_ok()); + cloud.is_bigendian = !cfg!(target_endian = "big"); + let original = cloud.data.clone(); + assert_eq!( + PointCloudReader::try_new(&cloud, selection).err(), + Some(PointCloudError::UnsupportedEndianness) + ); + assert_eq!( + PointCloudWriter::try_new(&mut cloud, selection).err(), + Some(PointCloudError::UnsupportedEndianness) + ); + assert_eq!(cloud.data, original); + cloud.is_bigendian = cfg!(target_endian = "big"); + } + cloud.is_bigendian = !cfg!(target_endian = "big"); + cloud + .fields + .push(PointField::new("invalid", 8, FieldDatatype::Uint16, 1)); + for selection in [None, Some(&[("missing", FieldDatatype::Uint8)][..])] { + assert_eq!( + PointCloudReader::try_new(&cloud, selection).err(), + Some(PointCloudError::UnsupportedEndianness) + ); + assert_eq!( + PointCloudWriter::try_new(&mut cloud, selection).err(), + Some(PointCloudError::UnsupportedEndianness) + ); + } + } + } + #[test] + fn point_indices_and_coordinates_traverse_padded_rows_equivalently() { + let cloud = projected_cloud(); + let reader = PointCloudReader::try_new(&cloud, None).unwrap(); + assert_eq!(reader.point_count(), 4); + for (index, expected) in [ + vec![1, 2, 3], + vec![11, 12, 13], + vec![101, 102, 103], + vec![111, 112, 113], + ] + .into_iter() + .enumerate() + { + let by_index = reader.point(index).unwrap(); + let by_coordinates = reader.point_at(index / 2, index % 2).unwrap(); + assert_eq!(by_index.bytes(), by_coordinates.bytes()); + assert_eq!(vector_at(&reader, index), expected); + assert_eq!( + by_index + .field("label") + .unwrap() + .into_typed::() + .unwrap() + .value(0), + Ok(u8::try_from([0_u16, 10, 100, 110][index]).unwrap()) + ); + } + assert_eq!(reader.data().get(16..19), Some(&[0xCC; 3][..])); + assert_eq!(reader.data().get(35..38), Some(&[0xCC; 3][..])); + assert!(matches!( + reader.point(4), + Err(PointCloudError::PointOutOfBounds { index: 4, count: 4 }) + )); + assert!(matches!( + reader.point(usize::MAX), + Err(PointCloudError::PointOutOfBounds { + index: usize::MAX, + count: 4 + }) + )); + assert!(matches!( + reader.point_at(2, 0), + Err(PointCloudError::RowOutOfBounds { + index: 2, + height: 2 + }) + )); + assert!(matches!( + reader.point_at(0, 2), + Err(PointCloudError::ColumnOutOfBounds { index: 2, width: 2 }) + )); + } + #[cfg(target_pointer_width = "32")] + #[test] + fn rejects_dimension_arithmetic_overflow() { + let cloud = message(u32::MAX, u32::MAX, u32::MAX, u32::MAX, vec![], vec![]); + assert_eq!( + PointCloudReader::try_new(&cloud, None).err(), + Some(PointCloudError::Overflow) + ); + } + #[test] + fn validates_cloud_shape_and_zero_sized_dimensions() { + let mut cloud = message(1, 1, 0, 0, vec![], vec![]); + assert_eq!( + PointCloudReader::try_new(&cloud, None).err(), + Some(PointCloudError::ZeroPointStep) + ); + + cloud.point_step = 4; + cloud.width = 2; + cloud.row_step = 7; + assert_eq!( + PointCloudReader::try_new(&cloud, None).err(), + Some(PointCloudError::InvalidRowStep { step: 7, min: 8 }) + ); + + cloud.row_step = 8; + cloud.data = vec![0; 7]; + assert_eq!( + PointCloudReader::try_new(&cloud, None).err(), + Some(PointCloudError::InvalidDataSize { + size: 7, + expected: 8, + }) + ); + + let zero_width = message(0, 2, 8, 3, vec![], vec![0; 6]); + let reader = PointCloudReader::try_new(&zero_width, None).unwrap(); + assert_eq!(reader.point_count(), 0); + assert!(matches!( + reader.point(0), + Err(PointCloudError::PointOutOfBounds { count: 0, .. }) + )); + let zero_height = message(3, 0, 8, 24, vec![], vec![]); + assert_eq!( + PointCloudReader::try_new(&zero_height, None) + .unwrap() + .point_count(), + 0 + ); + } +} diff --git a/applications/autoware/common_types/src/point/field.rs b/applications/autoware/common_types/src/point/field.rs new file mode 100644 index 000000000..364c50fc6 --- /dev/null +++ b/applications/autoware/common_types/src/point/field.rs @@ -0,0 +1,587 @@ +use core::{ + marker::PhantomData, + ops::{Deref, DerefMut, Range}, +}; + +use super::{FieldDatatype, PointCloudError, PointField, PointFieldError, PointFieldValue}; + +/// Validated, machine-sized metadata for a point field. +/// +/// Unlike [`PointField`], this represents a field after its offset, count, and +/// byte range have been checked against the containing point layout. +/// This metadata does not retain a statically selected Rust value type; +/// [`TypedCheckedField`] additionally retains that type proof. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct CheckedField { + name: &'static str, + datatype: FieldDatatype, + count: usize, + item_size: usize, + offset_start: usize, + offset_end: usize, +} + +impl CheckedField { + /// Validates a message descriptor and caches its machine-sized access metadata. + pub(super) fn try_new(field: PointField, point_step: u32) -> Result { + let offset_start = field.offset as usize; + let count = field.count as usize; + let item_size = field.datatype.size() as usize; + let point_step = point_step as usize; + let field_size = count + .checked_mul(item_size) + .ok_or(PointFieldError::Overflow)?; + let offset_end = offset_start + .checked_add(field_size) + .ok_or(PointFieldError::Overflow)?; + if offset_end > point_step { + return Err(PointFieldError::FieldOutOfBounds { + name: field.name, + offset_start, + offset_end, + point_step, + }); + } + + Ok(Self { + name: field.name, + datatype: field.datatype, + count, + item_size, + offset_start, + offset_end, + }) + } + + /// Returns the field name. + pub const fn name(&self) -> &'static str { + self.name + } + + /// Returns the field datatype. + pub const fn datatype(&self) -> FieldDatatype { + self.datatype + } + + /// Returns the number of values in the field. + pub const fn count(&self) -> usize { + self.count + } + + /// Returns the size in bytes of each value. + pub const fn item_size(&self) -> usize { + self.item_size + } + + /// Returns the inclusive start offset in the point. + pub const fn offset_start(&self) -> usize { + self.offset_start + } + + /// Returns the exclusive end offset in the point. + pub const fn offset_end(&self) -> usize { + self.offset_end + } + + /// Returns the byte range of the field within the point. + pub const fn range(&self) -> Range { + self.offset_start..self.offset_end + } + + pub const fn value_range(&self, value: usize) -> Result, PointFieldError> { + if value >= self.count { + return Err(PointFieldError::ValueOutOfBounds { + name: self.name, + index: value, + count: self.count, + }); + } + let start = value * self.item_size; + let end = start + self.item_size; + Ok(start..end) + } +} + +/// Validated field metadata retaining proof of a selected Rust value type. +/// +/// Dereferences to [`CheckedField`] for layout metadata. The metadata and +/// type witness are private, and construction is internal and checked, so +/// callers cannot forge the proof. Public only for selection associated types. +pub struct TypedCheckedField { + field: CheckedField, + value: PhantomData, +} + +impl Deref for TypedCheckedField { + type Target = CheckedField; + fn deref(&self) -> &CheckedField { + &self.field + } +} + +impl TypedCheckedField { + pub(super) fn resolve( + fields: &mut core::slice::Iter<'_, CheckedField>, + ) -> Result { + let field = *fields.next().ok_or(PointCloudError::InvalidData)?; + // Retain a validated type witness even if this trait method is + // invoked directly rather than through the cloud constructor. + if field.datatype() != T::DATATYPE { + return Err(PointFieldError::DatatypeMismatch { + name: field.name(), + expected: T::DATATYPE, + actual: field.datatype(), + } + .into()); + } + Ok(Self { + field, + value: PhantomData, + }) + } + + pub(super) fn validate_value(&self, bytes: &[u8], value: usize) -> Result<(), PointCloudError> { + let range = self.value_range(value)?; + let bytes = bytes + .get(self.field.range()) + .ok_or(PointCloudError::InvalidData)?; + let bytes = bytes.get(range).ok_or(PointCloudError::InvalidData)?; + if bytes.len() != T::SIZE { + return Err(PointFieldError::InvalidLength.into()); + } + Ok(()) + } + + pub(super) fn write_value( + &self, + bytes: &mut [u8], + value: usize, + replacement: T, + ) -> Result<(), PointCloudError> { + let range = self.value_range(value)?; + let bytes = bytes + .get_mut(self.field.range()) + .ok_or(PointCloudError::InvalidData)?; + let target = bytes.get_mut(range).ok_or(PointCloudError::InvalidData)?; + let encoded = replacement.to_ne_bytes(); + let encoded = encoded.as_ref(); + if target.len() != encoded.len() { + return Err(PointFieldError::InvalidLength.into()); + } + target.copy_from_slice(encoded); + Ok(()) + } + + pub(super) fn validate_slice(&self, bytes: &[u8], values: &[T]) -> Result<(), PointCloudError> { + if values.len() != self.field.count() { + return Err(PointCloudError::InvalidData); + } + bytes + .get(self.field.range()) + .ok_or(PointCloudError::InvalidData)?; + for value_index in 0..values.len() { + self.validate_value(bytes, value_index)?; + } + Ok(()) + } + + pub(super) fn write_slice( + &self, + bytes: &mut [u8], + values: &[T], + ) -> Result<(), PointCloudError> { + for (value_index, &value) in values.iter().enumerate() { + self.write_value(bytes, value_index, value)?; + } + Ok(()) + } + + pub(super) fn access<'a>( + &'a self, + bytes: &'a [u8], + ) -> Result, PointCloudError> { + let bytes = bytes + .get(self.field.range()) + .ok_or(PointCloudError::InvalidData)?; + Ok(TypedFieldAccessor { + raw: FieldAccessor { + field: &self.field, + bytes, + }, + _value: PhantomData, + }) + } +} + +/// A field backed by borrowed bytes, possibly unaligned. +/// +/// Bytes retain their stored byte order. Cloud construction requires native +/// byte order; typed access reads and writes that order without swapping. +#[derive(Clone, Copy)] +pub struct FieldAccessor<'a, D> { + pub(super) field: &'a CheckedField, + pub(super) bytes: D, +} + +impl<'a, D: AsRef<[u8]>> FieldAccessor<'a, D> { + /// Returns all bytes of the field, including every consecutive value. + #[inline] + pub fn bytes(&self) -> &[u8] { + self.bytes.as_ref() + } + + /// Returns the byte slice corresponding to a single value within the field. + #[inline] + pub fn value_bytes(&self, value: usize) -> Result<&[u8], PointCloudError> { + let range = self.value_range(value)?; + self.bytes().get(range).ok_or(PointCloudError::InvalidData) + } + + /// Checks the datatype once and converts this raw accessor to a typed one. + pub fn into_typed( + self, + ) -> Result, PointCloudError> { + if self.field.datatype() != T::DATATYPE { + return Err(PointFieldError::DatatypeMismatch { + name: self.field.name(), + expected: T::DATATYPE, + actual: self.field.datatype(), + } + .into()); + } + if self.field.item_size() != T::SIZE { + return Err(PointFieldError::InvalidLength.into()); + } + Ok(TypedFieldAccessor { + raw: self, + _value: PhantomData, + }) + } +} + +impl<'a, D: AsRef<[u8]> + AsMut<[u8]>> FieldAccessor<'a, D> { + /// Returns writable field bytes. The caller controls the encoded values. + #[inline] + pub fn bytes_mut(&mut self) -> &mut [u8] { + self.bytes.as_mut() + } + + /// Returns the mutable byte slice corresponding to a single value within the field. + #[inline] + pub fn value_bytes_mut(&mut self, value: usize) -> Result<&mut [u8], PointCloudError> { + let range = self.value_range(value)?; + self.bytes_mut() + .get_mut(range) + .ok_or(PointCloudError::InvalidData) + } +} + +impl<'a, D: AsRef<[u8]>> Deref for FieldAccessor<'a, D> { + type Target = CheckedField; + + fn deref(&self) -> &Self::Target { + self.field + } +} + +/// Typed conversion over a resolved field; no further name or datatype lookups. +#[derive(Clone, Copy)] +pub struct TypedFieldAccessor<'a, D, T> { + pub(super) raw: FieldAccessor<'a, D>, + pub(super) _value: PhantomData, +} + +impl<'a, D: AsRef<[u8]> + 'a, T: PointFieldValue + 'a> TypedFieldAccessor<'a, D, T> { + /// Decodes a single value in native byte order. + pub fn value(&self, value: usize) -> Result { + let bytes = self.value_bytes(value)?; + T::from_ne_slice(bytes).map_err(PointCloudError::from) + } + + /// Iterates over all values, decoding unaligned bytes in native order. + pub fn values(self) -> impl ExactSizeIterator> + 'a { + (0..self.count()).map(move |value| self.value(value)) + } +} + +impl<'a, D: AsRef<[u8]> + AsMut<[u8]>, T: PointFieldValue> TypedFieldAccessor<'a, D, T> { + /// Encodes one value in native byte order, preserving surrounding bytes. + pub fn write_value(&mut self, value: usize, replacement: T) -> Result<(), PointCloudError> { + let bytes = self.value_bytes_mut(value)?; + let replacement = replacement.to_ne_bytes(); + let replacement = replacement.as_ref(); + if bytes.len() != replacement.len() { + return Err(PointFieldError::InvalidLength.into()); + } + bytes.copy_from_slice(replacement); + Ok(()) + } + + pub fn values_mut(&mut self) -> impl ExactSizeIterator> { + self.raw + .bytes_mut() + .chunks_exact_mut(T::SIZE) + .map(|bytes| PointFieldValueMut { + bytes, + _value: PhantomData, + }) + } +} + +impl<'a, D: AsRef<[u8]>, T: PointFieldValue> Deref for TypedFieldAccessor<'a, D, T> { + type Target = FieldAccessor<'a, D>; + + fn deref(&self) -> &Self::Target { + &self.raw + } +} + +impl<'a, D: AsRef<[u8]> + AsMut<[u8]>, T: PointFieldValue> DerefMut + for TypedFieldAccessor<'a, D, T> +{ + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.raw + } +} + +/// A writable value in a point field, backed by possibly unaligned bytes. +#[derive(Debug)] +pub struct PointFieldValueMut<'a, T> { + bytes: &'a mut [u8], + _value: PhantomData, +} + +impl PointFieldValueMut<'_, T> { + /// Reads the value in native byte order. + pub fn read(&self) -> Result { + T::from_ne_slice(self.bytes) + } + + /// Writes the value in native byte order. + pub fn write(&mut self, value: T) { + for (target, source) in self.bytes.iter_mut().zip(value.to_ne_bytes().as_ref()) { + *target = *source; + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::point::test_support::{message, projected_cloud}; + use crate::point::*; + use alloc::{vec, vec::Vec}; + + #[test] + fn checked_fields_cache_message_metadata_and_datatype_sizes() { + for (datatype, item_size) in [ + (FieldDatatype::Int8, 1), + (FieldDatatype::Uint8, 1), + (FieldDatatype::Int16, 2), + (FieldDatatype::Uint16, 2), + (FieldDatatype::Int32, 4), + (FieldDatatype::Uint32, 4), + (FieldDatatype::Float32, 4), + (FieldDatatype::Float64, 8), + ] { + let offset: usize = 1; + let count: usize = 3; + let field = PointField { + name: "values", + offset: offset as u32, + count: count as u32, + datatype, + }; + let resolved = CheckedField::try_new(field, 25).unwrap(); + assert_eq!(resolved.name(), field.name); + assert_eq!(resolved.datatype(), datatype); + assert_eq!(resolved.count(), count); + assert_eq!(resolved.item_size(), item_size); + assert_eq!(resolved.offset_start(), 1); + assert_eq!(resolved.offset_end(), (offset + count * item_size)); + assert_eq!(resolved.range(), offset..(offset + count * item_size)); + } + } + + #[test] + fn checked_fields_check_complete_ranges_and_allow_empty_fields_at_end() { + let mut field = PointField { + name: "values", + offset: 1, + datatype: FieldDatatype::Uint16, + count: 3, + }; + assert_eq!( + CheckedField::try_new(field, 6), + Err(PointFieldError::FieldOutOfBounds { + name: "values", + offset_start: 1, + offset_end: 7, + point_step: 6, + }) + ); + assert_eq!(CheckedField::try_new(field, 7).unwrap().offset_end(), 7); + field.offset = 7; + field.count = 0; + let resolved = CheckedField::try_new(field, 7).unwrap(); + assert_eq!(resolved.offset_start(), 7); + assert_eq!(resolved.offset_end(), 7); + assert_eq!(resolved.range(), 7..7); + assert_eq!(resolved.count(), 0); + field.offset = 8; + assert_eq!( + CheckedField::try_new(field, 7), + Err(PointFieldError::FieldOutOfBounds { + name: "values", + offset_start: 8, + offset_end: 8, + point_step: 7, + }) + ); + } + + #[test] + fn malformed_field_ranges_fail_without_mutating_typed_field_bytes() { + let descriptor = PointField::new("values", 1, FieldDatatype::Uint16, 2); + assert!(matches!( + CheckedField::try_new(descriptor, 4), + Err(PointFieldError::FieldOutOfBounds { .. }) + )); + let resolved = CheckedField::try_new(descriptor, 5).unwrap(); + let mut bytes = [0xAA; 1]; + { + let raw = FieldAccessor { + field: &resolved, + bytes: &mut bytes[..], + }; + let mut typed = raw.into_typed::().unwrap(); + assert_eq!(typed.write_value(0, 42), Err(PointCloudError::InvalidData)); + } + assert_eq!(bytes, [0xAA; 1]); + } + #[test] + fn empty_and_overlapping_fields_support_sequential_access() { + let mut cloud = message( + 1, + 1, + 4, + 4, + vec![ + PointField::new("empty", 4, FieldDatatype::Uint16, 0), + PointField::new("first", 0, FieldDatatype::Uint16, 1), + PointField::new("alias", 0, FieldDatatype::Uint16, 1), + ], + vec![0xAA; 4], + ); + let mut writer = PointCloudWriter::try_new(&mut cloud, None).unwrap(); + { + let mut point = writer.point_mut(0).unwrap(); + let mut empty = point + .field_mut("empty") + .unwrap() + .into_typed::() + .unwrap(); + assert_eq!(empty.values_mut().len(), 0); + assert!(matches!( + empty.write_value(0, 1), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "empty", + index: 0, + count: 0, + } + )) + )); + point + .field_mut("first") + .unwrap() + .into_typed::() + .unwrap() + .write_value(0, 42) + .unwrap(); + assert_eq!( + point + .field("alias") + .unwrap() + .into_typed::() + .unwrap() + .value(0), + Ok(42) + ); + assert!(point.field("empty").unwrap().bytes().is_empty()); + } + let mut expected = 42_u16.to_ne_bytes().to_vec(); + expected.extend_from_slice(&[0xAA; 2]); + assert_eq!(writer.data(), expected); + } + #[test] + fn unaligned_native_values_iterate_and_mutate_without_touching_neighbors() { + let mut cloud = projected_cloud(); + let mut writer = PointCloudWriter::try_new(&mut cloud, None).unwrap(); + let original = writer.data().to_vec(); + { + let point = writer.point_at(1, 1).unwrap(); + let raw = point.field("vector").unwrap(); + assert_eq!(raw.bytes(), &original[28..34]); + assert!(core::ptr::eq( + raw.bytes().as_ptr(), + writer.data().get(28..34).unwrap().as_ptr() + )); + let typed = raw.into_typed::().unwrap(); + assert_eq!(typed.value(0), Ok(111)); + assert!(matches!( + typed.value(3), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "vector", + index: 3, + count: 3 + } + )) + )); + assert_eq!( + typed.values().collect::, _>>().unwrap(), + vec![111, 112, 113] + ); + assert!(matches!( + point.field("vector").unwrap().into_typed::(), + Err(PointCloudError::InvalidField( + PointFieldError::DatatypeMismatch { .. } + )) + )); + } + { + let mut point = writer.point_at_mut(1, 1).unwrap(); + { + let mut raw = point.field_mut("vector").unwrap(); + raw.bytes_mut()[0..2].copy_from_slice(&200_u16.to_ne_bytes()); + let mut typed = raw.into_typed::().unwrap(); + assert_eq!(typed.value(0), Ok(200)); + typed.write_value(1, 201).unwrap(); + for (mut value, expected) in typed.values_mut().zip([200_u16, 201, 113]) { + assert_eq!(value.read(), Ok(expected)); + value.write(expected + 1); + } + let before_invalid_write = typed.raw.bytes().to_vec(); + assert!(matches!( + typed.write_value(3, 999), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "vector", + index: 3, + count: 3 + } + )) + )); + assert_eq!(typed.raw.bytes(), before_invalid_write); + } + } + let mut expected = original; + for (offset, value) in [(28, 201_u16), (30, 202), (32, 114)] { + expected[offset..offset + 2].copy_from_slice(&value.to_ne_bytes()); + } + assert_eq!(writer.data(), expected); + assert_eq!(writer.data().get(16..19), Some(&[0xCC; 3][..])); + assert_eq!(writer.data().get(35..38), Some(&[0xCC; 3][..])); + } +} diff --git a/applications/autoware/common_types/src/point/selection.rs b/applications/autoware/common_types/src/point/selection.rs new file mode 100644 index 000000000..2e8bf4310 --- /dev/null +++ b/applications/autoware/common_types/src/point/selection.rs @@ -0,0 +1,517 @@ +use core::marker::PhantomData; + +use super::{ + field::{CheckedField, TypedCheckedField, TypedFieldAccessor}, + FieldDatatype, PointCloudError, PointFieldValue, +}; + +/// A field name retaining its requested Rust scalar type. +/// +/// Pass a tuple of these to [`super::PointCloudReader::try_new_typed`]. +#[derive(Clone, Copy, Debug)] +pub struct Field { + name: &'static str, + value: PhantomData, +} + +impl Field { + #[must_use] + pub const fn new(name: &'static str) -> Self { + Self { + name, + value: PhantomData, + } + } +} + +/// A sealed tuple selection of one through eight [`Field`] descriptors. +/// +/// Supported scalar types are those implementing [`PointFieldValue`]. +pub trait FieldSelection: typed_selection::Selection {} + +pub(super) mod typed_selection { + use super::*; + + pub trait Selection { + type Requests: AsRef<[(&'static str, FieldDatatype)]>; + type Checked; + type Fields<'a>: Copy; + type Values; + type Slices<'a> + where + Self: 'a; + + fn requests(&self) -> Self::Requests; + fn resolve( + fields: &mut core::slice::Iter<'_, CheckedField>, + ) -> Result; + fn access<'a>( + checked: &'a Self::Checked, + bytes: &'a [u8], + ) -> Result, PointCloudError>; + fn write_values( + checked: &Self::Checked, + bytes: &mut [u8], + value: usize, + values: Self::Values, + ) -> Result<(), PointCloudError>; + fn write_fields( + checked: &Self::Checked, + values: &Self::Slices<'_>, + bytes: &mut [u8], + ) -> Result<(), PointCloudError>; + } + + macro_rules! tuples { + ($($ty:ident : $index:tt),+ $(,)?) => { + impl<$($ty: PointFieldValue),+> FieldSelection for ($(Field<$ty>,)+) {} + + impl<$($ty: PointFieldValue),+> Selection for ($(Field<$ty>,)+) { + type Requests = [(&'static str, FieldDatatype); tuples!(@count $($ty),+)]; + type Checked = ($(TypedCheckedField<$ty>,)+); + type Fields<'a> = ($(TypedFieldAccessor<'a, &'a [u8], $ty>,)+); + type Values = ($($ty,)+); + type Slices<'a> = ($(&'a [$ty],)+) where Self: 'a; + + fn requests(&self) -> Self::Requests { + [$( (self.$index.name, $ty::DATATYPE), )+] + } + + fn resolve( + fields: &mut core::slice::Iter<'_, CheckedField>, + ) -> Result { + Ok(($(TypedCheckedField::<$ty>::resolve(fields)?,)+)) + } + + fn access<'a>( + checked: &'a Self::Checked, + bytes: &'a [u8], + ) -> Result, PointCloudError> { + Ok(($(checked.$index.access(bytes)?,)+)) + } + + fn write_values( + checked: &Self::Checked, + bytes: &mut [u8], + value: usize, + values: Self::Values, + ) -> Result<(), PointCloudError> { + $(checked.$index.validate_value(bytes, value)?;)+ + $(checked.$index.write_value(bytes, value, values.$index)?;)+ + Ok(()) + } + + fn write_fields( + checked: &Self::Checked, + values: &Self::Slices<'_>, + bytes: &mut [u8], + ) -> Result<(), PointCloudError> { + $(checked.$index.validate_slice(bytes, values.$index)?;)+ + $(checked.$index.write_slice(bytes, values.$index)?;)+ + Ok(()) + } + } + }; + (@count $($ty:ident),+) => { <[()]>::len(&[$(tuples!(@unit $ty)),+]) }; + (@unit $ty:ident) => { () }; + } + + tuples!(A: 0); + tuples!(A: 0, B: 1); + tuples!(A: 0, B: 1, C: 2); + tuples!(A: 0, B: 1, C: 2, D: 3); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6, H: 7); +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::point::test_support::{message, projected_cloud}; + use crate::point::*; + use alloc::{vec, vec::Vec}; + + #[test] + fn eight_field_tuple_decodes_every_supported_primitive() { + let mut data = vec![0xAA]; + data.extend_from_slice(&(-3_i8).to_ne_bytes()); + data.extend_from_slice(&4_u8.to_ne_bytes()); + data.extend_from_slice(&(-300_i16).to_ne_bytes()); + data.extend_from_slice(&400_u16.to_ne_bytes()); + data.extend_from_slice(&(-50_000_i32).to_ne_bytes()); + data.extend_from_slice(&60_000_u32.to_ne_bytes()); + data.extend_from_slice(&1.25_f32.to_ne_bytes()); + data.extend_from_slice(&2.5_f64.to_ne_bytes()); + let cloud = message( + 1, + 1, + 27, + 27, + vec![ + PointField::new("a", 1, FieldDatatype::Int8, 1), + PointField::new("b", 2, FieldDatatype::Uint8, 1), + PointField::new("c", 3, FieldDatatype::Int16, 1), + PointField::new("d", 5, FieldDatatype::Uint16, 1), + PointField::new("e", 7, FieldDatatype::Int32, 1), + PointField::new("f", 11, FieldDatatype::Uint32, 1), + PointField::new("g", 15, FieldDatatype::Float32, 1), + PointField::new("h", 19, FieldDatatype::Float64, 1), + ], + data, + ); + let reader = PointCloudReader::try_new_typed( + &cloud, + ( + Field::::new("a"), + Field::::new("b"), + Field::::new("c"), + Field::::new("d"), + Field::::new("e"), + Field::::new("f"), + Field::::new("g"), + Field::::new("h"), + ), + ) + .unwrap(); + let (a, b, c, d, e, f, g, h) = reader.point(0).unwrap().fields(); + assert_eq!(a.value(0), Ok(-3)); + assert_eq!(b.value(0), Ok(4)); + assert_eq!(c.value(0), Ok(-300)); + assert_eq!(d.value(0), Ok(400)); + assert_eq!(e.value(0), Ok(-50_000)); + assert_eq!(f.value(0), Ok(60_000)); + assert_eq!(g.value(0), Ok(1.25)); + assert_eq!(h.value(0), Ok(2.5)); + } + #[test] + fn selected_duplicate_name_uses_first_descriptor() { + let cloud = message( + 1, + 1, + 2, + 2, + vec![ + PointField::new("duplicate", 0, FieldDatatype::Uint8, 1), + PointField::new("duplicate", 1, FieldDatatype::Uint16, 1), + ], + vec![7, 9], + ); + assert!(matches!( + PointCloudReader::try_new(&cloud, None), + Err(PointCloudError::InvalidField( + PointFieldError::FieldOutOfBounds { + name: "duplicate", + .. + } + )) + )); + let selected = + PointCloudReader::try_new(&cloud, Some(&[("duplicate", FieldDatatype::Uint8)])) + .unwrap(); + assert_eq!( + selected + .point(0) + .unwrap() + .field("duplicate") + .unwrap() + .into_typed::() + .unwrap() + .value(0), + Ok(7) + ); + let mut cloud = cloud; + cloud.fields[1].offset = 0; + assert_eq!( + PointCloudReader::try_new(&cloud, Some(&[("duplicate", FieldDatatype::Uint16)])).err(), + Some(PointCloudError::InvalidField( + PointFieldError::DatatypeMismatch { + name: "duplicate", + expected: FieldDatatype::Uint16, + actual: FieldDatatype::Uint8, + } + )) + ); + } + #[test] + fn selections_validate_requested_fields_and_preserve_order_and_bytes() { + let mut cloud = message( + 1, + 1, + 4, + 4, + vec![ + PointField::new("x", 0, FieldDatatype::Uint8, 1), + PointField::new("y", 1, FieldDatatype::Uint8, 1), + PointField::new("unused", 3, FieldDatatype::Uint16, 1), + ], + vec![7, 9, 0xAA, 0xBB], + ); + assert!(matches!( + PointCloudReader::try_new(&cloud, None), + Err(PointCloudError::InvalidField( + PointFieldError::FieldOutOfBounds { name: "unused", .. } + )) + )); + assert!(matches!( + PointCloudWriter::try_new(&mut cloud, None), + Err(PointCloudError::InvalidField( + PointFieldError::FieldOutOfBounds { name: "unused", .. } + )) + )); + assert!(matches!( + PointCloudReader::try_new(&cloud, Some(&[("unused", FieldDatatype::Uint16)])), + Err(PointCloudError::InvalidField( + PointFieldError::FieldOutOfBounds { name: "unused", .. } + )) + )); + + let selected = PointCloudReader::try_new( + &cloud, + Some(&[ + ("y", FieldDatatype::Uint8), + ("x", FieldDatatype::Uint8), + ("y", FieldDatatype::Uint8), + ]), + ) + .unwrap(); + assert_eq!( + selected + .fields() + .map(CheckedField::name) + .collect::>(), + vec!["y", "x", "y"] + ); + let point = selected.point(0).unwrap(); + assert_eq!(point.bytes(), &[7, 9, 0xAA, 0xBB]); + assert_eq!( + point + .field("y") + .unwrap() + .into_typed::() + .unwrap() + .value(0), + Ok(9) + ); + assert!(matches!( + PointCloudReader::try_new(&cloud, Some(&[("missing", FieldDatatype::Uint8)])), + Err(PointCloudError::InvalidField( + PointFieldError::FieldNotFound("missing") + )) + )); + + let empty = PointCloudReader::try_new(&cloud, Some(&[])).unwrap(); + assert_eq!(empty.fields().len(), 0); + assert_eq!(empty.point(0).unwrap().bytes(), &[7, 9, 0xAA, 0xBB]); + assert!(matches!( + empty.point(0).unwrap().field("x"), + Err(PointCloudError::InvalidField( + PointFieldError::FieldNotFound("x") + )) + )); + cloud.data.pop(); + assert_eq!( + PointCloudReader::try_new(&cloud, Some(&[])).err(), + Some(PointCloudError::InvalidDataSize { + size: 3, + expected: 4, + }) + ); + + cloud.height = 0; + cloud.data.clear(); + assert!(matches!( + PointCloudReader::try_new(&cloud, None), + Err(PointCloudError::InvalidField(_)) + )); + assert!(PointCloudReader::try_new(&cloud, Some(&[])).is_ok()); + } + #[test] + fn tuple_duplicates_overlap_and_zero_counts_are_read_only() { + let mut cloud = message( + 1, + 1, + 2, + 2, + vec![ + PointField::new("x", 0, FieldDatatype::Uint8, 1), + PointField::new("x", 1, FieldDatatype::Uint16, 1), + PointField::new("overlap", 0, FieldDatatype::Uint8, 1), + PointField::new("empty", 2, FieldDatatype::Uint16, 0), + ], + vec![7, 9], + ); + let reader = PointCloudReader::try_new_typed( + &cloud, + ( + Field::::new("x"), + Field::::new("x"), + Field::::new("overlap"), + Field::::new("empty"), + ), + ) + .unwrap(); + let (x, repeated, overlap, empty) = reader.point(0).unwrap().fields(); + assert_eq!( + (x.value(0), repeated.value(0), overlap.value(0)), + (Ok(7), Ok(7), Ok(7)) + ); + assert_eq!(empty.values().len(), 0); + assert!(matches!( + empty.value(0), + Err(PointCloudError::InvalidField( + PointFieldError::ValueOutOfBounds { + name: "empty", + index: 0, + count: 0, + } + )) + )); + assert_eq!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("x"),)).err(), + Some( + PointFieldError::DatatypeMismatch { + name: "x", + expected: FieldDatatype::Uint16, + actual: FieldDatatype::Uint8, + } + .into() + ) + ); + assert_eq!(cloud.data, vec![7, 9]); + // Existing sequential writes remain valid even with overlapping fields. + let mut writer = PointCloudWriter::try_new( + &mut cloud, + Some(&[ + ("x", FieldDatatype::Uint8), + ("overlap", FieldDatatype::Uint8), + ]), + ) + .unwrap(); + let mut point = writer.point_mut(0).unwrap(); + point + .field_mut("x") + .unwrap() + .into_typed::() + .unwrap() + .write_value(0, 8) + .unwrap(); + point + .field_mut("overlap") + .unwrap() + .into_typed::() + .unwrap() + .write_value(0, 10) + .unwrap(); + assert_eq!(cloud.data, vec![10, 9]); + } + #[test] + fn tuple_validates_missing_mismatched_layout_and_endian_even_without_points() { + for height in [2, 0] { + let mut cloud = projected_cloud(); + cloud.height = height; + if height == 0 { + cloud.data.clear(); + } + let original = cloud.data.clone(); + let valid = + PointCloudReader::try_new_typed(&cloud, (Field::::new("vector"),)).unwrap(); + assert_eq!(valid.point_count(), usize::try_from(height).unwrap() * 2); + if height == 0 { + assert!(matches!( + valid.point(0), + Err(PointCloudError::PointOutOfBounds { count: 0, .. }) + )); + } + assert_eq!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("vector"),)).err(), + Some( + PointFieldError::DatatypeMismatch { + name: "vector", + expected: FieldDatatype::Float32, + actual: FieldDatatype::Uint16, + } + .into() + ) + ); + assert_eq!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("missing"),)).err(), + Some(PointFieldError::FieldNotFound("missing").into()) + ); + cloud + .fields + .push(PointField::new("bad", 8, FieldDatatype::Uint8, 1)); + assert!(matches!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("bad"),)), + Err(PointCloudError::InvalidField( + PointFieldError::FieldOutOfBounds { .. } + )) + )); + cloud.is_bigendian = !cfg!(target_endian = "big"); + assert_eq!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("missing"),)).err(), + Some(PointCloudError::UnsupportedEndianness) + ); + assert_eq!(cloud.data, original); + cloud.is_bigendian = cfg!(target_endian = "big"); + cloud.data.push(0); + assert!(matches!( + PointCloudReader::try_new_typed(&cloud, (Field::::new("vector"),)), + Err(PointCloudError::InvalidDataSize { .. }) + )); + } + } + #[test] + fn typed_selections_reject_mismatches_for_readers_and_writers_even_when_empty() { + let selection = [ + ("label", FieldDatatype::Uint8), + ("vector", FieldDatatype::Float32), + ]; + let expected = PointCloudError::InvalidField(PointFieldError::DatatypeMismatch { + name: "vector", + expected: FieldDatatype::Float32, + actual: FieldDatatype::Uint16, + }); + for height in [2, 0] { + let mut cloud = projected_cloud(); + cloud.height = height; + if height == 0 { + cloud.data.clear(); + } + let original = cloud.data.clone(); + assert_eq!( + PointCloudReader::try_new(&cloud, Some(&selection)).err(), + Some(expected.clone()) + ); + assert_eq!( + PointCloudWriter::try_new(&mut cloud, Some(&selection)).err(), + Some(expected.clone()) + ); + assert_eq!(cloud.data, original); + } + } + #[test] + fn typed_writer_checks_wrong_types_for_empty_clouds() { + let mut cloud = message( + 1, + 0, + 4, + 4, + vec![PointField::new("value", 0, FieldDatatype::Uint16, 1)], + vec![], + ); + assert_eq!( + PointCloudWriter::try_new_typed(&mut cloud, (Field::::new("value"),)).err(), + Some( + PointFieldError::DatatypeMismatch { + name: "value", + expected: FieldDatatype::Uint8, + actual: FieldDatatype::Uint16, + } + .into() + ) + ); + assert!(cloud.data.is_empty()); + } +} diff --git a/applications/autoware/common_types/src/point/test_support.rs b/applications/autoware/common_types/src/point/test_support.rs new file mode 100644 index 000000000..55777bff4 --- /dev/null +++ b/applications/autoware/common_types/src/point/test_support.rs @@ -0,0 +1,53 @@ +//! Utility functions for testing point cloud handling. + +use alloc::{vec, vec::Vec}; + +use crate::{ + point::{FieldDatatype, PointCloud, PointField}, + Header, +}; + +pub(super) fn message( + width: u32, + height: u32, + point_step: u32, + row_step: u32, + fields: Vec, + data: Vec, +) -> PointCloud { + PointCloud { + header: Header { + frame_id: "lidar", + timestamp: 0, + }, + width, + height, + fields, + is_bigendian: cfg!(target_endian = "big"), + point_step, + row_step, + data, + is_dense: true, + } +} + +/// Two rows of two points: an unaligned three-value u16 vector and a u8 label, +/// with three bytes of row padding. Values are encoded in native byte order. +pub(super) fn projected_cloud() -> PointCloud { + let fields = vec![ + PointField::new("vector", 1, FieldDatatype::Uint16, 3), + PointField::new("label", 7, FieldDatatype::Uint8, 1), + ]; + let mut data = Vec::new(); + for (index, base) in [0_u16, 10, 100, 110].into_iter().enumerate() { + data.push(0xAA); + for value in [base + 1, base + 2, base + 3] { + data.extend_from_slice(&value.to_ne_bytes()); + } + data.push(u8::try_from(base).unwrap()); + if index % 2 == 1 { + data.extend_from_slice(&[0xCC; 3]); + } + } + message(2, 2, 8, 19, fields, data) +} From f4d44e4c2d24f7f4b6ae4c1165a5ed7b418446cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Wed, 11 Nov 2026 09:42:36 +0900 Subject: [PATCH 2/6] feat(autoware): add point cloud messages and accessors --- applications/autoware/common_types/README.md | 23 +++++++------------ applications/autoware/common_types/src/lib.rs | 2 +- .../src/{point.rs => point_cloud.rs} | 12 ++++------ .../src/{point => point_cloud}/cloud.rs | 18 +++++++-------- .../src/{point => point_cloud}/field.rs | 10 ++++---- .../accessor.rs => point_cloud/point.rs} | 22 +++++++++--------- .../src/{point => point_cloud}/selection.rs | 14 +++++------ .../{point => point_cloud}/test_support.rs | 6 ++--- 8 files changed, 48 insertions(+), 59 deletions(-) rename applications/autoware/common_types/src/{point.rs => point_cloud.rs} (97%) rename applications/autoware/common_types/src/{point => point_cloud}/cloud.rs (97%) rename applications/autoware/common_types/src/{point => point_cloud}/field.rs (98%) rename applications/autoware/common_types/src/{point/accessor.rs => point_cloud/point.rs} (97%) rename applications/autoware/common_types/src/{point => point_cloud}/selection.rs (98%) rename applications/autoware/common_types/src/{point => point_cloud}/test_support.rs (91%) diff --git a/applications/autoware/common_types/README.md b/applications/autoware/common_types/README.md index 0a71b213f..87fe3e1eb 100644 --- a/applications/autoware/common_types/README.md +++ b/applications/autoware/common_types/README.md @@ -1,21 +1,16 @@ # Autoware common types -This `no_std` crate defines shared Autoware message, geometry, transform, and -point-cloud access types. It uses `alloc`. +This `no_std` crate defines shared Autoware message and access types. It uses `alloc`. ## Data types -### Messages and geometry +### Messages | Type | Purpose | | --- | --- | | `Header` | A `frame_id` and `timestamp` attached to a message. | -| `PointCloud` | A public, ROS-style binary point-cloud message: header, dimensions, field descriptors, byte order, point and row strides, data, and density flag. Its public fields are not validated on construction. | +| `PointCloud` | A public, ROS-style binary point-cloud message: header, dimensions, field descriptors, byte order, point and row strides, data, and density flag. Its public fields are not validated until an accessor is constructed. | | `PointField` | A message descriptor for one named field: its byte offset, `FieldDatatype`, and number of consecutive values per point. Offsets and counts are `u32`. | -| `FieldDatatype` | The runtime encoding of a field: signed or unsigned 8-, 16-, or 32-bit integers, `f32`, or `f64`. | -| `PointFieldValue` | A sealed trait implemented by those eight Rust scalar types. It provides their datatype, size, and native-endian encoding and decoding. | -| `Point32` | A `Vector3` used as a polygon vertex. | -| `Polygon` / `PolygonStamped` | A sequence of 3D vertices, optionally paired with a `Header`. | | `Transform` | A rigid transform with `f64` translation and a unit quaternion. `try_new` validates finite inputs and a near-unit quaternion, then normalizes it. | | `TransformStamped` | A transform accompanied by a header and child frame ID. | | `TransformListener` / `MockTransformListener` | An interface for transform lookup and an in-memory implementation for tests or other callers. | @@ -27,7 +22,8 @@ compatibility helper; prefer explicit conversion at the I/O boundary. ### Point-cloud access Point-cloud messages might not be well-formed and their fields could be corrupted. -To ensure a safe point-cloud manipulation, we provide **accessors** that uses Rust's typesystem to ensure a correct behavior. +To ensure safe point-cloud manipulation, the crate provides accessors that validate +the layout before allowing access. The accessor types borrow a message and validate its layout before access. @@ -64,7 +60,7 @@ classDiagram #### Layout and selection rules -Cloud construction requires native byte order, a nonzero `point_step`, +Constructing an accessor requires native byte order, a nonzero `point_step`, `row_step >= width * point_step`, and exactly `row_step * height` data bytes. Row padding is allowed. Selected field ranges must fit inside a point; field overlaps and zero-count fields are allowed. A point is accessed by row-major @@ -73,7 +69,7 @@ index or by `(row, column)`; its bytes exclude row padding. * `PointCloudReader::try_new(&cloud, None)` resolves every field descriptor. * `Some(&[(name, datatype), ...])` resolves only the requested fields, in request order. The first descriptor with each name is used; a missing name - or datatype mismatch is an error. Unselected descriptors are not validated. + or datatype mismatch is an error. **Unselected descriptors are not validated**. * `Some(&[])` resolves no fields but still validates the cloud layout and permits access to raw point bytes. @@ -139,7 +135,7 @@ before mutation. Writes occur left to right, so a later tuple element wins if fields overlap or a field is selected more than once. ```rust -use common_types::point::{Field, PointCloud, PointCloudError, PointCloudReader, PointCloudWriter}; +use common_types::point_cloud::{Field, PointCloud, PointCloudError, PointCloudReader, PointCloudWriter}; fn read_xyz(cloud: &PointCloud) -> Result<(f32, f32, f32), PointCloudError> { let reader = PointCloudReader::try_new_typed( @@ -171,6 +167,3 @@ cargo clippy -p common_types --all-targets --all-features -- -D warnings Tests cover layout and typed access, ordered overlapping writes, empty clouds, transforms, metadata, extra values, unaligned fields, and padded input rows. - -See the [crop box filter](../crop_box_filter/README.md) for a consumer of these -types. diff --git a/applications/autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs index 737a518f7..74daad414 100644 --- a/applications/autoware/common_types/src/lib.rs +++ b/applications/autoware/common_types/src/lib.rs @@ -17,7 +17,7 @@ extern crate alloc; pub mod geometry; -pub mod point; +pub mod point_cloud; pub mod transform; #[derive(Debug, Clone)] diff --git a/applications/autoware/common_types/src/point.rs b/applications/autoware/common_types/src/point_cloud.rs similarity index 97% rename from applications/autoware/common_types/src/point.rs rename to applications/autoware/common_types/src/point_cloud.rs index 48ab30571..3c87a9852 100644 --- a/applications/autoware/common_types/src/point.rs +++ b/applications/autoware/common_types/src/point_cloud.rs @@ -1,11 +1,10 @@ -mod accessor; mod cloud; mod field; +mod point; mod selection; #[cfg(test)] mod test_support; -pub use accessor::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}; pub use cloud::{ PointCloudAccessor, PointCloudReader, PointCloudWriter, TypedPointCloudAccessor, TypedPointCloudReader, TypedPointCloudWriter, @@ -13,6 +12,7 @@ pub use cloud::{ pub use field::{ CheckedField, FieldAccessor, PointFieldValueMut, TypedCheckedField, TypedFieldAccessor, }; +pub use point::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}; pub use selection::{Field, FieldSelection}; use crate::Header; @@ -33,7 +33,7 @@ pub enum FieldDatatype { } impl FieldDatatype { - pub const fn size(self) -> u32 { + pub const fn size(self) -> usize { match self { Self::Int8 | Self::Uint8 => 1, Self::Int16 | Self::Uint16 => 2, @@ -53,11 +53,9 @@ pub trait PointFieldValue: Copy + sealed::Sealed { type Bytes: AsRef<[u8]>; /// Decodes a native-endian value; returns an error if `bytes.len() != SIZE`. - /// - /// # Errors - /// - /// Returns [`PointFieldError::InvalidLength`] for a non-scalar byte length. fn from_ne_slice(bytes: &[u8]) -> Result; + + /// Encodes the value as native-endian bytes. fn to_ne_bytes(self) -> Self::Bytes; } diff --git a/applications/autoware/common_types/src/point/cloud.rs b/applications/autoware/common_types/src/point_cloud/cloud.rs similarity index 97% rename from applications/autoware/common_types/src/point/cloud.rs rename to applications/autoware/common_types/src/point_cloud/cloud.rs index 365bf139e..00ee54eef 100644 --- a/applications/autoware/common_types/src/point/cloud.rs +++ b/applications/autoware/common_types/src/point_cloud/cloud.rs @@ -5,8 +5,8 @@ use core::{ }; use super::{ - accessor::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}, field::CheckedField, + point::{PointAccessor, PointReader, PointWriter, TypedPointReader, TypedPointWriter}, selection::FieldSelection, FieldDatatype, PointCloud, PointCloudError, PointFieldError, }; @@ -31,7 +31,7 @@ pub type PointCloudWriter<'a> = PointCloudAccessor<&'a mut PointCloud>; /// [`Self::point`] or [`Self::point_at`]: /// /// ``` -/// use common_types::point::{FieldDatatype, PointCloud, PointCloudError, PointCloudWriter}; +/// use common_types::point_cloud::{FieldDatatype, PointCloud, PointCloudError, PointCloudWriter}; /// /// fn update_first_normal(cloud: &mut PointCloud) -> Result<(), PointCloudError> { /// let mut writer = PointCloudWriter::try_new( @@ -333,8 +333,8 @@ impl DerefMut for TypedPointCloudAccessor { #[cfg(test)] mod tests { use super::*; - use crate::point::test_support::{message, projected_cloud}; - use crate::point::*; + use crate::point_cloud::test_support::{point_cloud, projected_cloud}; + use crate::point_cloud::*; use alloc::{vec, vec::Vec}; use core::borrow::Borrow; @@ -473,7 +473,7 @@ mod tests { for mut cloud in [ projected_cloud(), empty, - message(0, 1, 8, 0, vec![], vec![]), + point_cloud(0, 1, 8, 0, vec![], vec![]), ] { let fields: &[(&'static str, FieldDatatype)] = if cloud.fields.is_empty() { &[] @@ -568,7 +568,7 @@ mod tests { #[cfg(target_pointer_width = "32")] #[test] fn rejects_dimension_arithmetic_overflow() { - let cloud = message(u32::MAX, u32::MAX, u32::MAX, u32::MAX, vec![], vec![]); + let cloud = point_cloud(u32::MAX, u32::MAX, u32::MAX, u32::MAX, vec![], vec![]); assert_eq!( PointCloudReader::try_new(&cloud, None).err(), Some(PointCloudError::Overflow) @@ -576,7 +576,7 @@ mod tests { } #[test] fn validates_cloud_shape_and_zero_sized_dimensions() { - let mut cloud = message(1, 1, 0, 0, vec![], vec![]); + let mut cloud = point_cloud(1, 1, 0, 0, vec![], vec![]); assert_eq!( PointCloudReader::try_new(&cloud, None).err(), Some(PointCloudError::ZeroPointStep) @@ -600,14 +600,14 @@ mod tests { }) ); - let zero_width = message(0, 2, 8, 3, vec![], vec![0; 6]); + let zero_width = point_cloud(0, 2, 8, 3, vec![], vec![0; 6]); let reader = PointCloudReader::try_new(&zero_width, None).unwrap(); assert_eq!(reader.point_count(), 0); assert!(matches!( reader.point(0), Err(PointCloudError::PointOutOfBounds { count: 0, .. }) )); - let zero_height = message(3, 0, 8, 24, vec![], vec![]); + let zero_height = point_cloud(3, 0, 8, 24, vec![], vec![]); assert_eq!( PointCloudReader::try_new(&zero_height, None) .unwrap() diff --git a/applications/autoware/common_types/src/point/field.rs b/applications/autoware/common_types/src/point_cloud/field.rs similarity index 98% rename from applications/autoware/common_types/src/point/field.rs rename to applications/autoware/common_types/src/point_cloud/field.rs index 364c50fc6..5feb38045 100644 --- a/applications/autoware/common_types/src/point/field.rs +++ b/applications/autoware/common_types/src/point_cloud/field.rs @@ -26,7 +26,7 @@ impl CheckedField { pub(super) fn try_new(field: PointField, point_step: u32) -> Result { let offset_start = field.offset as usize; let count = field.count as usize; - let item_size = field.datatype.size() as usize; + let item_size = field.datatype.size(); let point_step = point_step as usize; let field_size = count .checked_mul(item_size) @@ -124,8 +124,6 @@ impl TypedCheckedField { fields: &mut core::slice::Iter<'_, CheckedField>, ) -> Result { let field = *fields.next().ok_or(PointCloudError::InvalidData)?; - // Retain a validated type witness even if this trait method is - // invoked directly rather than through the cloud constructor. if field.datatype() != T::DATATYPE { return Err(PointFieldError::DatatypeMismatch { name: field.name(), @@ -368,8 +366,8 @@ impl PointFieldValueMut<'_, T> { #[cfg(test)] mod tests { use super::*; - use crate::point::test_support::{message, projected_cloud}; - use crate::point::*; + use crate::point_cloud::test_support::{point_cloud, projected_cloud}; + use crate::point_cloud::*; use alloc::{vec, vec::Vec}; #[test] @@ -461,7 +459,7 @@ mod tests { } #[test] fn empty_and_overlapping_fields_support_sequential_access() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 4, diff --git a/applications/autoware/common_types/src/point/accessor.rs b/applications/autoware/common_types/src/point_cloud/point.rs similarity index 97% rename from applications/autoware/common_types/src/point/accessor.rs rename to applications/autoware/common_types/src/point_cloud/point.rs index e4556f663..6a10023aa 100644 --- a/applications/autoware/common_types/src/point/accessor.rs +++ b/applications/autoware/common_types/src/point_cloud/point.rs @@ -142,7 +142,7 @@ impl TypedPointWriter<'_, S> { /// selection order, so overlapping or repeated selections are last-wins. /// /// ``` - /// use common_types::point::{ + /// use common_types::point_cloud::{ /// Field, PointCloud, PointCloudError, PointCloudWriter, /// }; /// @@ -165,9 +165,9 @@ impl TypedPointWriter<'_, S> { #[cfg(test)] mod tests { use super::*; - use crate::point::selection::typed_selection; - use crate::point::test_support::{message, projected_cloud}; - use crate::point::*; + use crate::point_cloud::selection::typed_selection; + use crate::point_cloud::test_support::{point_cloud, projected_cloud}; + use crate::point_cloud::*; use alloc::{vec, vec::Vec}; #[test] @@ -250,7 +250,7 @@ mod tests { } #[test] fn typed_point_write_fields_accepts_empty_whole_field_slices() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 4, @@ -277,7 +277,7 @@ mod tests { } #[test] fn typed_point_write_fields_apply_complete_overlapping_fields_in_tuple_order() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 3, @@ -304,7 +304,7 @@ mod tests { } #[test] fn typed_point_write_fields_checks_lengths_and_orders_repeated_fields() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 4, @@ -342,7 +342,7 @@ mod tests { } #[test] fn typed_point_writer_writes_mixed_fields_without_touching_padding_or_neighbors() { - let mut cloud = message( + let mut cloud = point_cloud( 2, 2, 8, @@ -410,7 +410,7 @@ mod tests { } #[test] fn typed_writer_validates_every_value_before_mutating_and_orders_overlaps() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 4, @@ -439,7 +439,7 @@ mod tests { )); assert_eq!(writer.data(), original); - let mut empty = message( + let mut empty = point_cloud( 1, 1, 3, @@ -468,7 +468,7 @@ mod tests { )); assert_eq!(writer.data(), original); - let mut overlap = message( + let mut overlap = point_cloud( 1, 1, 2, diff --git a/applications/autoware/common_types/src/point/selection.rs b/applications/autoware/common_types/src/point_cloud/selection.rs similarity index 98% rename from applications/autoware/common_types/src/point/selection.rs rename to applications/autoware/common_types/src/point_cloud/selection.rs index 2e8bf4310..6e94fcd15 100644 --- a/applications/autoware/common_types/src/point/selection.rs +++ b/applications/autoware/common_types/src/point_cloud/selection.rs @@ -129,8 +129,8 @@ pub(super) mod typed_selection { #[cfg(test)] mod tests { use super::*; - use crate::point::test_support::{message, projected_cloud}; - use crate::point::*; + use crate::point_cloud::test_support::{point_cloud, projected_cloud}; + use crate::point_cloud::*; use alloc::{vec, vec::Vec}; #[test] @@ -144,7 +144,7 @@ mod tests { data.extend_from_slice(&60_000_u32.to_ne_bytes()); data.extend_from_slice(&1.25_f32.to_ne_bytes()); data.extend_from_slice(&2.5_f64.to_ne_bytes()); - let cloud = message( + let cloud = point_cloud( 1, 1, 27, @@ -187,7 +187,7 @@ mod tests { } #[test] fn selected_duplicate_name_uses_first_descriptor() { - let cloud = message( + let cloud = point_cloud( 1, 1, 2, @@ -236,7 +236,7 @@ mod tests { } #[test] fn selections_validate_requested_fields_and_preserve_order_and_bytes() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 4, @@ -329,7 +329,7 @@ mod tests { } #[test] fn tuple_duplicates_overlap_and_zero_counts_are_read_only() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 1, 2, @@ -493,7 +493,7 @@ mod tests { } #[test] fn typed_writer_checks_wrong_types_for_empty_clouds() { - let mut cloud = message( + let mut cloud = point_cloud( 1, 0, 4, diff --git a/applications/autoware/common_types/src/point/test_support.rs b/applications/autoware/common_types/src/point_cloud/test_support.rs similarity index 91% rename from applications/autoware/common_types/src/point/test_support.rs rename to applications/autoware/common_types/src/point_cloud/test_support.rs index 55777bff4..d6c3f15ef 100644 --- a/applications/autoware/common_types/src/point/test_support.rs +++ b/applications/autoware/common_types/src/point_cloud/test_support.rs @@ -3,11 +3,11 @@ use alloc::{vec, vec::Vec}; use crate::{ - point::{FieldDatatype, PointCloud, PointField}, + point_cloud::{FieldDatatype, PointCloud, PointField}, Header, }; -pub(super) fn message( +pub(super) fn point_cloud( width: u32, height: u32, point_step: u32, @@ -49,5 +49,5 @@ pub(super) fn projected_cloud() -> PointCloud { data.extend_from_slice(&[0xCC; 3]); } } - message(2, 2, 8, 19, fields, data) + point_cloud(2, 2, 8, 19, fields, data) } From b48efeea6b1782624c54a9e90cd81839993bd443 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Mon, 16 Nov 2026 16:26:33 +0900 Subject: [PATCH 3/6] feat(autoware): add point_writer_from_bytes method --- .../common_types/src/point_cloud/cloud.rs | 69 +++++++++++++++++++ 1 file changed, 69 insertions(+) diff --git a/applications/autoware/common_types/src/point_cloud/cloud.rs b/applications/autoware/common_types/src/point_cloud/cloud.rs index 00ee54eef..65c774db8 100644 --- a/applications/autoware/common_types/src/point_cloud/cloud.rs +++ b/applications/autoware/common_types/src/point_cloud/cloud.rs @@ -278,6 +278,30 @@ pub struct TypedPointCloudAccessor { } impl, S: FieldSelection> TypedPointCloudAccessor { + /// Borrows external point bytes using this accessor's cached field selection. + /// + /// The slice must contain exactly one complete point in native byte order, including point padding but excluding row padding. + /// A different length returns [`PointCloudError::InvalidDataSize`]. + /// No field lookups or datatype checks are repeated, and the cloud's own data remains unchanged. + pub fn point_writer_from_bytes<'a>( + &'a self, + bytes: &'a mut [u8], + ) -> Result, PointCloudError> { + if bytes.len() != self.raw.point_step { + return Err(PointCloudError::InvalidDataSize { + size: bytes.len(), + expected: self.raw.point_step, + }); + } + Ok(TypedPointWriter { + raw: PointAccessor { + bytes, + fields: &self.raw.fields, + }, + checked: &self.checked, + }) + } + pub fn point(&self, index: usize) -> Result, PointCloudError> { let (row, col) = self.raw.point_coordinates(index)?; self.point_at(row, col) @@ -371,6 +395,51 @@ mod tests { assert_eq!(point.fields.as_ptr(), metadata); } + #[test] + fn external_point_writer_reuses_selection_without_changing_source_cloud() { + for empty in [false, true] { + let mut cloud = projected_cloud(); + if empty { + cloud.height = 0; + cloud.data.clear(); + } + let original = cloud.data.clone(); + let reader = PointCloudReader::try_new_typed( + &cloud, + (Field::::new("label"), Field::::new("vector")), + ) + .unwrap(); + let mut bytes = [0xAA; 8]; + { + let mut point = reader.point_writer_from_bytes(&mut bytes).unwrap(); + assert!(core::ptr::eq(point.checked, &reader.checked)); + assert_eq!(point.raw.fields.as_ptr(), reader.raw.fields.as_ptr()); + point.write_values(0, (42, 0x1234)).unwrap(); + } + let mut expected = [0xAA; 8]; + expected[1..3].copy_from_slice(&0x1234_u16.to_ne_bytes()); + expected[7] = 42; + assert_eq!(bytes, expected); + assert_eq!(cloud.data, original); + } + } + + #[test] + fn external_point_writer_rejects_incomplete_or_multiple_points_without_mutation() { + let cloud = projected_cloud(); + let reader = + PointCloudReader::try_new_typed(&cloud, (Field::::new("vector"),)).unwrap(); + for size in [0, 6, 7, 9, 16] { + let mut bytes = vec![0xAA; size]; + let original = bytes.clone(); + assert_eq!( + reader.point_writer_from_bytes(&mut bytes).err(), + Some(PointCloudError::InvalidDataSize { size, expected: 8 }) + ); + assert_eq!(bytes, original); + } + } + #[test] fn typed_writer_reads_and_writes_points_across_padded_rows() { let mut cloud = projected_cloud(); From fb9ae4bc3c585dea0840d8840e55aec817aaccdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Mon, 16 Nov 2026 17:37:36 +0900 Subject: [PATCH 4/6] feat(autoware:point_cloud): implement typed field selection for up to 10 fields --- .../autoware/common_types/src/point_cloud/selection.rs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/applications/autoware/common_types/src/point_cloud/selection.rs b/applications/autoware/common_types/src/point_cloud/selection.rs index 6e94fcd15..9f1193387 100644 --- a/applications/autoware/common_types/src/point_cloud/selection.rs +++ b/applications/autoware/common_types/src/point_cloud/selection.rs @@ -24,7 +24,7 @@ impl Field { } } -/// A sealed tuple selection of one through eight [`Field`] descriptors. +/// A sealed tuple selection of one through ten [`Field`] descriptors. /// /// Supported scalar types are those implementing [`PointFieldValue`]. pub trait FieldSelection: typed_selection::Selection {} @@ -124,6 +124,9 @@ pub(super) mod typed_selection { tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5); tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6); tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6, H: 7); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6, H: 7, I: 8); + tuples!(A: 0, B: 1, C: 2, D: 3, E: 4, F: 5, G: 6, H: 7, I: 8, J: 9); + // In the future, you can add more tuples as needed for larger selections. } #[cfg(test)] From 8aeae6429d5bbf845f98682ff45bc194d7003480 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Mon, 16 Nov 2026 17:38:18 +0900 Subject: [PATCH 5/6] feat(autoware:point_cloud): derive Clone for PointCloud --- .../autoware/common_types/src/point_cloud.rs | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/applications/autoware/common_types/src/point_cloud.rs b/applications/autoware/common_types/src/point_cloud.rs index 3c87a9852..fcb1f7e83 100644 --- a/applications/autoware/common_types/src/point_cloud.rs +++ b/applications/autoware/common_types/src/point_cloud.rs @@ -130,6 +130,12 @@ impl PointField { /// This is a plain message: all fields are public and may contain an invalid layout /// or either byte order. Validated access requires native-endian data. /// Borrow it through [`PointCloudReader`] or [`PointCloudWriter`] before access. +/// +/// # Using `PointCloud` as message +/// +/// While you can send a `PointCloud` as a message, it is recommended to wrap it under +/// an `Arc` to ensure shared ownership and avoid unnecessary copies. +#[derive(Clone)] pub struct PointCloud { /// Time of sensor data acquisition, and the coordinate frame ID (for 3D points). pub header: Header, @@ -154,6 +160,22 @@ pub struct PointCloud { pub is_dense: bool, } +impl core::fmt::Debug for PointCloud { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + f.debug_struct("PointCloud") + .field("header", &self.header) + .field("height", &self.height) + .field("width", &self.width) + .field("fields", &self.fields) + .field("is_bigendian", &self.is_bigendian) + .field("point_step", &self.point_step) + .field("row_step", &self.row_step) + // Skipping the actual data for brevity in debug output + .field("is_dense", &self.is_dense) + .finish() + } +} + mod sealed { pub trait Sealed {} } From 9867345d88786be40a9656517d07c83b3e738e6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rom=C3=A1n=20C=C3=A1rdenas=20Rodr=C3=ADguez?= Date: Mon, 16 Nov 2026 17:43:34 +0900 Subject: [PATCH 6/6] feat(autoware:point_cloud): allow raw data bigger than minimum --- .../autoware/common_types/src/point_cloud.rs | 9 ++++---- .../common_types/src/point_cloud/cloud.rs | 23 ++++++++----------- .../common_types/src/point_cloud/selection.rs | 5 +--- 3 files changed, 15 insertions(+), 22 deletions(-) diff --git a/applications/autoware/common_types/src/point_cloud.rs b/applications/autoware/common_types/src/point_cloud.rs index fcb1f7e83..c91d89082 100644 --- a/applications/autoware/common_types/src/point_cloud.rs +++ b/applications/autoware/common_types/src/point_cloud.rs @@ -262,7 +262,7 @@ pub enum PointCloudError { ZeroPointStep, InvalidRowStep { step: usize, min: usize }, InvalidField(PointFieldError), - InvalidDataSize { size: usize, expected: usize }, + InvalidDataSize { size: usize, min: usize }, RowOutOfBounds { index: usize, height: usize }, PointOutOfBounds { index: usize, count: usize }, ColumnOutOfBounds { index: usize, width: usize }, @@ -280,10 +280,9 @@ impl core::fmt::Display for PointCloudError { "Row step {step} is smaller than the minimum row size {min}" ), Self::InvalidField(error) => write!(f, "Invalid point field: {error}"), - Self::InvalidDataSize { size, expected } => write!( - f, - "Data size {size} does not match expected size {expected}", - ), + Self::InvalidDataSize { size, min } => { + write!(f, "Data size {size} is smaller than the minimumsize {min}",) + } Self::Overflow => write!(f, "Point cloud size overflows usize"), Self::RowOutOfBounds { index, height } => { write!(f, "Row {index} is outside cloud height {height}") diff --git a/applications/autoware/common_types/src/point_cloud/cloud.rs b/applications/autoware/common_types/src/point_cloud/cloud.rs index 65c774db8..e5fdb47c6 100644 --- a/applications/autoware/common_types/src/point_cloud/cloud.rs +++ b/applications/autoware/common_types/src/point_cloud/cloud.rs @@ -94,7 +94,7 @@ impl> PointCloudAccessor { let min_row_step = width .checked_mul(point_step) .ok_or(PointCloudError::Overflow)?; - let expected_size = row_step + let min_size = row_step .checked_mul(height) .ok_or(PointCloudError::Overflow)?; @@ -105,11 +105,11 @@ impl> PointCloudAccessor { min: min_row_step, }); } - // Check for valid data size (must match exactly the expected size) - if message.data.len() != expected_size { + // Check for valid data size (trailing bytes allowed) + if min_size > message.data.len() { return Err(PointCloudError::InvalidDataSize { size: message.data.len(), - expected: expected_size, + min: min_size, }); } @@ -280,17 +280,17 @@ pub struct TypedPointCloudAccessor { impl, S: FieldSelection> TypedPointCloudAccessor { /// Borrows external point bytes using this accessor's cached field selection. /// - /// The slice must contain exactly one complete point in native byte order, including point padding but excluding row padding. + /// The slice must contain at least one complete point in native byte order, including point padding but excluding row padding. /// A different length returns [`PointCloudError::InvalidDataSize`]. /// No field lookups or datatype checks are repeated, and the cloud's own data remains unchanged. pub fn point_writer_from_bytes<'a>( &'a self, bytes: &'a mut [u8], ) -> Result, PointCloudError> { - if bytes.len() != self.raw.point_step { + if self.raw.point_step > bytes.len() { return Err(PointCloudError::InvalidDataSize { size: bytes.len(), - expected: self.raw.point_step, + min: self.raw.point_step, }); } Ok(TypedPointWriter { @@ -429,12 +429,12 @@ mod tests { let cloud = projected_cloud(); let reader = PointCloudReader::try_new_typed(&cloud, (Field::::new("vector"),)).unwrap(); - for size in [0, 6, 7, 9, 16] { + for size in [0, 6, 7] { let mut bytes = vec![0xAA; size]; let original = bytes.clone(); assert_eq!( reader.point_writer_from_bytes(&mut bytes).err(), - Some(PointCloudError::InvalidDataSize { size, expected: 8 }) + Some(PointCloudError::InvalidDataSize { size, min: 8 }) ); assert_eq!(bytes, original); } @@ -663,10 +663,7 @@ mod tests { cloud.data = vec![0; 7]; assert_eq!( PointCloudReader::try_new(&cloud, None).err(), - Some(PointCloudError::InvalidDataSize { - size: 7, - expected: 8, - }) + Some(PointCloudError::InvalidDataSize { size: 7, min: 8 }) ); let zero_width = point_cloud(0, 2, 8, 3, vec![], vec![0; 6]); diff --git a/applications/autoware/common_types/src/point_cloud/selection.rs b/applications/autoware/common_types/src/point_cloud/selection.rs index 9f1193387..b2be902af 100644 --- a/applications/autoware/common_types/src/point_cloud/selection.rs +++ b/applications/autoware/common_types/src/point_cloud/selection.rs @@ -316,10 +316,7 @@ mod tests { cloud.data.pop(); assert_eq!( PointCloudReader::try_new(&cloud, Some(&[])).err(), - Some(PointCloudError::InvalidDataSize { - size: 3, - expected: 4, - }) + Some(PointCloudError::InvalidDataSize { size: 3, min: 4 }) ); cloud.height = 0;