diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md index 77d63b5674fa..6945908a3293 100644 --- a/ARCHITECTURE.md +++ b/ARCHITECTURE.md @@ -233,6 +233,7 @@ Update instructions: | re_mcap | Convert MCAP into Rerun-compatible data. | | re_memory | Run-time memory tracking and profiling. | | re_perf_telemetry | In and out of process performance profiling utilities for Rerun & Redap | +| re_ros_msg | Parsing and deserializing ROS messages | | re_smart_channel | A channel that keeps track of latency and queue length. | | re_span | An integer range that always has a non-negative length | | re_string_interner | Yet another string interning library | diff --git a/Cargo.lock b/Cargo.lock index fc186cd4fe35..2692629f0c49 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -8988,6 +8988,7 @@ dependencies = [ "re_chunk", "re_log", "re_log_types", + "re_ros_msg", "re_tracing", "re_types", "serde", @@ -9300,6 +9301,15 @@ dependencies = [ "winit", ] +[[package]] +name = "re_ros_msg" +version = "0.26.0-alpha.1+dev" +dependencies = [ + "anyhow", + "serde", + "thiserror 1.0.69", +] + [[package]] name = "re_sdk" version = "0.26.0-alpha.1+dev" diff --git a/Cargo.toml b/Cargo.toml index 08ff39738fe9..16efaf4b9ba2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -106,6 +106,7 @@ re_log = { path = "crates/utils/re_log", version = "=0.26.0-alpha.1", default-fe re_mcap = { path = "crates/utils/re_mcap", version = "=0.26.0-alpha.1", default-features = false } re_memory = { path = "crates/utils/re_memory", version = "=0.26.0-alpha.1", default-features = false } re_perf_telemetry = { path = "crates/utils/re_perf_telemetry", version = "=0.26.0-alpha.1", default-features = false } +re_ros_msg = { path = "crates/utils/re_ros_msg", version = "=0.26.0-alpha.1", default-features = false } re_smart_channel = { path = "crates/utils/re_smart_channel", version = "=0.26.0-alpha.1", default-features = false } re_span = { path = "crates/utils/re_span", version = "=0.26.0-alpha.1", default-features = false } re_string_interner = { path = "crates/utils/re_string_interner", version = "=0.26.0-alpha.1", default-features = false } diff --git a/crates/store/re_data_loader/tests/snapshots/test_mcap_loader__tests__ros2.snap b/crates/store/re_data_loader/tests/snapshots/test_mcap_loader__tests__ros2.snap index d5a40c0e19c2..c3b068d7fcf9 100644 --- a/crates/store/re_data_loader/tests/snapshots/test_mcap_loader__tests__ros2.snap +++ b/crates/store/re_data_loader/tests/snapshots/test_mcap_loader__tests__ros2.snap @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8a85ec291c84c9dc8a2632b99e0ec6ebb2773a2a8e6c2c85f65290974c89db5a -size 443136 +oid sha256:b1a4ac6fa84a2a951884291e6e4ea971186c23dd6369b4f4d0538b030ed0b77a +size 449123 diff --git a/crates/utils/re_mcap/Cargo.toml b/crates/utils/re_mcap/Cargo.toml index 8426f5d84513..1c6145e78ed0 100644 --- a/crates/utils/re_mcap/Cargo.toml +++ b/crates/utils/re_mcap/Cargo.toml @@ -18,6 +18,7 @@ workspace = true re_chunk.workspace = true re_log.workspace = true re_log_types.workspace = true +re_ros_msg.workspace = true re_tracing.workspace = true re_types = { workspace = true, features = ["ecolor", "glam", "image", "video"] } diff --git a/crates/utils/re_mcap/src/layers/mod.rs b/crates/utils/re_mcap/src/layers/mod.rs index bfc3821dd8e7..c550fc392d54 100644 --- a/crates/utils/re_mcap/src/layers/mod.rs +++ b/crates/utils/re_mcap/src/layers/mod.rs @@ -2,6 +2,7 @@ mod protobuf; mod raw; mod recording_info; mod ros2; +mod ros2_reflection; mod schema; mod stats; @@ -10,7 +11,8 @@ use std::collections::{BTreeMap, BTreeSet}; pub use self::{ protobuf::McapProtobufLayer, raw::McapRawLayer, recording_info::McapRecordingInfoLayer, - ros2::McapRos2Layer, schema::McapSchemaLayer, stats::McapStatisticLayer, + ros2::McapRos2Layer, ros2_reflection::McapRos2ReflectionLayer, schema::McapSchemaLayer, + stats::McapStatisticLayer, }; use crate::{ @@ -324,6 +326,7 @@ impl LayerRegistry { .register_file_layer::() // message layers (priority order): .register_message_layer::() + .register_message_layer::() .register_message_layer::(); if raw_fallback_enabled { diff --git a/crates/utils/re_mcap/src/layers/raw.rs b/crates/utils/re_mcap/src/layers/raw.rs index c9b255d638ae..55be7b67b7dc 100644 --- a/crates/utils/re_mcap/src/layers/raw.rs +++ b/crates/utils/re_mcap/src/layers/raw.rs @@ -26,6 +26,7 @@ impl MessageParser for RawMcapMessageParser { msg: &::mcap::Message<'_>, ) -> anyhow::Result<()> { re_tracing::profile_function!(); + self.data.values().values().append_slice(&msg.data); self.data.values().append(true); self.data.append(true); diff --git a/crates/utils/re_mcap/src/layers/ros2.rs b/crates/utils/re_mcap/src/layers/ros2.rs index b7147b01cd17..a44e07a6e4dc 100644 --- a/crates/utils/re_mcap/src/layers/ros2.rs +++ b/crates/utils/re_mcap/src/layers/ros2.rs @@ -1,9 +1,9 @@ use std::collections::BTreeMap; use super::MessageLayer; -use crate::{ - parsers::MessageParser, - parsers::ros2msg::{ +use crate::parsers::{ + MessageParser, + ros2msg::{ Ros2MessageParser, rcl_interfaces::LogMessageParser, sensor_msgs::{ @@ -114,6 +114,7 @@ impl MessageLayer for McapRos2Layer { "Message schema {:?} is currently not supported", schema.name ); + None } } diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs new file mode 100644 index 000000000000..a7192faf01d3 --- /dev/null +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -0,0 +1,524 @@ +use std::sync::Arc; + +use anyhow::Context as _; +use arrow::{ + array::{ + ArrayBuilder, ArrowPrimitiveType, BooleanBuilder, FixedSizeListBuilder, Float32Builder, + Float64Builder, Int8Builder, Int16Builder, Int32Builder, Int64Builder, ListBuilder, + PrimitiveBuilder, StringBuilder, StructBuilder, UInt8Builder, UInt16Builder, UInt32Builder, + UInt64Builder, + }, + datatypes::{ + DataType, Field, Fields, Float32Type, Float64Type, Int8Type, Int16Type, Int32Type, + Int64Type, UInt8Type, UInt16Type, UInt32Type, UInt64Type, + }, +}; +use cdr_encoding::CdrDeserializer; +use re_chunk::{ + Chunk, ChunkId, EntityPath, TimeColumn, TimelineName, external::nohash_hasher::IntMap, +}; +use re_ros_msg::{ + MessageSchema, + deserialize::{MapResolver, MessageSeed, Value, primitive_array::PrimitiveArray}, + message_spec::{ArraySize, BuiltInType, ComplexType, MessageSpecification, Type}, +}; +use re_types::{ComponentDescriptor, reflection::ComponentDescriptorExt as _}; +use serde::de::DeserializeSeed as _; + +use crate::parsers::{MessageParser, ParserContext, dds}; +use crate::{Error, LayerIdentifier, MessageLayer}; + +pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { + // 4-byte encapsulation header + if buf.len() < 4 { + anyhow::bail!("short encapsulation"); + } + + let representation_identifier = dds::RepresentationIdentifier::from_bytes([buf[0], buf[1]]) + .with_context(|| "failed to parse CDR representation identifier")?; + + let resolver = MapResolver::new(top.dependencies.iter().map(|dep| (dep.name.clone(), dep))); + + let seed = MessageSeed::new(&top.spec, &resolver); + + if representation_identifier.is_big_endian() { + let mut de = CdrDeserializer::::new(&buf[4..]); + seed.deserialize(&mut de) + .with_context(|| "failed to deserialize CDR message") + } else { + let mut de = CdrDeserializer::::new(&buf[4..]); + seed.deserialize(&mut de) + .with_context(|| "failed to deserialize CDR message") + } +} + +struct Ros2ReflectionMessageParser { + num_rows: usize, + message_schema: MessageSchema, + fields: Vec<(String, FixedSizeListBuilder>)>, +} + +#[derive(Debug, thiserror::Error)] +pub enum Ros2ReflectionError { + #[error("Invalid message on channel {channel} for schema {schema}: {source}")] + InvalidMessage { + schema: String, + channel: String, + source: anyhow::Error, + }, + + #[error("Failed to downcast builder to expected type: {0}")] + Downcast(&'static str), +} + +impl Ros2ReflectionMessageParser { + fn new(num_rows: usize, message_schema: MessageSchema) -> anyhow::Result { + let mut fields = Vec::new(); + + // Build Arrow builders for each field in the message, preserving order + for field in &message_schema.spec.fields { + let name = field.name.clone(); + let builder = arrow_builder_from_type(&field.ty, &message_schema.dependencies)?; + fields.push(( + name.clone(), + FixedSizeListBuilder::with_capacity(builder, 1, num_rows), + )); + } + + Ok(Self { + num_rows, + message_schema, + fields, + }) + } +} + +impl MessageParser for Ros2ReflectionMessageParser { + fn append(&mut self, _ctx: &mut ParserContext, msg: &mcap::Message<'_>) -> anyhow::Result<()> { + re_tracing::profile_function!(); + + let value = decode_bytes(&self.message_schema, msg.data.as_ref()).map_err(|err| { + Ros2ReflectionError::InvalidMessage { + schema: self.message_schema.spec.name.clone(), + channel: msg.channel.topic.clone(), + source: err, + } + })?; + + if let Value::Message(message_fields) = value { + // We always need to make sure to iterate over all our builders, adding null values whenever + // a field is missing from the message that we received. + for (field_name, builder) in &mut self.fields { + if let Some(field_value) = message_fields.get(field_name) { + append_value(builder.values(), field_value, &self.message_schema)?; + builder.append(true); + } else { + builder.append(false); + } + } + } else { + return Err(anyhow::anyhow!("Expected message value, got {:?}", value)); + } + + Ok(()) + } + + fn finalize(self: Box, ctx: ParserContext) -> anyhow::Result> { + re_tracing::profile_function!(); + let entity_path = ctx.entity_path().clone(); + let timelines = ctx.build_timelines(); + + let Self { + num_rows, + message_schema, + fields, + } = *self; + + let archetype_name = message_schema.spec.name.clone().replace('/', "."); + + if fields.is_empty() { + return create_empty_message_chunk(entity_path, timelines, num_rows, &archetype_name) + .map(|chunk| vec![chunk]); + } + + let message_chunk = Chunk::from_auto_row_ids( + ChunkId::new(), + entity_path, + timelines, + fields + .into_iter() + .map(|(field_name, mut builder)| { + ( + ComponentDescriptor::partial(field_name) + .with_builtin_archetype(archetype_name.clone()), + builder.finish().into(), + ) + }) + .collect(), + ) + .map_err(|err| Error::Other(anyhow::anyhow!(err)))?; + + Ok(vec![message_chunk]) + } +} + +fn create_empty_message_chunk( + entity_path: EntityPath, + timelines: IntMap, + num_rows: usize, + archetype_name: &str, +) -> anyhow::Result { + let empty_list = arrow::array::ListArray::new_null( + std::sync::Arc::new(Field::new( + "empty", + DataType::FixedSizeList(Arc::new(Field::new("item", DataType::Null, true)), 1), + true, + )), + num_rows, + ); + + let chunk = Chunk::from_auto_row_ids( + ChunkId::new(), + entity_path, + timelines, + std::iter::once(( + ComponentDescriptor::partial("empty").with_builtin_archetype(archetype_name), + empty_list, + )) + .collect(), + ) + .map_err(|err| Error::Other(anyhow::anyhow!(err)))?; + Ok(chunk) +} + +fn downcast_builder( + builder: &mut dyn ArrayBuilder, +) -> Result<&mut T, Ros2ReflectionError> { + builder.as_any_mut().downcast_mut::().ok_or_else(|| { + let type_name = std::any::type_name::(); + Ros2ReflectionError::Downcast(type_name.strip_suffix("Builder").unwrap_or(type_name)) + }) +} + +fn append_slice_to_list( + builder: &mut dyn ArrayBuilder, + vec: &[T::Native], +) -> Result<(), Ros2ReflectionError> +where + T: ArrowPrimitiveType, + PrimitiveBuilder: 'static, +{ + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::>(list_builder.values())?; + values_builder.append_slice(vec); + list_builder.append(true); + Ok(()) +} + +fn append_primitive_array( + builder: &mut dyn ArrayBuilder, + prim_array: &PrimitiveArray, +) -> Result<(), Ros2ReflectionError> { + match prim_array { + PrimitiveArray::Bool(vec) => { + // `Bool` is a special case since Arrow doesn't have a primitive boolean array + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::(list_builder.values())?; + values_builder.append_slice(vec); + list_builder.append(true); + Ok(()) + } + PrimitiveArray::I8(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::U8(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::I16(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::U16(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::I32(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::U32(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::I64(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::U64(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::F32(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::F64(vec) => append_slice_to_list::(builder, vec), + PrimitiveArray::String(items) => { + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::(list_builder.values())?; + for item in items { + values_builder.append_value(item); + } + list_builder.append(true); + Ok(()) + } + } +} + +fn append_value( + builder: &mut dyn ArrayBuilder, + val: &Value, + schema: &MessageSchema, +) -> Result<(), Ros2ReflectionError> { + match val { + Value::Bool(x) => downcast_builder::(builder)?.append_value(*x), + Value::I8(x) => downcast_builder::(builder)?.append_value(*x), + Value::U8(x) => downcast_builder::(builder)?.append_value(*x), + Value::I16(x) => downcast_builder::(builder)?.append_value(*x), + Value::U16(x) => downcast_builder::(builder)?.append_value(*x), + Value::I32(x) => downcast_builder::(builder)?.append_value(*x), + Value::U32(x) => downcast_builder::(builder)?.append_value(*x), + Value::I64(x) => downcast_builder::(builder)?.append_value(*x), + Value::U64(x) => downcast_builder::(builder)?.append_value(*x), + Value::F32(x) => downcast_builder::(builder)?.append_value(*x), + Value::F64(x) => downcast_builder::(builder)?.append_value(*x), + Value::String(x) => { + downcast_builder::(builder)?.append_value(x.clone()); + } + Value::Message(message_fields) => { + let struct_builder = downcast_builder::(builder)?; + + // For nested messages, we need to find the matching specification from dependencies + // Since we don't have type information here, we'll try to match by field names + let matching_spec = find_matching_message_spec(schema, message_fields); + + if let Some(spec) = matching_spec { + // Use the specification field order to iterate through struct builder fields + for (i, spec_field) in spec.fields.iter().enumerate() { + if let Some(field_builder) = struct_builder.field_builders_mut().get_mut(i) { + if let Some(field_value) = message_fields.get(&spec_field.name) { + append_value(field_builder, field_value, schema)?; + } else { + //TODO(gijsd): Field is missing in the message, append null + re_log::warn_once!( + "Field {} is missing in the message, appending null", + spec_field.name + ); + } + } + } + } else { + re_log::warn_once!( + "Could not find matching message specification for nested message with fields: {:?}", + message_fields.keys().collect::>() + ); + } + + struct_builder.append(true); + } + Value::Array(vec) | Value::Sequence(vec) => { + let list_builder = downcast_builder::>>(builder)?; + + for val in vec { + append_value(list_builder.values(), val, schema)?; + } + list_builder.append(true); + } + Value::PrimitiveArray(prim_array) | Value::PrimitiveSeq(prim_array) => { + append_primitive_array(builder, prim_array)?; + } + } + + Ok(()) +} + +fn find_matching_message_spec<'a>( + schema: &'a MessageSchema, + message_fields: &'a std::collections::BTreeMap, +) -> Option<&'a MessageSpecification> { + schema.dependencies.iter().find(|spec| { + spec.fields.len() == message_fields.len() + && spec + .fields + .iter() + .all(|f| message_fields.contains_key(&f.name)) + }) +} + +fn struct_builder_from_message_spec( + spec: &MessageSpecification, + dependencies: &[MessageSpecification], +) -> anyhow::Result { + let fields = spec + .fields + .iter() + .map(|f| { + Ok(( + arrow_field_from_type(&f.ty, &f.name, dependencies)?, + arrow_builder_from_type(&f.ty, dependencies)?, + )) + }) + .collect::>>()?; + + let (fields, field_builders): (Vec, Vec>) = + fields.into_iter().unzip(); + + Ok(StructBuilder::new(fields, field_builders)) +} + +fn arrow_builder_from_type( + ty: &Type, + dependencies: &[MessageSpecification], +) -> anyhow::Result> { + Ok(match ty { + Type::BuiltIn(p) => match p { + BuiltInType::Bool => Box::new(BooleanBuilder::new()), + BuiltInType::Byte | BuiltInType::UInt8 => Box::new(UInt8Builder::new()), + BuiltInType::Char | BuiltInType::Int8 => Box::new(Int8Builder::new()), + BuiltInType::Int16 => Box::new(Int16Builder::new()), + BuiltInType::UInt16 => Box::new(UInt16Builder::new()), + BuiltInType::Int32 => Box::new(Int32Builder::new()), + BuiltInType::UInt32 => Box::new(UInt32Builder::new()), + BuiltInType::Int64 => Box::new(Int64Builder::new()), + BuiltInType::UInt64 => Box::new(UInt64Builder::new()), + BuiltInType::Float32 => Box::new(Float32Builder::new()), + BuiltInType::Float64 => Box::new(Float64Builder::new()), + BuiltInType::String(_) | BuiltInType::WString(_) => Box::new(StringBuilder::new()), + }, + Type::Complex(complex_type) => { + // Look up the message spec in dependencies + let spec = resolve_complex_type(complex_type, dependencies).ok_or_else(|| { + anyhow::anyhow!("Could not resolve complex type: {:?}", complex_type) + })?; + Box::new(struct_builder_from_message_spec(spec, dependencies)?) + } + Type::Array { ty, .. } => { + Box::new(ListBuilder::new(arrow_builder_from_type(ty, dependencies)?)) + } + }) +} + +fn arrow_field_from_type( + ty: &Type, + name: &str, + dependencies: &[MessageSpecification], +) -> anyhow::Result { + datatype_from_type(ty, dependencies).map(|data_type| Field::new(name, data_type, true)) +} + +fn datatype_from_type( + ty: &Type, + dependencies: &[MessageSpecification], +) -> anyhow::Result { + Ok(match ty { + Type::BuiltIn(p) => match p { + BuiltInType::Bool => DataType::Boolean, + BuiltInType::Byte | BuiltInType::UInt8 => DataType::UInt8, + BuiltInType::Char | BuiltInType::Int8 => DataType::Int8, + BuiltInType::Int16 => DataType::Int16, + BuiltInType::UInt16 => DataType::UInt16, + BuiltInType::Int32 => DataType::Int32, + BuiltInType::UInt32 => DataType::UInt32, + BuiltInType::Int64 => DataType::Int64, + BuiltInType::UInt64 => DataType::UInt64, + BuiltInType::Float32 => DataType::Float32, + BuiltInType::Float64 => DataType::Float64, + BuiltInType::String(_) | BuiltInType::WString(_) => DataType::Utf8, // No wstring in Arrow + }, + Type::Complex(complex_type) => { + let spec = resolve_complex_type(complex_type, dependencies).ok_or_else(|| { + anyhow::anyhow!("Could not resolve complex type: {:?}", complex_type) + })?; + let fields = spec + .fields + .iter() + .map(|f| arrow_field_from_type(&f.ty, &f.name, dependencies)) + .collect::>()?; + DataType::Struct(fields) + } + Type::Array { ty, size } => match size { + ArraySize::Fixed(_) | ArraySize::Bounded(_) | ArraySize::Unbounded => { + DataType::new_list(datatype_from_type(ty, dependencies)?, true) + } + }, + }) +} + +fn resolve_complex_type<'a>( + complex_type: &ComplexType, + dependencies: &'a [MessageSpecification], +) -> Option<&'a MessageSpecification> { + dependencies.iter().find(|spec| match complex_type { + ComplexType::Absolute { package, name } => { + spec.name == format!("{package}/{name}") || spec.name == *name + } + ComplexType::Relative { name } => { + spec.name == *name || spec.name.ends_with(&format!("/{name}")) + } + }) +} + +/// Provides reflection-based conversion of ROS2-encoded MCAP messages. +/// +/// This layer dynamically parses ROS2 messages at runtime, allowing for +/// a direct arrow representation of the messages fields, similar to the protobuf layer. +#[derive(Debug, Default)] +pub struct McapRos2ReflectionLayer { + schemas_per_topic: ahash::HashMap, +} + +impl MessageLayer for McapRos2ReflectionLayer { + fn identifier() -> LayerIdentifier { + "ros2_reflection".into() + } + + fn init(&mut self, summary: &mcap::Summary) -> Result<(), Error> { + for channel in summary.channels.values() { + let schema = channel + .schema + .as_ref() + .ok_or(Error::NoSchema(channel.topic.clone()))?; + + if schema.encoding.as_str() != "ros2msg" { + continue; + } + + let schema_content = String::from_utf8_lossy(schema.data.as_ref()); + let message_schema = + MessageSchema::parse(&schema.name, &schema_content).map_err(|err| { + Error::InvalidSchema { + schema: schema.name.clone(), + source: err, + } + })?; + + let found = self + .schemas_per_topic + .insert(channel.topic.clone(), message_schema); + + debug_assert!( + found.is_none(), + "Duplicate schema for topic {}", + channel.topic + ); + } + + Ok(()) + } + + fn supports_channel(&self, channel: &mcap::Channel<'_>) -> bool { + let Some(schema) = channel.schema.as_ref() else { + return false; + }; + + if schema.encoding.as_str() != "ros2msg" { + return false; + } + + // Only support channels if the semantic layer doesn't support them + // First check if we have parsed the schema successfully + if !self.schemas_per_topic.contains_key(&channel.topic) { + return false; + } + + // Check if the semantic layer would handle this message type + let semantic_layer = super::McapRos2Layer::new(); + !semantic_layer.supports_schema(&schema.name) + } + + fn message_parser( + &self, + channel: &mcap::Channel<'_>, + num_rows: usize, + ) -> Option> { + let message_schema = self.schemas_per_topic.get(&channel.topic)?; + Some(Box::new( + Ros2ReflectionMessageParser::new(num_rows, message_schema.clone()).ok()?, + )) + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/camera_info.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/camera_info.rs index d6bab94cdf5f..fbc3a1225e9b 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/camera_info.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/camera_info.rs @@ -1,13 +1,6 @@ use super::super::definitions::sensor_msgs; -use arrow::{ - array::{BooleanBuilder, StructBuilder}, - datatypes::Field, -}; -use re_chunk::{ - Chunk, ChunkId, - external::arrow::array::{FixedSizeListBuilder, Float64Builder, StringBuilder, UInt32Builder}, -}; -use re_types::{ComponentDescriptor, archetypes::Pinhole, reflection::ComponentDescriptorExt as _}; +use re_chunk::{Chunk, ChunkId}; +use re_types::archetypes::Pinhole; use super::super::Ros2MessageParser; use crate::{ @@ -15,7 +8,6 @@ use crate::{ parsers::{ cdr, decode::{MessageParser, ParserContext}, - util::fixed_size_list_builder, }, }; @@ -24,58 +16,13 @@ use crate::{ pub struct CameraInfoSchemaPlugin; pub struct CameraInfoMessageParser { - distortion_models: FixedSizeListBuilder, - k_matrices: FixedSizeListBuilder, - d_coefficients: Vec>, - r_matrices: FixedSizeListBuilder, - p_matrices: FixedSizeListBuilder, - widths: FixedSizeListBuilder, - heights: FixedSizeListBuilder, - binning_x: FixedSizeListBuilder, - binning_y: FixedSizeListBuilder, - rois: FixedSizeListBuilder, - frame_ids: FixedSizeListBuilder, image_from_cameras: Vec<[f32; 9]>, resolutions: Vec<(f32, f32)>, } -impl CameraInfoMessageParser { - const ARCHETYPE_NAME: &str = "sensor_msgs.msg.CameraInfo"; -} - impl Ros2MessageParser for CameraInfoMessageParser { fn new(num_rows: usize) -> Self { Self { - distortion_models: fixed_size_list_builder(1, num_rows), - k_matrices: fixed_size_list_builder(9, num_rows), - d_coefficients: Vec::with_capacity(num_rows), - r_matrices: fixed_size_list_builder(9, num_rows), - p_matrices: fixed_size_list_builder(12, num_rows), - widths: fixed_size_list_builder(1, num_rows), - heights: fixed_size_list_builder(1, num_rows), - binning_x: fixed_size_list_builder(1, num_rows), - binning_y: fixed_size_list_builder(1, num_rows), - rois: FixedSizeListBuilder::with_capacity( - StructBuilder::new( - vec![ - Field::new("x_offset", arrow::datatypes::DataType::UInt32, false), - Field::new("y_offset", arrow::datatypes::DataType::UInt32, false), - Field::new("width", arrow::datatypes::DataType::UInt32, false), - Field::new("height", arrow::datatypes::DataType::UInt32, false), - Field::new("do_rectify", arrow::datatypes::DataType::Boolean, false), - ], - vec![ - Box::new(UInt32Builder::new()), - Box::new(UInt32Builder::new()), - Box::new(UInt32Builder::new()), - Box::new(UInt32Builder::new()), - Box::new(BooleanBuilder::new()), - ], - ), - 1, - num_rows, - ), - frame_ids: fixed_size_list_builder(1, num_rows), image_from_cameras: Vec::with_capacity(num_rows), resolutions: Vec::with_capacity(num_rows), } @@ -88,14 +35,8 @@ impl MessageParser for CameraInfoMessageParser { header, width, height, - distortion_model, - d, k, - r, - p, - binning_x, - binning_y, - roi, + .. } = cdr::try_decode_message::(&msg.data)?; // add the sensor timestamp to the context, `log_time` and `publish_time` are added automatically @@ -103,66 +44,6 @@ impl MessageParser for CameraInfoMessageParser { header.stamp.as_nanos() as u64, )); - self.distortion_models - .values() - .append_value(&distortion_model); - self.distortion_models.append(true); - self.k_matrices.values().append_slice(&k); - self.k_matrices.append(true); - - self.r_matrices.values().append_slice(&r); - self.r_matrices.append(true); - - self.p_matrices.values().append_slice(&p); - self.p_matrices.append(true); - - self.d_coefficients.push(d); - - self.widths.values().append_value(width); - self.widths.append(true); - - self.heights.values().append_value(height); - self.heights.append(true); - - self.binning_x.values().append_value(binning_x); - self.binning_x.append(true); - - self.binning_y.values().append_value(binning_y); - self.binning_y.append(true); - - self.frame_ids.values().append_value(&header.frame_id); - self.frame_ids.append(true); - - let struct_builder = self.rois.values(); - - struct_builder - .field_builder::(0) - .expect("has to exist") - .append_value(roi.x_offset); - - struct_builder - .field_builder::(1) - .expect("has to exist") - .append_value(roi.y_offset); - - struct_builder - .field_builder::(2) - .expect("has to exist") - .append_value(roi.width); - - struct_builder - .field_builder::(3) - .expect("has to exist") - .append_value(roi.height); - - struct_builder - .field_builder::(4) - .expect("has to exist") - .append_value(roi.do_rectify); - - struct_builder.append(true); - self.rois.append(true); - // ROS2 stores the intrinsic matrix K as a row-major 9-element array: // [fx, 0, cx, 0, fy, cy, 0, 0, 1] // this corresponds to the matrix: @@ -188,17 +69,6 @@ impl MessageParser for CameraInfoMessageParser { fn finalize(self: Box, ctx: ParserContext) -> anyhow::Result> { let Self { - mut distortion_models, - mut k_matrices, - mut r_matrices, - mut p_matrices, - d_coefficients, - mut widths, - mut heights, - mut binning_x, - mut binning_y, - mut frame_ids, - mut rois, image_from_cameras, resolutions, } = *self; @@ -206,76 +76,6 @@ impl MessageParser for CameraInfoMessageParser { let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); - let d_array = { - let mut list_builder = arrow::array::ListBuilder::new(Float64Builder::new()); - for d_vec in d_coefficients { - list_builder.values().append_slice(&d_vec); - list_builder.append(true); - } - list_builder.finish() - }; - - let chunk = Chunk::from_auto_row_ids( - ChunkId::new(), - entity_path.clone(), - timelines.clone(), - [ - ( - ComponentDescriptor::partial("distortion_model") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - distortion_models.finish().into(), - ), - ( - ComponentDescriptor::partial("k").with_builtin_archetype(Self::ARCHETYPE_NAME), - k_matrices.finish().into(), - ), - ( - ComponentDescriptor::partial("width") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - widths.finish().into(), - ), - ( - ComponentDescriptor::partial("height") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - heights.finish().into(), - ), - ( - ComponentDescriptor::partial("d").with_builtin_archetype(Self::ARCHETYPE_NAME), - d_array, - ), - ( - ComponentDescriptor::partial("r").with_builtin_archetype(Self::ARCHETYPE_NAME), - r_matrices.finish().into(), - ), - ( - ComponentDescriptor::partial("p").with_builtin_archetype(Self::ARCHETYPE_NAME), - p_matrices.finish().into(), - ), - ( - ComponentDescriptor::partial("binning_x") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - binning_x.finish().into(), - ), - ( - ComponentDescriptor::partial("binning_y") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - binning_y.finish().into(), - ), - ( - ComponentDescriptor::partial("roi") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - rois.finish().into(), - ), - ( - ComponentDescriptor::partial("frame_id") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - frame_ids.finish().into(), - ), - ] - .into_iter() - .collect(), - )?; - let pinhole_chunk = Chunk::from_auto_row_ids( ChunkId::new(), entity_path.clone(), @@ -288,6 +88,6 @@ impl MessageParser for CameraInfoMessageParser { .collect(), )?; - Ok(vec![chunk, pinhole_chunk]) + Ok(vec![pinhole_chunk]) } } diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/compressed_image.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/compressed_image.rs index 6e490cbfa7c1..6586793eb348 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/compressed_image.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/compressed_image.rs @@ -1,13 +1,8 @@ use super::super::definitions::sensor_msgs; -use re_chunk::{ - Chunk, ChunkId, RowId, TimePoint, - external::arrow::array::{FixedSizeListBuilder, StringBuilder}, -}; +use re_chunk::{Chunk, ChunkId, RowId, TimePoint}; use re_types::{ - ComponentDescriptor, archetypes::{EncodedImage, VideoStream}, components::VideoCodec, - reflection::ComponentDescriptorExt as _, }; use super::super::Ros2MessageParser; @@ -23,19 +18,13 @@ pub struct CompressedImageMessageParser { /// /// Note: These blobs are directly moved into a `Blob`, without copying. blobs: Vec>, - formats: FixedSizeListBuilder, is_h264: bool, } -impl CompressedImageMessageParser { - const ARCHETYPE_NAME: &str = "sensor_msgs.msg.CompressedImage"; -} - impl Ros2MessageParser for CompressedImageMessageParser { fn new(num_rows: usize) -> Self { Self { blobs: Vec::with_capacity(num_rows), - formats: FixedSizeListBuilder::with_capacity(StringBuilder::new(), 1, num_rows), is_h264: false, } } @@ -62,19 +51,12 @@ impl MessageParser for CompressedImageMessageParser { self.is_h264 = true; } - self.formats.values().append_value(format.as_str()); - self.formats.append(true); - Ok(()) } fn finalize(self: Box, ctx: ParserContext) -> anyhow::Result> { re_tracing::profile_function!(); - let Self { - blobs, - mut formats, - is_h264, - } = *self; + let Self { blobs, is_h264 } = *self; let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); @@ -98,17 +80,6 @@ impl MessageParser for CompressedImageMessageParser { components, )?; - let meta_chunk = Chunk::from_auto_row_ids( - ChunkId::new(), - entity_path.clone(), - timelines, - std::iter::once(( - ComponentDescriptor::partial("format").with_builtin_archetype(Self::ARCHETYPE_NAME), - formats.finish().into(), - )) - .collect(), - )?; - if is_h264 { // codec should be logged once per entity, as static data. let codec_chunk = Chunk::builder(entity_path.clone()) @@ -118,9 +89,9 @@ impl MessageParser for CompressedImageMessageParser { &VideoStream::update_fields().with_codec(VideoCodec::H264), ) .build()?; - Ok(vec![chunk, meta_chunk, codec_chunk]) + Ok(vec![chunk, codec_chunk]) } else { - Ok(vec![chunk, meta_chunk]) + Ok(vec![chunk]) } } } diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/image.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/image.rs index c8735716c9fa..3077dd347946 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/image.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/image.rs @@ -1,8 +1,6 @@ use anyhow::Context as _; -use arrow::array::{FixedSizeListArray, FixedSizeListBuilder, StringBuilder, UInt32Builder}; use re_chunk::{Chunk, ChunkId}; use re_types::{ - ComponentDescriptor, SerializedComponentColumn, archetypes::{DepthImage, Image}, datatypes::{ChannelDatatype, ColorModel, ImageFormat, PixelFormat}, }; @@ -12,7 +10,6 @@ use crate::parsers::{ cdr, decode::{MessageParser, ParserContext}, ros2msg::definitions::sensor_msgs, - util::fixed_size_list_builder, }; /// Plugin that parses `sensor_msgs/msg/CompressedImage` messages. @@ -25,36 +22,14 @@ pub struct ImageMessageParser { /// Note: These blobs are directly moved into a `Blob`, without copying. blobs: Vec>, image_formats: Vec, - height: FixedSizeListBuilder, - width: FixedSizeListBuilder, - encoding: FixedSizeListBuilder, - is_bigendian: FixedSizeListBuilder, - step: FixedSizeListBuilder, is_depth_image: bool, } -impl ImageMessageParser { - const ARCHETYPE_NAME: &str = "sensor_msgs.msg.Image"; - - fn create_metadata_column(name: &str, array: FixedSizeListArray) -> SerializedComponentColumn { - SerializedComponentColumn { - list_array: array.into(), - descriptor: ComponentDescriptor::partial(name) - .with_archetype(Self::ARCHETYPE_NAME.into()), - } - } -} - impl Ros2MessageParser for ImageMessageParser { fn new(num_rows: usize) -> Self { Self { blobs: Vec::with_capacity(num_rows), image_formats: Vec::with_capacity(num_rows), - height: fixed_size_list_builder(1, num_rows), - width: fixed_size_list_builder(1, num_rows), - encoding: fixed_size_list_builder(1, num_rows), - is_bigendian: fixed_size_list_builder(1, num_rows), - step: fixed_size_list_builder(1, num_rows), is_depth_image: false, } } @@ -69,8 +44,7 @@ impl MessageParser for ImageMessageParser { height, width, encoding, - is_bigendian, - step, + .. } = cdr::try_decode_message::>(&msg.data) .context("Failed to decode sensor_msgs::Image message from CDR data")?; @@ -90,23 +64,6 @@ impl MessageParser for ImageMessageParser { self.blobs.push(data.into_owned()); self.image_formats.push(img_format); - self.height.values().append_slice(&[height]); - self.height.append(true); - - self.width.values().append_slice(&[width]); - self.width.append(true); - - self.encoding.values().append_value(encoding); - self.encoding.append(true); - - self.is_bigendian - .values() - .append_slice(&[is_bigendian as u32]); - self.is_bigendian.append(true); - - self.step.values().append_slice(&[step]); - self.step.append(true); - Ok(()) } @@ -115,18 +72,13 @@ impl MessageParser for ImageMessageParser { let Self { blobs, image_formats, - mut height, - mut width, - mut encoding, - mut is_bigendian, - mut step, is_depth_image, } = *self; let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); - let mut chunk_components: Vec<_> = if is_depth_image { + let chunk_components: Vec<_> = if is_depth_image { DepthImage::update_fields() .with_many_buffer(blobs) .with_many_format(image_formats) @@ -140,14 +92,6 @@ impl MessageParser for ImageMessageParser { .collect() }; - chunk_components.extend([ - Self::create_metadata_column("height", height.finish()), - Self::create_metadata_column("width", width.finish()), - Self::create_metadata_column("encoding", encoding.finish()), - Self::create_metadata_column("is_bigendian", is_bigendian.finish()), - Self::create_metadata_column("step", step.finish()), - ]); - Ok(vec![Chunk::from_auto_row_ids( ChunkId::new(), entity_path.clone(), diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/imu.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/imu.rs index c80c1066f216..40626c80a35e 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/imu.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/imu.rs @@ -29,9 +29,6 @@ fn fixed_size_list_builder( pub struct ImuMessageParser { orientation: FixedSizeListBuilder, sensor_readings: FixedSizeListBuilder, - orientation_covariance: FixedSizeListBuilder, - angular_velocity_covariance: FixedSizeListBuilder, - linear_acceleration_covariance: FixedSizeListBuilder, } impl ImuMessageParser { @@ -61,9 +58,6 @@ impl Ros2MessageParser for ImuMessageParser { Self { orientation: fixed_size_list_builder(4, num_rows), sensor_readings: fixed_size_list_builder(6, num_rows), - orientation_covariance: fixed_size_list_builder(9, num_rows), - angular_velocity_covariance: fixed_size_list_builder(9, num_rows), - linear_acceleration_covariance: fixed_size_list_builder(9, num_rows), } } } @@ -94,21 +88,8 @@ impl MessageParser for ImuMessageParser { imu.linear_acceleration.z, ]); - self.orientation_covariance - .values() - .append_slice(&imu.orientation_covariance); - self.angular_velocity_covariance - .values() - .append_slice(&imu.angular_velocity_covariance); - self.linear_acceleration_covariance - .values() - .append_slice(&imu.linear_acceleration_covariance); - self.orientation.append(true); self.sensor_readings.append(true); - self.orientation_covariance.append(true); - self.angular_velocity_covariance.append(true); - self.linear_acceleration_covariance.append(true); Ok(()) } @@ -121,9 +102,6 @@ impl MessageParser for ImuMessageParser { let Self { mut orientation, mut sensor_readings, - mut orientation_covariance, - mut angular_velocity_covariance, - mut linear_acceleration_covariance, } = *self; let data_chunk = Chunk::from_auto_row_ids( @@ -142,21 +120,6 @@ impl MessageParser for ImuMessageParser { orientation.finish().into(), ), // TODO(#10728): Figure out what to do with the covariance matrices. - ( - ComponentDescriptor::partial("orientation_covariance") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - orientation_covariance.finish().into(), - ), - ( - ComponentDescriptor::partial("angular_velocity_covariance") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - angular_velocity_covariance.finish().into(), - ), - ( - ComponentDescriptor::partial("linear_acceleration_covariance") - .with_builtin_archetype(Self::ARCHETYPE_NAME), - linear_acceleration_covariance.finish().into(), - ), ] .into_iter() .collect(), diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/magnetic_field.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/magnetic_field.rs index 4ce021ef92e3..5aa17f09090e 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/magnetic_field.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/magnetic_field.rs @@ -1,17 +1,11 @@ use super::super::definitions::sensor_msgs; -use arrow::array::{FixedSizeListBuilder, Float64Builder}; use re_chunk::{Chunk, ChunkId}; -use re_types::{ - ComponentDescriptor, SerializedComponentColumn, archetypes::Arrows3D, datatypes::Vec3D, -}; +use re_types::{archetypes::Arrows3D, datatypes::Vec3D}; use crate::{ Error, - parsers::{ - MessageParser, ParserContext, cdr, ros2msg::Ros2MessageParser, - util::fixed_size_list_builder, - }, + parsers::{MessageParser, ParserContext, cdr, ros2msg::Ros2MessageParser}, }; /// Plugin that parses `sensor_msgs/msg/MagneticField` messages. @@ -20,18 +14,12 @@ pub struct MagneticFieldSchemaPlugin; pub struct MagneticFieldMessageParser { vectors: Vec, - magnetic_field_covariance: FixedSizeListBuilder, -} - -impl MagneticFieldMessageParser { - const ARCHETYPE_NAME: &str = "sensor_msgs.msg.MagneticField"; } impl Ros2MessageParser for MagneticFieldMessageParser { fn new(num_rows: usize) -> Self { Self { vectors: Vec::with_capacity(num_rows), - magnetic_field_covariance: fixed_size_list_builder(9, num_rows), } } } @@ -54,12 +42,6 @@ impl MessageParser for MagneticFieldMessageParser { magnetic_field.magnetic_field.z as f32, ])); - // Store covariance - self.magnetic_field_covariance - .values() - .append_slice(&magnetic_field.magnetic_field_covariance); - self.magnetic_field_covariance.append(true); - Ok(()) } @@ -67,27 +49,16 @@ impl MessageParser for MagneticFieldMessageParser { let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); - let Self { - vectors, - mut magnetic_field_covariance, - } = *self; - - let mut chunk_components: Vec<_> = Arrows3D::update_fields() - .with_vectors(vectors) - .columns_of_unit_batches()? - .collect(); - - chunk_components.push(SerializedComponentColumn { - descriptor: ComponentDescriptor::partial("magnetic_field_covariance") - .with_archetype(Self::ARCHETYPE_NAME.into()), - list_array: magnetic_field_covariance.finish().into(), - }); + let Self { vectors } = *self; let data_chunk = Chunk::from_auto_row_ids( ChunkId::new(), entity_path.clone(), timelines, - chunk_components.into_iter().collect(), + Arrows3D::update_fields() + .with_vectors(vectors) + .columns_of_unit_batches()? + .collect(), )?; Ok(vec![data_chunk]) diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/nav_sat_fix.rs b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/nav_sat_fix.rs index a1acecca1b4c..973b40e4d4ca 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/nav_sat_fix.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/sensor_msgs/nav_sat_fix.rs @@ -1,8 +1,5 @@ use anyhow::Context as _; -use arrow::array::{ - FixedSizeListArray, FixedSizeListBuilder, Float64Builder, Int8Builder, UInt8Builder, - UInt16Builder, -}; +use arrow::array::{FixedSizeListArray, FixedSizeListBuilder, Float64Builder}; use re_chunk::{Chunk, ChunkId}; use re_types::{ ComponentDescriptor, SerializedComponentColumn, archetypes::GeoPoints, components::LatLon, @@ -23,10 +20,6 @@ pub struct NavSatFixSchemaPlugin; pub struct NavSatFixMessageParser { geo_points: Vec, altitude: FixedSizeListBuilder, - status: FixedSizeListBuilder, - service: FixedSizeListBuilder, - position_covariance: FixedSizeListBuilder, - position_covariance_type: FixedSizeListBuilder, } impl NavSatFixMessageParser { @@ -46,10 +39,6 @@ impl Ros2MessageParser for NavSatFixMessageParser { Self { geo_points: Vec::with_capacity(num_rows), altitude: fixed_size_list_builder(1, num_rows), - status: fixed_size_list_builder(1, num_rows), - service: fixed_size_list_builder(1, num_rows), - position_covariance: fixed_size_list_builder(9, num_rows), - position_covariance_type: fixed_size_list_builder(1, num_rows), } } } @@ -59,12 +48,10 @@ impl MessageParser for NavSatFixMessageParser { re_tracing::profile_function!(); let sensor_msgs::NavSatFix { header, - status, latitude, longitude, altitude, - position_covariance, - position_covariance_type, + .. } = cdr::try_decode_message::(&msg.data) .context("Failed to decode sensor_msgs::NavSatFix message from CDR data")?; @@ -80,22 +67,6 @@ impl MessageParser for NavSatFixMessageParser { self.altitude.values().append_slice(&[altitude]); self.altitude.append(true); - self.status.values().append_slice(&[status.status as i8]); - self.status.append(true); - - self.service.values().append_slice(&[status.service as u16]); - self.service.append(true); - - self.position_covariance - .values() - .append_slice(&position_covariance); - self.position_covariance.append(true); - - self.position_covariance_type - .values() - .append_slice(&[position_covariance_type as u8]); - self.position_covariance_type.append(true); - Ok(()) } @@ -104,10 +75,6 @@ impl MessageParser for NavSatFixMessageParser { let Self { geo_points, mut altitude, - mut status, - mut service, - mut position_covariance, - mut position_covariance_type, } = *self; let entity_path = ctx.entity_path().clone(); @@ -118,16 +85,7 @@ impl MessageParser for NavSatFixMessageParser { .columns_of_unit_batches()? .collect(); - chunk_components.extend([ - Self::create_metadata_column("altitude", altitude.finish()), - Self::create_metadata_column("status", status.finish()), - Self::create_metadata_column("service", service.finish()), - Self::create_metadata_column("position_covariance", position_covariance.finish()), - Self::create_metadata_column( - "position_covariance_type", - position_covariance_type.finish(), - ), - ]); + chunk_components.extend([Self::create_metadata_column("altitude", altitude.finish())]); Ok(vec![Chunk::from_auto_row_ids( ChunkId::new(), diff --git a/crates/utils/re_ros_msg/Cargo.toml b/crates/utils/re_ros_msg/Cargo.toml new file mode 100644 index 000000000000..5655ed95ceda --- /dev/null +++ b/crates/utils/re_ros_msg/Cargo.toml @@ -0,0 +1,20 @@ +[package] +name = "re_ros_msg" +description = "Parse and decode ROS messages" +authors.workspace = true +edition.workspace = true +homepage.workspace = true +include.workspace = true +license.workspace = true +publish = true +repository.workspace = true +rust-version.workspace = true +version.workspace = true + +[lints] +workspace = true + +[dependencies] +anyhow.workspace = true +serde.workspace = true +thiserror.workspace = true diff --git a/crates/utils/re_ros_msg/src/deserialize/mod.rs b/crates/utils/re_ros_msg/src/deserialize/mod.rs new file mode 100644 index 000000000000..19ef44fdc508 --- /dev/null +++ b/crates/utils/re_ros_msg/src/deserialize/mod.rs @@ -0,0 +1,375 @@ +use std::collections::{BTreeMap, HashMap}; + +use serde::de::{self, DeserializeSeed}; + +use crate::{ + deserialize::primitive_array::PrimitiveArraySeed, + message_spec::{ComplexType, MessageSpecification, Type}, +}; + +pub mod primitive; +pub mod primitive_array; + +use primitive::{PrimitiveVisitor, StringVisitor}; + +/// A single deserialized value of any type that can appear in a ROS message. +#[derive(Clone, PartialEq)] +pub enum Value { + Bool(bool), + I8(i8), + U8(u8), + I16(i16), + U16(u16), + I32(i32), + U32(u32), + I64(i64), + U64(u64), + F32(f32), + F64(f64), + String(String), + + /// Fixed-size array of values. + Array(Vec), + + /// Variable-size or bounded array of values. + Sequence(Vec), + + /// Fixed-size array of primitive values. + PrimitiveArray(primitive_array::PrimitiveArray), + + /// Variable-size or bounded array of primitive values. + PrimitiveSeq(primitive_array::PrimitiveArray), + + /// Nested message. + Message(BTreeMap), +} + +impl std::fmt::Debug for Value { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + Self::Bool(v) => write!(f, "Bool({v})"), + Self::I8(v) => write!(f, "I8({v})"), + Self::U8(v) => write!(f, "U8({v})"), + Self::I16(v) => write!(f, "I16({v})"), + Self::U16(v) => write!(f, "U16({v})"), + Self::I32(v) => write!(f, "I32({v})"), + Self::U32(v) => write!(f, "U32({v})"), + Self::I64(v) => write!(f, "I64({v})"), + Self::U64(v) => write!(f, "U64({v})"), + Self::F32(v) => write!(f, "F32({v})"), + Self::F64(v) => write!(f, "F64({v})"), + Self::String(v) => write!(f, "String({v:?})"), + Self::Array(v) => write!(f, "Array({})", v.len()), + Self::Sequence(v) => write!(f, "Seq({})", v.len()), + Self::PrimitiveArray(v) | Self::PrimitiveSeq(v) => write!(f, "{v:?}"), + Self::Message(v) => { + write!(f, "Message({{")?; + for (i, (key, value)) in v.iter().enumerate() { + if i > 0 { + write!(f, ", ")?; + } + write!(f, "{key}: {value:?}")?; + } + write!(f, "}}") + } + } + } +} + +/// How we resolve a [`ComplexType`] at runtime. +pub trait TypeResolver { + fn resolve(&self, ty: &ComplexType) -> Option<&MessageSpecification>; +} + +/// Efficient type resolver with separate maps for absolute and relative lookups. +pub struct MapResolver<'a> { + /// Maps "pkg/Type" -> [`MessageSpecification`] + absolute: HashMap, + + /// Maps "Type" -> [`MessageSpecification`] + relative: HashMap, +} + +impl<'a> MapResolver<'a> { + pub fn new(specs: impl IntoIterator) -> Self { + let mut absolute = HashMap::new(); + let mut relative = HashMap::new(); + + for (full_name, spec) in specs { + if let Some((_, name)) = full_name.rsplit_once('/') { + // This is an absolute type like "pkg/Type" + absolute.insert(full_name.clone(), spec); + relative.insert(name.to_owned(), spec); + } else { + // This is already a relative type like "Type" + relative.insert(full_name, spec); + } + } + + Self { absolute, relative } + } +} + +impl TypeResolver for MapResolver<'_> { + fn resolve(&self, ty: &ComplexType) -> Option<&MessageSpecification> { + match ty { + ComplexType::Absolute { package, name } => { + let full_name = format!("{package}/{name}"); + self.absolute.get(&full_name).copied() + } + ComplexType::Relative { name } => self.relative.get(name).copied(), + } + } +} + +/// Whole message (struct) in field order. +pub struct MessageSeed<'a, R: TypeResolver> { + specification: &'a MessageSpecification, + type_resolver: &'a R, +} + +impl<'a, R: TypeResolver> MessageSeed<'a, R> { + pub fn new(spec: &'a MessageSpecification, type_resolver: &'a R) -> Self { + Self { + specification: spec, + type_resolver, + } + } +} + +impl<'de, R: TypeResolver> DeserializeSeed<'de> for MessageSeed<'_, R> { + type Value = Value; + + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + de.deserialize_tuple( + self.specification.fields.len(), + MessageVisitor { + spec: self.specification, + type_resolver: self.type_resolver, + }, + ) + } +} + +struct MessageVisitor<'a, R: TypeResolver> { + spec: &'a MessageSpecification, + type_resolver: &'a R, +} + +impl<'de, R: TypeResolver> serde::de::Visitor<'de> for MessageVisitor<'_, R> { + type Value = Value; + + fn expecting(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "cdr struct as fixed-length tuple") + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let mut out = std::collections::BTreeMap::new(); + for field in &self.spec.fields { + let v = seq + .next_element_seed(SchemaSeed::new(&field.ty, self.type_resolver))? + .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; + out.insert(field.name.clone(), v); + } + Ok(Value::Message(out)) + } +} + +/// One value, driven by a [`Type`] + resolver. +pub(super) struct SchemaSeed<'a, R: TypeResolver> { + ty: &'a Type, + resolver: &'a R, +} + +impl<'a, R: TypeResolver> SchemaSeed<'a, R> { + pub fn new(ty: &'a Type, resolver: &'a R) -> Self { + Self { ty, resolver } + } +} + +impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { + type Value = Value; + + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + use crate::message_spec::{ + ArraySize::{Bounded, Fixed, Unbounded}, + BuiltInType::{ + Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, String, UInt8, + UInt16, UInt32, UInt64, WString, + }, + Type, + }; + + match self.ty { + Type::BuiltIn(primitive_type) => match primitive_type { + Bool => de + .deserialize_bool(PrimitiveVisitor::::new()) + .map(Value::Bool), + Byte | UInt8 => de + .deserialize_u8(PrimitiveVisitor::::new()) + .map(Value::U8), // ROS2: octet + Char | Int8 => de + .deserialize_i8(PrimitiveVisitor::::new()) + .map(Value::I8), // ROS2: char (int8) + Float32 => de + .deserialize_f32(PrimitiveVisitor::::new()) + .map(Value::F32), + Float64 => de + .deserialize_f64(PrimitiveVisitor::::new()) + .map(Value::F64), + Int16 => de + .deserialize_i16(PrimitiveVisitor::::new()) + .map(Value::I16), + Int32 => de + .deserialize_i32(PrimitiveVisitor::::new()) + .map(Value::I32), + Int64 => de + .deserialize_i64(PrimitiveVisitor::::new()) + .map(Value::I64), + UInt16 => de + .deserialize_u16(PrimitiveVisitor::::new()) + .map(Value::U16), + UInt32 => de + .deserialize_u32(PrimitiveVisitor::::new()) + .map(Value::U32), + UInt64 => de + .deserialize_u64(PrimitiveVisitor::::new()) + .map(Value::U64), + String(_bound) | WString(_bound) => { + de.deserialize_string(StringVisitor).map(Value::String) + } + }, + Type::Array { ty, size } => match size { + Fixed(len) => { + // Check if this is a primitive array and use optimized path + if let Type::BuiltIn(prim_type) = ty.as_ref() { + PrimitiveArraySeed { + elem: prim_type, + fixed_len: Some(*len), + } + .deserialize(de) + .map(Value::PrimitiveArray) + } else { + SequenceSeed::new(ty, Some(*len), self.resolver) + .deserialize(de) + .map(Value::Array) + } + } + Bounded(_) | Unbounded => { + // Check if this is a primitive sequence and use optimized path + if let Type::BuiltIn(prim_type) = ty.as_ref() { + PrimitiveArraySeed { + elem: prim_type, + fixed_len: None, + } + .deserialize(de) + .map(Value::PrimitiveSeq) + } else { + // CDR: length-prefixed sequence; serde side is a seq. + SequenceSeed::new(ty, None, self.resolver) + .deserialize(de) + .map(Value::Sequence) + } + } + }, + Type::Complex(complex_ty) => { + let msg = self.resolver.resolve(complex_ty).ok_or_else(|| { + de::Error::custom(format!("unknown ComplexType: {complex_ty:?}")) + })?; + + MessageSeed::new(msg, self.resolver).deserialize(de) + } + } + } +} + +// Sequence/array of elements. +pub(super) struct SequenceSeed<'a, R: TypeResolver> { + elem: &'a Type, + fixed_len: Option, + resolver: &'a R, +} + +impl<'a, R: TypeResolver> SequenceSeed<'a, R> { + pub fn new(elem: &'a Type, fixed_len: Option, resolver: &'a R) -> Self { + Self { + elem, + fixed_len, + resolver, + } + } +} + +impl<'de, R: TypeResolver> DeserializeSeed<'de> for SequenceSeed<'_, R> { + type Value = Vec; + + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + match self.fixed_len { + Some(len) => de.deserialize_tuple( + len, + SequenceVisitor { + elem: self.elem, + fixed_len: Some(len), + type_resolver: self.resolver, + }, + ), + None => de.deserialize_seq(SequenceVisitor { + elem: self.elem, + fixed_len: None, + type_resolver: self.resolver, + }), + } + } +} + +struct SequenceVisitor<'a, R: TypeResolver> { + elem: &'a Type, + fixed_len: Option, + type_resolver: &'a R, +} + +impl<'de, R: TypeResolver> serde::de::Visitor<'de> for SequenceVisitor<'_, R> { + type Value = Vec; + + fn expecting(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "cdr-encoded sequence/array") + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let len = self.fixed_len.or_else(|| seq.size_hint()); + let mut out = Vec::with_capacity(len.unwrap_or(0)); + + if let Some(len) = len { + for _ in 0..len { + let v = seq + .next_element_seed(SchemaSeed::new(self.elem, self.type_resolver))? + .ok_or_else(|| serde::de::Error::custom("short sequence"))?; + out.push(v); + } + Ok(out) + } else { + // Fallback for truly unbounded streams + while let Some(v) = + seq.next_element_seed(SchemaSeed::new(self.elem, self.type_resolver))? + { + out.push(v); + } + Ok(out) + } + } +} diff --git a/crates/utils/re_ros_msg/src/deserialize/primitive.rs b/crates/utils/re_ros_msg/src/deserialize/primitive.rs new file mode 100644 index 000000000000..29bf9a8be034 --- /dev/null +++ b/crates/utils/re_ros_msg/src/deserialize/primitive.rs @@ -0,0 +1,78 @@ +use std::fmt; + +use serde::de::{self, Visitor}; + +pub(super) struct PrimitiveVisitor(std::marker::PhantomData); + +impl PrimitiveVisitor { + pub fn new() -> Self { + Self(std::marker::PhantomData) + } +} + +macro_rules! impl_primitive_visitor { + ($t:ty, $m:ident) => { + impl Visitor<'_> for PrimitiveVisitor<$t> { + type Value = $t; + + fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, stringify!($t)) + } + + fn $m(self, v: $t) -> Result<$t, E> { + Ok(v) + } + } + }; +} + +impl_primitive_visitor!(i8, visit_i8); +impl_primitive_visitor!(u8, visit_u8); +impl_primitive_visitor!(i16, visit_i16); +impl_primitive_visitor!(u16, visit_u16); +impl_primitive_visitor!(i32, visit_i32); +impl_primitive_visitor!(u32, visit_u32); +impl_primitive_visitor!(i64, visit_i64); +impl_primitive_visitor!(u64, visit_u64); +impl_primitive_visitor!(f32, visit_f32); +impl_primitive_visitor!(f64, visit_f64); + +impl Visitor<'_> for PrimitiveVisitor { + type Value = bool; + + fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "bool") + } + + fn visit_bool(self, v: bool) -> Result { + Ok(v) + } + + fn visit_u8(self, v: u8) -> Result + where + E: de::Error, + { + Ok(v != 0) + } +} + +pub(super) struct StringVisitor; + +impl Visitor<'_> for StringVisitor { + type Value = String; + + fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "string") + } + + fn visit_string(self, v: String) -> Result { + Ok(v) + } + + fn visit_str(self, v: &str) -> Result + where + E: de::Error, + { + Ok(v.to_owned()) + } +} diff --git a/crates/utils/re_ros_msg/src/deserialize/primitive_array.rs b/crates/utils/re_ros_msg/src/deserialize/primitive_array.rs new file mode 100644 index 000000000000..5ece14e4a045 --- /dev/null +++ b/crates/utils/re_ros_msg/src/deserialize/primitive_array.rs @@ -0,0 +1,124 @@ +use crate::message_spec::BuiltInType; +use serde::de::{self, DeserializeSeed, Visitor}; +use std::fmt; + +#[derive(Clone, PartialEq)] +pub enum PrimitiveArray { + Bool(Vec), + I8(Vec), + U8(Vec), + I16(Vec), + U16(Vec), + I32(Vec), + U32(Vec), + I64(Vec), + U64(Vec), + F32(Vec), + F64(Vec), + String(Vec), +} + +impl std::fmt::Debug for PrimitiveArray { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::Bool(v) => write!(f, "BoolArray({})", v.len()), + Self::I8(v) => write!(f, "I8Array({})", v.len()), + Self::U8(v) => write!(f, "U8Array({})", v.len()), + Self::I16(v) => write!(f, "I16Array({})", v.len()), + Self::U16(v) => write!(f, "U16Array({})", v.len()), + Self::I32(v) => write!(f, "I32Array({})", v.len()), + Self::U32(v) => write!(f, "U32Array({})", v.len()), + Self::I64(v) => write!(f, "I64Array({})", v.len()), + Self::U64(v) => write!(f, "U64Array({})", v.len()), + Self::F32(v) => write!(f, "F32Array({})", v.len()), + Self::F64(v) => write!(f, "F64Array({})", v.len()), + Self::String(v) => write!(f, "StringArray({})", v.len()), + } + } +} + +/// Specialized seed for primitive arrays (arrays/sequences of built-in types). +pub struct PrimitiveArraySeed<'a> { + pub elem: &'a BuiltInType, + pub fixed_len: Option, +} + +macro_rules! impl_primitive_array_visitor { + ($prim_type:ty, $array_variant:ident, $visit_method:ident) => { + struct $array_variant; + + impl<'de> Visitor<'de> for $array_variant { + type Value = Vec<$prim_type>; + + fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "array of {}", stringify!($prim_type)) + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: de::SeqAccess<'de>, + { + let size_hint = seq.size_hint().unwrap_or(0); + let mut vec = Vec::with_capacity(size_hint); + + while let Some(elem) = seq.next_element()? { + vec.push(elem); + } + + Ok(vec) + } + } + }; +} + +impl_primitive_array_visitor!(bool, BoolArrayVisitor, visit_bool); +impl_primitive_array_visitor!(i8, I8ArrayVisitor, visit_i8); +impl_primitive_array_visitor!(u8, U8ArrayVisitor, visit_u8); +impl_primitive_array_visitor!(i16, I16ArrayVisitor, visit_i16); +impl_primitive_array_visitor!(u16, U16ArrayVisitor, visit_u16); +impl_primitive_array_visitor!(i32, I32ArrayVisitor, visit_i32); +impl_primitive_array_visitor!(u32, U32ArrayVisitor, visit_u32); +impl_primitive_array_visitor!(i64, I64ArrayVisitor, visit_i64); +impl_primitive_array_visitor!(u64, U64ArrayVisitor, visit_u64); +impl_primitive_array_visitor!(f32, F32ArrayVisitor, visit_f32); +impl_primitive_array_visitor!(f64, F64ArrayVisitor, visit_f64); +impl_primitive_array_visitor!(String, StringArrayVisitor, visit_string); + +impl<'de> DeserializeSeed<'de> for PrimitiveArraySeed<'_> { + type Value = PrimitiveArray; + + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + use BuiltInType::{ + Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, String, UInt8, UInt16, + UInt32, UInt64, WString, + }; + + macro_rules! deserialize_array { + ($de:expr, $visitor:expr) => { + match self.fixed_len { + Some(n) => $de.deserialize_tuple(n, $visitor), + None => $de.deserialize_seq($visitor), + } + }; + } + + match self.elem { + Bool => deserialize_array!(de, BoolArrayVisitor).map(PrimitiveArray::Bool), + Byte | UInt8 => deserialize_array!(de, U8ArrayVisitor).map(PrimitiveArray::U8), + Char | Int8 => deserialize_array!(de, I8ArrayVisitor).map(PrimitiveArray::I8), + Float32 => deserialize_array!(de, F32ArrayVisitor).map(PrimitiveArray::F32), + Float64 => deserialize_array!(de, F64ArrayVisitor).map(PrimitiveArray::F64), + Int16 => deserialize_array!(de, I16ArrayVisitor).map(PrimitiveArray::I16), + Int32 => deserialize_array!(de, I32ArrayVisitor).map(PrimitiveArray::I32), + Int64 => deserialize_array!(de, I64ArrayVisitor).map(PrimitiveArray::I64), + UInt16 => deserialize_array!(de, U16ArrayVisitor).map(PrimitiveArray::U16), + UInt32 => deserialize_array!(de, U32ArrayVisitor).map(PrimitiveArray::U32), + UInt64 => deserialize_array!(de, U64ArrayVisitor).map(PrimitiveArray::U64), + String(_) => deserialize_array!(de, StringArrayVisitor).map(PrimitiveArray::String), + WString(_) => Err(de::Error::custom("wstring arrays not supported")), + } + } +} diff --git a/crates/utils/re_ros_msg/src/lib.rs b/crates/utils/re_ros_msg/src/lib.rs new file mode 100644 index 000000000000..1508c7beb952 --- /dev/null +++ b/crates/utils/re_ros_msg/src/lib.rs @@ -0,0 +1,109 @@ +//! Minimal ROS 2 `.msg` reflection parser (messages only). +//! +//! This module parses the textual ROS 2 message definition format (aka `.msg`) +//! into a typed, reflection-friendly representation. It is intentionally kept +//! generic and does not rely on any pre-baked message definitions, so it can be +//! used to parse unknown types and still extract semantic meaning (types, +//! arrays, names, constants, default values). +use anyhow::Context as _; + +use crate::message_spec::MessageSpecification; + +pub mod deserialize; +pub mod message_spec; + +/// Parse a schema name from a line starting with "MSG: ". +fn parse_schema_name(line: &str) -> Option<&str> { + line.trim().strip_prefix("MSG: ").map(str::trim) +} + +#[derive(Debug, Clone, PartialEq)] +pub struct MessageSchema { + /// Specification of the main message type. + pub spec: MessageSpecification, + + /// Dependent message types referenced by the main type. + pub dependencies: Vec, // Other message types referenced by this one. +} + +impl MessageSchema { + pub fn parse(name: &str, input: &str) -> anyhow::Result { + let main_spec_content = extract_main_msg_spec(input); + let specs = extract_msg_specs(input); + + let main_spec = MessageSpecification::parse(name, &main_spec_content) + .with_context(|| format!("failed to parse main message spec `{name}`"))?; + + let mut dependencies = Vec::new(); + for (dep_name, dep_content) in specs { + let dep_spec = MessageSpecification::parse(&dep_name, &dep_content) + .with_context(|| format!("failed to parse dependent message spec `{dep_name}`"))?; + dependencies.push(dep_spec); + } + + Ok(Self { + spec: main_spec, + dependencies, + }) + } +} + +/// Check if a line is a schema separator (a line of at least 3 '=' characters). +pub fn is_schema_separator(line: &str) -> bool { + let line = line.trim(); + line.len() >= 3 && line.chars().all(|c| c == '=') +} + +/// Extract the main message specification from input, stopping at the first schema separator. +/// +/// The main spec is everything before the first "====" separator line. +fn extract_main_msg_spec(input: &str) -> String { + input + .lines() + .take_while(|line| !is_schema_separator(line)) + .filter(|line| !line.is_empty()) + .collect::>() + .join("\n") +} + +/// Find "MSG: ``" and take the rest as content +/// Extract all message specifications from input that are separated by schema separators. +/// +/// Returns a vector of `(message_name, message_body)` pairs for each schema found. +fn extract_msg_specs(input: &str) -> Vec<(String, String)> { + let mut specs = Vec::new(); + let mut current_section = Vec::new(); + + for line in input.lines() { + if is_schema_separator(line) { + if let Some(spec) = parse_section(¤t_section) { + specs.push(spec); + } + current_section.clear(); + } else { + current_section.push(line); + } + } + + // Handle the final section if it doesn't end with a separator + if let Some(spec) = parse_section(¤t_section) { + specs.push(spec); + } + + specs +} + +/// Parse a section of lines into a (name, body) pair. +/// +/// The first line should contain "MSG: ``" and subsequent lines form the message body. +fn parse_section(lines: &[&str]) -> Option<(String, String)> { + if lines.len() < 2 { + return None; + } + + let first_line = lines[0].trim(); + let name = parse_schema_name(first_line)?; + let body = lines[1..].join("\n"); + + Some((name.to_owned(), body)) +} diff --git a/crates/utils/re_ros_msg/src/message_spec.rs b/crates/utils/re_ros_msg/src/message_spec.rs new file mode 100644 index 000000000000..ba4b23bd42b4 --- /dev/null +++ b/crates/utils/re_ros_msg/src/message_spec.rs @@ -0,0 +1,891 @@ +use thiserror::Error; + +#[derive(Error, Debug)] +pub enum ParseError { + /// Bad text/shape: missing tokens, bad delimiters, malformed constructs + #[error("syntax error: {0}")] + Syntax(String), + + /// Type names/annotations: unknown/invalid types, bad bounds, array forms + #[error("type error: {0}")] + Type(String), + + /// Literal/default/constant value parsing and type mismatches + #[error("value error: {0}")] + Value(String), + + /// Cross-entry checks: duplicates, bounds exceeded, defaults not allowed, naming rules + #[error("validation error: {0}")] + Validate(String), +} + +/// A parsed ROS 2 message specification, including fields and constants. +/// +/// ```text +/// # Example message spec: +/// int32 id +/// string name +/// float64[3] position # bounded array of 3 float64 values +/// geometry_msgs/Point[] points # unbounded array of complex types +/// +/// uint32 CONST_VALUE=42 +/// string CONST_STR="hello" +/// ``` +#[derive(Debug, Clone, PartialEq)] +pub struct MessageSpecification { + /// Name of the message type. + pub name: String, + + /// Fields that make up the message payload. + pub fields: Vec, + + /// Compile-time constants defined alongside fields. + pub constants: Vec, +} + +impl MessageSpecification { + pub(super) fn parse(name: &str, input: &str) -> Result { + let mut fields = Vec::new(); + let mut constants = Vec::new(); + + for line in input.lines() { + let line = strip_comment(line).trim(); + if line.is_empty() { + continue; + } + + if super::is_schema_separator(line) { + continue; + } + + // Parse type and name (common to both constants and fields) + let (ty_start, ty_end) = next_token_bounds(line, 0) + .ok_or_else(|| ParseError::Syntax(format!("missing type in line: `{line}`")))?; + let (name_start, name_end) = next_token_bounds(line, ty_end) + .ok_or_else(|| ParseError::Syntax(format!("missing name in line: `{line}`")))?; + + let ty_str = &line[ty_start..ty_end]; + let name = &line[name_start..name_end]; + let ty = Type::parse(ty_str)?; + + // Check if rest starts with '=' to differentiate constant from field + let rest = line[name_end..].trim(); + + if rest.starts_with('=') { + let constant = Constant::parse(ty, name, rest)?; + constants.push(constant); + } else { + let field = Field::parse(ty, name, rest)?; + fields.push(field); + } + } + + Ok(Self { + name: name.to_owned(), + fields, + constants, + }) + } +} + +/// A compile-time constant defined alongside fields. +/// Includes type, name, and value. +/// +/// Examples: +/// ```text +/// // Integer constant +/// int32 CONST_NAME=42 +/// +/// // String constant +/// string CONST_STR="hello" +/// +/// // Float constant +/// float64 CONST_FLOAT=3.14 +/// ``` +#[derive(Debug, Clone, PartialEq)] +pub struct Constant { + pub ty: Type, + pub name: String, + pub value: Literal, +} + +impl Constant { + fn parse(ty: Type, name: &str, rest: &str) -> Result { + if !Self::is_valid_constant_name(name) { + return Err(ParseError::Validate(format!( + "constant name must be all-caps alphanumeric and underscores only, got `{name}`" + ))); + } + + if matches!(ty, Type::Array { .. }) { + return Err(ParseError::Type( + "constant type cannot be an array".to_owned(), + )); + } + + if matches!(ty, Type::Complex(_)) { + return Err(ParseError::Type( + "constant type must be a built-in type".to_owned(), + )); + } + + // rest should start with '=' + let value_str = rest + .strip_prefix('=') + .ok_or_else(|| ParseError::Syntax("constant definition missing '='".to_owned()))? + .trim(); + + let value = Literal::parse(value_str, &ty)?; + + Ok(Self { + ty, + name: name.to_owned(), + value, + }) + } + + /// Constant names must be uppercase alphanumeric characters with underscores for separating words. + /// They must start with an alphabetic character, they must not end with an underscore and never have two consecutive underscores. + fn is_valid_constant_name(name: &str) -> bool { + if name.is_empty() { + return false; + } + + // Must start with uppercase letter and not end with underscore + let mut chars = name.chars(); + if !chars.next().is_some_and(|c| c.is_ascii_uppercase()) || name.ends_with('_') { + return false; + } + + // Check for valid characters and no consecutive underscores + !name.contains("__") + && name + .chars() + .all(|c| c.is_ascii_uppercase() || c.is_ascii_digit() || c == '_') + } +} + +/// A message field definition. +/// Includes type, name, and optional default value. +/// +/// Examples: +/// ```text +/// // Simple int32 field with no default value +/// int32 field_name +/// +/// // Bounded string with max length 10, default value "default" +/// string<=10 name "default" +/// +/// // Array of 3 float64s with default value [0.0, 0.0, 0.0] +/// float64[3] position [0.0, 0.0, 0.0] +/// +/// // Unbounded array of complex types +/// pkg/Type[] items +/// ``` +#[derive(Debug, Clone, PartialEq)] +pub struct Field { + pub ty: Type, + pub name: String, + pub default: Option, +} + +impl Field { + fn parse(ty: Type, name: &str, rest: &str) -> Result { + let default = if rest.is_empty() { + None + } else { + Some(Literal::parse(rest, &ty)?) + }; + + Ok(Self { + ty, + name: name.to_owned(), + default, + }) + } +} + +/// A built-in field type, e.g. `int32`, `float64`, `bool`, etc. +/// +/// Types are taken from: +#[derive(Debug, Clone, PartialEq, Eq)] +pub enum BuiltInType { + Bool, + Byte, + Char, + Float32, + Float64, + Int8, + Int16, + Int32, + Int64, + UInt8, + UInt16, + UInt32, + UInt64, + String(Option), // Optional max length for bounded strings. + WString(Option), // Optional max length for bounded wide strings. +} + +#[derive(Debug, Clone, PartialEq)] +pub enum Type { + BuiltIn(BuiltInType), + Complex(ComplexType), // Possibly qualified with package path, e.g. `pkg/Type + Array { ty: Box, size: ArraySize }, +} + +impl Type { + /// Parse a type definition, e.g. `int32`, `string<=10`, `float32[3]`, `pkg/Type[]`, `pkg/Type[<=5]`, etc. + fn parse(s: &str) -> Result { + let s = s.trim(); + + if let Some((base, array_part)) = s.split_once('[') { + let base = Self::parse(base)?; + let array_size = ArraySize::parse(array_part)?; + + Ok(Self::Array { + ty: Box::new(base), + size: array_size, + }) + } else { + match s { + "bool" => Ok(Self::BuiltIn(BuiltInType::Bool)), + "byte" => Ok(Self::BuiltIn(BuiltInType::Byte)), + "char" => Ok(Self::BuiltIn(BuiltInType::Char)), + "float32" => Ok(Self::BuiltIn(BuiltInType::Float32)), + "float64" => Ok(Self::BuiltIn(BuiltInType::Float64)), + "int8" => Ok(Self::BuiltIn(BuiltInType::Int8)), + "int16" => Ok(Self::BuiltIn(BuiltInType::Int16)), + "int32" => Ok(Self::BuiltIn(BuiltInType::Int32)), + "int64" => Ok(Self::BuiltIn(BuiltInType::Int64)), + "uint8" => Ok(Self::BuiltIn(BuiltInType::UInt8)), + "uint16" => Ok(Self::BuiltIn(BuiltInType::UInt16)), + "uint32" => Ok(Self::BuiltIn(BuiltInType::UInt32)), + "uint64" => Ok(Self::BuiltIn(BuiltInType::UInt64)), + "string" => Ok(Self::BuiltIn(BuiltInType::String(None))), + "wstring" => Ok(Self::BuiltIn(BuiltInType::WString(None))), + s if s.starts_with("string<=") => { + // e.g. `string<=10` + Ok(Self::BuiltIn(BuiltInType::String(Some( + Self::parse_bounded_string_length(s, "string<=")?, + )))) + } + s if s.starts_with("wstring<=") => { + // e.g. `wstring<=10` + Ok(Self::BuiltIn(BuiltInType::WString(Some( + Self::parse_bounded_string_length(s, "wstring<=")?, + )))) + } + s => ComplexType::parse(s).map(Type::Complex), + } + } + } + + fn parse_bounded_string_length(s: &str, prefix: &str) -> Result { + if let Some(len_str) = s.strip_prefix(prefix) { + let len = len_str.parse::().map_err(|err| { + ParseError::Type(format!("failed to parse bounded string length: {err}")) + })?; + + Ok(len) + } else { + Err(ParseError::Type(format!( + "invalid string type specifier: `{s}`" + ))) + } + } +} + +/// A complex (non-primitive) type, possibly qualified with a package path. +/// +/// Examples: +/// ```text +/// // Absolute type with package +/// pkg/Type +/// +/// // Relative type without package +/// RelativeType +/// ``` +#[derive(Debug, Clone, PartialEq, Eq, Hash)] +pub enum ComplexType { + /// An absolute type with package, e.g. `pkg/Type` + Absolute { package: String, name: String }, + + /// A relative type without package, e.g. `Type` + Relative { name: String }, +} + +impl ComplexType { + fn parse(s: &str) -> Result { + if let Some((package, name)) = s.rsplit_once('/') { + if package.is_empty() || name.is_empty() { + Err(ParseError::Type(format!( + "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` format" + ))) + } else { + Ok(Self::Absolute { + package: package.to_owned(), + name: name.to_owned(), + }) + } + } else if s.is_empty() { + Err(ParseError::Type(format!( + "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` or `SomeMessage` format" + ))) + } else { + Ok(Self::Relative { name: s.to_owned() }) + } + } +} + +/// Size specifier for array types. +#[derive(Debug, Clone, PartialEq, Eq)] +pub enum ArraySize { + /// A fixed-size array, e.g. `[3]` + Fixed(usize), + + /// A bounded-size array, e.g. `[<=5]` + Bounded(usize), + + /// An unbounded array, e.g. `[]` + Unbounded, +} + +impl ArraySize { + fn parse(array_part: &str) -> Result { + let array_part = array_part + .strip_suffix(']') + .ok_or_else(|| ParseError::Syntax("Missing closing ']' in array type".to_owned()))?; + + let array_size = if array_part.is_empty() { + Self::Unbounded + } else if let Ok(size) = array_part.parse::() { + Self::Fixed(size) + } else if let Some(n) = array_part.strip_prefix("<=") { + let size = n.parse::().map_err(|err| { + ParseError::Value(format!("Failed to parse bounded array size: {err}")) + })?; + Self::Bounded(size) + } else { + return Err(ParseError::Value(format!( + "invalid array size specifier: `{array_part}`" + ))); + }; + + Ok(array_size) + } +} + +/// A literal value, used for default values and constant definitions. Literals can only +/// be of [`BuiltInType`]s or arrays thereof. +#[derive(Debug, Clone, PartialEq)] +pub enum Literal { + Bool(bool), + Int(i64), + UInt(u64), + Float(f64), + String(String), + Array(Vec), +} + +impl Literal { + fn parse(s: &str, ty: &Type) -> Result { + use BuiltInType::{ + Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, String, UInt8, UInt16, + UInt32, UInt64, WString, + }; + + match ty { + Type::BuiltIn(p) => match p { + Bool => match s { + "true" => Ok(Self::Bool(true)), + "false" => Ok(Self::Bool(false)), + _ => Err(ParseError::Value(format!( + "failed to parse bool literal: `{s}`" + ))), + }, + // Char is a signed 8-bit integer representing an ASCII character. + Char | Int8 | Int16 | Int32 | Int64 => { + s.parse::().map(Self::Int).map_err(|err| { + ParseError::Value(format!("failed to parse integer literal `{s}`: {err}")) + }) + } + // Byte is an unsigned 8-bit integer. + Byte | UInt8 | UInt16 | UInt32 | UInt64 => { + s.parse::().map(Self::UInt).map_err(|err| { + ParseError::Value(format!( + "failed to parse unsigned integer literal `{s}`: {err}" + )) + }) + } + Float32 | Float64 => s.parse::().map(Self::Float).map_err(|err| { + ParseError::Value(format!("failed to parse float literal `{s}`: {err}")) + }), + String(_) | WString(_) => { + let s = if (s.starts_with('"') && s.ends_with('"')) + || (s.starts_with('\'') && s.ends_with('\'')) + { + // Remove quotes from quoted strings (both " and ') + &s[1..s.len() - 1] + } else { + // Use unquoted strings as-is + s + }; + Ok(Self::String(s.to_owned())) + } + }, + Type::Array { + ty: elem_ty, + size: _, + } => { + let s = s.trim(); + + if !s.starts_with('[') || !s.ends_with(']') { + Err(ParseError::Value(format!( + "array literal must start with '[' and end with ']': `{s}`" + ))) + } else { + let inner = &s[1..s.len() - 1]; + let elems = inner + .split(',') + .map(|e| e.trim()) + .filter(|e| !e.is_empty()) + .map(|elem_str| Self::parse(elem_str, elem_ty)) + .collect::, ParseError>>()?; + + Ok(Self::Array(elems)) + } + } + Type::Complex(_) => Err(ParseError::Value( + "literals of complex types are not supported".to_owned(), + )), + } + } +} + +/// Parse the bounds of the next whitespace-delimited token in a string. +/// Returns (`start_index`, `end_index`) of the token, or `None` if no token found. +/// Treats '=' as a separate single-character token, except when part of '<=' type bounds. +fn next_token_bounds(s: &str, start: usize) -> Option<(usize, usize)> { + let remaining = s.get(start..)?; + let token_start = start + remaining.len() - remaining.trim_start().len(); + let token = remaining.trim_start(); + + if token.is_empty() { + return None; + } + + // If the token starts with '=', return it as a single-character token + if token.starts_with('=') { + return Some((token_start, token_start + 1)); + } + + // Find the end of the token + // Stop at whitespace or standalone '=' (not part of '<=' type bound) + let chars: Vec = token.chars().collect(); + let mut token_len_bytes = 0; + + for (i, &c) in chars.iter().enumerate() { + if c.is_whitespace() { + break; + } + + // Check if this is '=' and it's not part of '<=' + if c == '=' { + // Check if previous char was '<' + let prev_is_lt = i > 0 && chars[i - 1] == '<'; + if !prev_is_lt { + // This '=' is standalone, stop here + break; + } + } + + token_len_bytes += c.len_utf8(); + } + + if token_len_bytes == 0 { + None + } else { + Some((token_start, token_start + token_len_bytes)) + } +} + +/// Strip comments from a line (anything after a '#'). +fn strip_comment(s: &str) -> &str { + let mut in_quote = false; + let mut quote_char: Option = None; + let mut escaped = false; + for (i, c) in s.char_indices() { + if escaped { + escaped = false; + continue; + } + match c { + '\\' => { + // escape next character only inside quotes; outside it doesn't matter for '#' + if in_quote { + escaped = true; + } + } + '"' | '\'' => { + if !in_quote { + in_quote = true; + quote_char = Some(c); + } else if quote_char == Some(c) { + in_quote = false; + quote_char = None; + } + } + '#' if !in_quote => { + return &s[..i]; + } + _ => {} + } + } + s +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn valid_constant() { + assert_eq!( + Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONST_NAME", "=42").unwrap(), + Constant { + ty: Type::BuiltIn(BuiltInType::Int32), + name: "CONST_NAME".to_owned(), + value: Literal::Int(42) + } + ); + + assert_eq!( + Constant::parse( + Type::BuiltIn(BuiltInType::String(None)), + "CONST_STR", + "=\"hello\"" + ) + .unwrap(), + Constant { + ty: Type::BuiltIn(BuiltInType::String(None)), + name: "CONST_STR".to_owned(), + value: Literal::String("hello".to_owned()) + } + ); + + assert_eq!( + Constant::parse(Type::BuiltIn(BuiltInType::Float64), "CONST_FLOAT", "=3.1").unwrap(), + Constant { + ty: Type::BuiltIn(BuiltInType::Float64), + name: "CONST_FLOAT".to_owned(), + value: Literal::Float(3.1) + } + ); + } + + #[test] + fn invalid_constant() { + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONST_NAME", "").is_err()); // missing '=' and value + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONST_NAME", "=abc").is_err()); // invalid int value + assert!( + Constant::parse( + Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::Int32)), + size: ArraySize::Unbounded + }, + "CONST_NAME", + "=42" + ) + .is_err() + ); // array type not allowed + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "const_name", "=42").is_err()); // invalid name (not all-caps) + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONST__NAME", "=42").is_err()); // invalid name (consecutive underscores + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "_CONSTNAME", "=42").is_err()); // invalid name (doesn't start with letter) + assert!(Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONSTNAME_", "=42").is_err()); // invalid name (ends with underscore) + } + + #[test] + fn valid_field() { + assert_eq!( + Field::parse(Type::BuiltIn(BuiltInType::Int32), "field_name", "").unwrap(), + Field { + ty: Type::BuiltIn(BuiltInType::Int32), + name: "field_name".to_owned(), + default: None + } + ); + + assert_eq!( + Field::parse( + Type::BuiltIn(BuiltInType::String(Some(10))), + "name", + "\"default\"" + ) + .unwrap(), + Field { + name: "name".to_owned(), + ty: Type::BuiltIn(BuiltInType::String(Some(10))), + default: Some(Literal::String("default".to_owned())) + } + ); + + assert_eq!( + Field::parse( + Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::Float64)), + size: ArraySize::Fixed(3) + }, + "position", + "[0.0, 1.0, 2.0]" + ) + .unwrap(), + Field { + name: "position".to_owned(), + ty: Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::Float64)), + size: ArraySize::Fixed(3) + }, + default: Some(Literal::Array(vec![ + Literal::Float(0.0), + Literal::Float(1.0), + Literal::Float(2.0) + ])) + } + ); + + assert_eq!( + Field::parse( + Type::Array { + ty: Box::new(Type::Complex(ComplexType::Absolute { + package: "geometry_msgs".to_owned(), + name: "Point".to_owned() + })), + size: ArraySize::Unbounded + }, + "points", + "" + ) + .unwrap(), + Field { + name: "points".to_owned(), + ty: Type::Array { + ty: Box::new(Type::Complex(ComplexType::Absolute { + package: "geometry_msgs".to_owned(), + name: "Point".to_owned() + })), + size: ArraySize::Unbounded + }, + default: None + } + ); + + assert_eq!( + Field::parse(Type::BuiltIn(BuiltInType::Bool), "enabled", "true").unwrap(), + Field { + ty: Type::BuiltIn(BuiltInType::Bool), + name: "enabled".to_owned(), + default: Some(Literal::Bool(true)) + } + ); + } + + #[test] + fn invalid_field() { + assert!(Field::parse(Type::BuiltIn(BuiltInType::Bool), "enabled", "maybe").is_err()); // invalid bool literal + } + + #[test] + fn strip_comment_works() { + assert_eq!(strip_comment("int32 field # comment"), "int32 field "); + assert_eq!( + strip_comment("string name \"value # not a comment\" # comment"), + "string name \"value # not a comment\" " + ); + assert_eq!( + strip_comment("string name 'value # not a comment' # comment"), + "string name 'value # not a comment' " + ); + assert_eq!( + strip_comment("string name \"value \\\" # still not a comment\" # comment"), + "string name \"value \\\" # still not a comment\" " + ); + assert_eq!( + strip_comment("string name 'value \\' # still not a comment' # comment"), + "string name 'value \\' # still not a comment' " + ); + assert_eq!(strip_comment("int32 field"), "int32 field"); + assert_eq!(strip_comment("# full line comment"), ""); + assert_eq!(strip_comment(""), ""); + } + + #[test] + fn valid_message_spec() { + let input = r#" +# "Enum"-like constants for mode/state +uint8 MODE_IDLE=0 +uint8 MODE_INIT=1 +uint8 MODE_RUN=2 +uint8 MODE_PAUSE=3 +uint8 MODE_ERROR=255 + +# Bitmask-style flags +uint32 FLAG_NONE=0 +uint32 FLAG_VERBOSE=1 +uint32 FLAG_RECORD=2 +uint32 FLAG_DEBUG=4 +uint32 FLAG_ALL=7 # FLAG_VERBOSE|FLAG_RECORD|FLAG_DEBUG + +# Misc constants of various types +int32 MAX_RETRIES=5 +float32 DEFAULT_SCALE=1.5 +string DEFAULT_LABEL="calib_v1" + +# Header & basic identification +std_msgs/Header header # standard ROS header +uint8 mode 1 # default: MODE_INIT +uint32 flags 0 # default bitmask + +# Scalar defaults & bounded string default +bool enabled true # default: true +float32 scale 1.5 # mirrors DEFAULT_SCALE +string<=32 label "default_label" # bounded string with default + +# Free-form description (unbounded string, optional empty) +string description + +# Times & durations +builtin_interfaces/Time last_update +builtin_interfaces/Duration timeout + +# Arrays & sequences +# Fixed-size numeric arrays +float32[9] K # 3x3 intrinsics (row-major) +float32[9] R # 3x3 rectification matrix +float32[12] P # 3x4 projection matrix + +# Unbounded arrays +int32[] indices +float64[] residuals + +# Upper-bounded sequence +uint8[<=16] small_buffer # capacity-limited byte buffer + +# Nested types & arrays of nested types +geometry_msgs/Pose[] trajectory # a path as poses +geometry_msgs/Pose goal_pose # single nested message + +# Example of "message-like" content with scattered comments + +# Camera model parameters (assorted scalars with defaults) +float32 fx 0.0 +float32 fy 0.0 +float32 cx 0.0 +float32 cy 0.0 + +# Distortion coefficients (variable length) +float64[] D + +# Optional tags (strings). Bounded to keep memory in check. +string<=16 frame_id "map" +string<=16[] child_frame_id ["base_link", "camera_link", "lidar_link"] + +float32 quality 0.0 + +# Retry counters +uint16 retry_count 0 +uint16 max_retry 5 + +# Edge cases: blank lines, odd spacing, trailing comments, etc. + +# (blank line below) + +# leading spaces before a field + bool has_calibration true # default with extra spaces + +# weird spacing between type/name/default +int32 status_code 0 + +# comment after an unbounded array +string[] notes # foobar + "#; + + let spec = MessageSpecification::parse("test", input).unwrap(); + assert_eq!(spec.fields.len(), 30); + assert_eq!(spec.constants.len(), 13); + + // check first constant + assert_eq!(spec.constants[0].name, "MODE_IDLE"); + assert_eq!(spec.constants[0].value, Literal::UInt(0)); + + // check float constant + assert_eq!(spec.constants[11].name, "DEFAULT_SCALE"); + assert_eq!(spec.constants[11].value, Literal::Float(1.5)); + + // check string constant with quotes + assert_eq!(spec.constants[12].name, "DEFAULT_LABEL"); + assert_eq!( + spec.constants[12].value, + Literal::String("calib_v1".to_owned()) + ); + + // check first field + assert_eq!(spec.fields[0].name, "header"); + assert_eq!( + spec.fields[0].ty, + Type::Complex(ComplexType::Absolute { + package: "std_msgs".to_owned(), + name: "Header".to_owned() + }) + ); + + // check bounded string field with default + assert_eq!(spec.fields[5].name, "label"); + assert_eq!( + spec.fields[5].ty, + Type::BuiltIn(BuiltInType::String(Some(32))) + ); + assert_eq!( + spec.fields[5].default, + Some(Literal::String("default_label".to_owned())) + ); + + // check unbounded array field + assert_eq!(spec.fields[12].name, "indices"); + assert_eq!( + spec.fields[12].ty, + Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::Int32)), + size: ArraySize::Unbounded + } + ); + + // check fixed-size array field + assert_eq!(spec.fields[9].name, "K"); + assert_eq!( + spec.fields[9].ty, + Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::Float32)), + size: ArraySize::Fixed(9) + } + ); + + // check bounded string array field + assert_eq!(spec.fields[23].name, "child_frame_id"); + assert_eq!( + spec.fields[23].ty, + Type::Array { + ty: Box::new(Type::BuiltIn(BuiltInType::String(Some(16)))), + size: ArraySize::Unbounded + } + ); + assert_eq!( + spec.fields[23].default, + Some(Literal::Array(vec![ + Literal::String("base_link".to_owned()), + Literal::String("camera_link".to_owned()), + Literal::String("lidar_link".to_owned()), + ])) + ); + } +} diff --git a/tests/assets/rrd/snippets/howto/load_mcap.rrd b/tests/assets/rrd/snippets/howto/load_mcap.rrd index 12207f6ffcea..a9baf47d2fb5 100644 --- a/tests/assets/rrd/snippets/howto/load_mcap.rrd +++ b/tests/assets/rrd/snippets/howto/load_mcap.rrd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b805ccd67c52534a46ff653634ed5147bbeb61aaa2d7fa8733f6c8bb5e3c8fb9 -size 19413386 +oid sha256:c8f6ee32d3d1833719f437a11bffbe46a3e31fcbb598ae9b7ac9eb78c81eb286 +size 19248664