From 8cc7c0126816e00ef927d2ea3f915164d99e6ccd Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Fri, 19 Sep 2025 12:34:44 +0200 Subject: [PATCH 01/21] IDL --- crates/utils/re_mcap/src/layers/raw.rs | 1 + crates/utils/re_mcap/src/layers/ros2.rs | 31 +- .../re_mcap/src/parsers/ros2msg/idl/mod.rs | 579 ++++++++++++++++++ .../utils/re_mcap/src/parsers/ros2msg/mod.rs | 2 + 4 files changed, 608 insertions(+), 5 deletions(-) create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs 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..b7c35c25a980 100644 --- a/crates/utils/re_mcap/src/layers/ros2.rs +++ b/crates/utils/re_mcap/src/layers/ros2.rs @@ -1,10 +1,11 @@ use std::collections::BTreeMap; use super::MessageLayer; -use crate::{ - parsers::MessageParser, - parsers::ros2msg::{ +use crate::parsers::{ + MessageParser, + ros2msg::{ Ros2MessageParser, + idl::{MessageSchema, MessageSpec}, rcl_interfaces::LogMessageParser, sensor_msgs::{ BatteryStateMessageParser, CameraInfoMessageParser, CompressedImageMessageParser, @@ -92,9 +93,28 @@ impl MessageLayer for McapRos2Layer { } fn supports_channel(&self, channel: &mcap::Channel<'_>) -> bool { - channel.schema.as_ref().is_some_and(|s| { + if channel.schema.as_ref().is_some_and(|s| { s.encoding.as_str() == Self::ENCODING && self.registry.contains_key(&s.name) - }) + }) { + true + } else { + let schema = channel.schema.as_ref().unwrap(); + let schema_content = String::from_utf8_lossy(schema.data.as_ref()); + if let Ok(message_spec) = MessageSchema::parse(schema.name.clone(), &schema_content) { + re_log::warn_once!( + "Message schema {:?} is currently not supported, but parsed spec: {message_spec:#?}", + schema.name + ); + } else { + re_log::warn_once!( + "Message schema {:?} is currently not supported, and failed to parse spec", + schema.name + ); + re_log::warn_once!("Schema content:\n{}", schema_content); + } + + false + } } fn message_parser( @@ -114,6 +134,7 @@ impl MessageLayer for McapRos2Layer { "Message schema {:?} is currently not supported", schema.name ); + None } } diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs new file mode 100644 index 000000000000..8b9ea57306bd --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs @@ -0,0 +1,579 @@ +//! 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 _, Result, bail}; + +/// A parsed ROS 2 message. +#[derive(Debug, Clone, PartialEq)] +pub struct MessageSpec { + /// Fields that make up the message payload. + pub fields: Vec, + + /// Compile-time constants defined alongside fields. + pub constants: Vec, +} + +impl MessageSpec { + pub fn parse(input: &str) -> anyhow::Result { + let mut fields = Vec::new(); + let mut constants = Vec::new(); + + for (line_num, line) in input.lines().enumerate() { + let line = strip_comment(line).trim(); + if line.is_empty() { + continue; + } + + if is_schema_separator(line) { + continue; + } + + if Constant::is_constant_line(line) { + let constant = Constant::parse(line) + .with_context(|| format!("failed to parse constant on line {line_num}"))?; + constants.push(constant); + } else { + let field = Field::parse(line) + .with_context(|| format!("failed to parse field on line {line_num}"))?; + fields.push(field); + } + } + + Ok(Self { fields, constants }) + } +} + +fn is_schema_separator(line: &str) -> bool { + let line = line.trim(); + line.len() >= 79 && line.chars().all(|c| c == '=') +} + +fn parse_schema_name(line: &str) -> Option<&str> { + line.trim().strip_prefix("MSG: ").map(str::trim) +} + +/// A message field. +#[derive(Debug, Clone, PartialEq)] +pub struct Field { + pub ty: Type, + pub name: String, + pub default: Option, +} + +impl Field { + fn parse(line: &str) -> anyhow::Result { + let line = line.trim(); + let mut parts = line.split_whitespace(); + + let type_str = parts + .next() + .with_context(|| format!("field definition (`{line}`) missing type"))?; + + let name = parts + .next() + .with_context(|| format!("field definition (`{line}`) missing name"))?; + + let optional_default = parts.next(); + + if parts.next().is_some() { + bail!("field definition (`{line}`) has too many parts"); + } + + let ty = Type::parse(type_str).with_context(|| { + format!("failed to parse type `{type_str}` in field definition `{line}`") + })?; + + let default = if let Some(default_str) = optional_default { + Some(Literal::parse(default_str, &ty).with_context(|| { + format!( + "failed to parse default value `{default_str}` for type `{ty:?}` in field definition `{line}`" + ) + })?) + } else { + None + }; + + Ok(Self { + ty, + name: name.to_owned(), + default, + }) + } +} + +#[derive(Debug, Clone, PartialEq, Eq)] +pub enum PrimitiveType { + Bool, + Byte, + Char, + Float32, + Float64, + Int8, + Int16, + Int32, + Int64, + UInt8, + UInt16, + UInt32, + UInt64, +} + +#[derive(Debug, Clone, PartialEq)] +pub enum Type { + Primitive(PrimitiveType), + String(Option), // Optional max length for bounded strings. + 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) -> anyhow::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::Primitive(PrimitiveType::Bool)), + "byte" => Ok(Self::Primitive(PrimitiveType::Byte)), + "char" => Ok(Self::Primitive(PrimitiveType::Char)), + "float32" => Ok(Self::Primitive(PrimitiveType::Float32)), + "float64" => Ok(Self::Primitive(PrimitiveType::Float64)), + "int8" => Ok(Self::Primitive(PrimitiveType::Int8)), + "int16" => Ok(Self::Primitive(PrimitiveType::Int16)), + "int32" => Ok(Self::Primitive(PrimitiveType::Int32)), + "int64" => Ok(Self::Primitive(PrimitiveType::Int64)), + "uint8" => Ok(Self::Primitive(PrimitiveType::UInt8)), + "uint16" => Ok(Self::Primitive(PrimitiveType::UInt16)), + "uint32" => Ok(Self::Primitive(PrimitiveType::UInt32)), + "uint64" => Ok(Self::Primitive(PrimitiveType::UInt64)), + "string" => Ok(Self::String(None)), + s if s.starts_with("string") => Self::parse_bounded_string(s), // e.g. `string<=10` + s => ComplexType::parse(s).map(Type::Complex), + } + } + } + + fn parse_bounded_string(s: &str) -> anyhow::Result { + if s.starts_with("string<=") { + let len_str = &s["string<=".len()..s.len() - 1]; + let len = len_str + .parse::() + .context("failed to parse bounded string length")?; + Ok(Self::String(Some(len))) + } else { + bail!("invalid string type specifier: `{s}`"); + } + } +} + +#[derive(Debug, Clone, PartialEq, Eq, Hash)] +pub enum ComplexType { + Absolute { package: String, name: String }, + Relative { name: String }, +} + +impl ComplexType { + fn parse(s: &str) -> anyhow::Result { + if let Some((package, name)) = s.rsplit_once('/') { + if package.is_empty() || name.is_empty() { + bail!( + "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` format" + ); + } + Ok(Self::Absolute { + package: package.to_owned(), + name: name.to_owned(), + }) + } else { + if s.is_empty() { + bail!( + "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` or `SomeMessage` format" + ); + } + Ok(Self::Relative { name: s.to_owned() }) + } + } +} + +#[derive(Debug, Clone, PartialEq)] +pub enum ArraySize { + Fixed(usize), + Bounded(usize), + Unbounded, +} + +impl ArraySize { + fn parse(array_part: &str) -> Result { + let array_part = array_part + .strip_suffix(']') + .context("missing closing ']' in array type")?; + let array_size = if array_part.is_empty() { + Self::Unbounded + } else if let Ok(size) = array_part.parse::() { + Self::Fixed(size) + } else if array_part.ends_with('>') && array_part.starts_with('<') { + let size_str = &array_part[1..array_part.len() - 1]; + let size = size_str + .parse::() + .context("failed to parse bounded array size")?; + Self::Bounded(size) + } else { + bail!("invalid array size specifier: `{array_part}`"); + }; + Ok(array_size) + } +} + +#[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 { + match ty { + Type::Primitive(p) => match p { + PrimitiveType::Bool => { + let v = match s { + "true" => true, + "false" => false, + _ => bail!("invalid boolean literal: `{s}`"), + }; + Ok(Self::Bool(v)) + } + PrimitiveType::Byte + | PrimitiveType::Char + | PrimitiveType::Int8 + | PrimitiveType::Int16 + | PrimitiveType::Int32 + | PrimitiveType::Int64 => { + let v = s + .parse::() + .context("failed to parse integer literal")?; + Ok(Self::Int(v)) + } + PrimitiveType::UInt8 + | PrimitiveType::UInt16 + | PrimitiveType::UInt32 + | PrimitiveType::UInt64 => { + let v = s + .parse::() + .context("failed to parse unsigned integer literal")?; + Ok(Self::UInt(v)) + } + PrimitiveType::Float32 | PrimitiveType::Float64 => { + let v = s.parse::().context("failed to parse float literal")?; + Ok(Self::Float(v)) + } + }, + Type::String(_) => { + let s = s.trim_matches('"'); + Ok(Self::String(s.to_owned())) + } + Type::Array { + ty: elem_ty, + size: _, + } => { + let s = s.trim(); + if !s.starts_with('[') || !s.ends_with(']') { + bail!("array literal must start with '[' and end with ']': `{s}`"); + } + let inner = &s[1..s.len() - 1]; + let elems_str = inner.split(',').map(|e| e.trim()).filter(|e| !e.is_empty()); + let mut elems = Vec::new(); + for elem_str in elems_str { + let elem = Self::parse(elem_str, elem_ty)?; + elems.push(elem); + } + Ok(Self::Array(elems)) + } + Type::Complex(_) => bail!("cannot parse literal for named type"), + } + } +} + +/// A compile-time constant defined alongside fields. +#[derive(Debug, Clone, PartialEq)] +pub struct Constant { + pub ty: Type, + pub name: String, + pub value: Literal, +} + +impl Constant { + /// Determine if a line is a constant definition or a field definition. + /// + /// A constant definition has the following structure: ` =` + /// where `NAME` is all-caps with digits and underscores only, and `` is not an array. + /// + /// We look for the first `=` that is not inside quotes or brackets to make this determination. + fn is_constant_line(line: &str) -> bool { + let mut in_quote = false; + let mut bracket = 0usize; + for c in line.chars() { + match c { + '"' | '\'' => in_quote = !in_quote, + '[' => bracket += 1, + ']' => { + bracket = bracket.saturating_sub(1); + } + '=' if !in_quote && bracket == 0 => return true, + _ => {} + } + } + + false + } + + fn parse(line: &str) -> anyhow::Result { + let (type_and_name, value_str) = line + .split_once('=') + .context("constant definition missing '='")?; + let (type_str, name) = type_and_name + .trim() + .rsplit_once(' ') + .context("constant definition missing space between type and name")?; + + let ty = Type::parse(type_str)?; + let value = Literal::parse(value_str.trim(), &ty)?; + + if matches!(ty, Type::Array { .. }) { + bail!("constant type cannot be an array"); + } + if !name + .chars() + .all(|c| c.is_ascii_uppercase() || c.is_ascii_digit() || c == '_') + { + bail!("constant name must be all-caps alphanumeric and underscores only, got `{name}`"); + } + + Ok(Self { + ty, + name: name.to_owned(), + value, + }) + } +} + +fn strip_comment(s: &str) -> &str { + s.split_once('#').map(|(before, _)| before).unwrap_or(s) +} + +#[derive(Debug, Clone, PartialEq)] +pub struct MessageSchema { + pub name: String, + pub spec: MessageSpec, + pub dependencies: Vec, // Other message types referenced by this one. +} + +impl MessageSchema { + pub fn parse(name: String, input: &str) -> anyhow::Result { + let main_spec_content = extract_main_msg_spec(input); + let specs = extract_msg_specs(input); + + let main_spec = MessageSpec::parse(&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 = MessageSpec::parse(&dep_content) + .with_context(|| format!("failed to parse dependent message spec `{dep_name}`"))?; + dependencies.push(dep_spec); + } + + Ok(Self { + name, + spec: main_spec, + dependencies, + }) + } +} + +/// 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)) +} + +#[cfg(test)] +mod tests { + use cdr_encoding::CdrDeserializer; + use serde::Deserializer; + + use crate::parsers::dds; + + use super::*; + + #[test] + fn test_parse_message_spec() { + let input = r#" + # This is a comment + std_msgs/Header header + + int32 field1 + float64 field2 3.14 + string field3 "hello" + uint8[] field4 + + geometry_msgs/Point[] field5 + + uint32 CONST1=42 # inline comment + "#; + + MessageSpec::parse(input).unwrap(); + } + + #[test] + fn test_parse_message_schema() { + let input = r#" + geometry_msgs/TransformStamped[] transforms + + ================================================================================ + MSG: geometry_msgs/TransformStamped + # This expresses a transform from coordinate frame header.frame_id + # to the coordinate frame child_frame_id at the time of header.stamp + # + # This message is mostly used by the + # tf2 package. + # See its documentation for more information. + # + # The child_frame_id is necessary in addition to the frame_id + # in the Header to communicate the full reference for the transform + # in a self contained message. + + # The frame id in the header is used as the reference frame of this transform. + std_msgs/Header header + + # The frame id of the child frame to which this transform points. + string child_frame_id + + # Translation and rotation in 3-dimensions of child_frame_id from header.frame_id. + Transform transform + + ================================================================================ + MSG: geometry_msgs/Transform + # This represents the transform between two coordinate frames in free space. + + Vector3 translation + Quaternion rotation + + ================================================================================ + MSG: geometry_msgs/Quaternion + # This represents an orientation in free space in quaternion form. + + float64 x 0 + float64 y 0 + float64 z 0 + float64 w 1 + + ================================================================================ + MSG: geometry_msgs/Vector3 + # This represents a vector in free space. + + # This is semantically different than a point. + # A vector is always anchored at the origin. + # When a transform is applied to a vector, only the rotational component is applied. + + float64 x + float64 y + float64 z + + ================================================================================ + MSG: std_msgs/Header + # Standard metadata for higher-level stamped data types. + # This is generally used to communicate timestamped data + # in a particular coordinate frame. + + # Two-integer timestamp that is expressed as seconds and nanoseconds. + builtin_interfaces/Time stamp + + # Transform frame with which this data is associated. + string frame_id + + ================================================================================ + MSG: builtin_interfaces/Time + # This message communicates ROS Time defined here: + # https://design.ros2.org/articles/clock_and_time.html + + # The seconds component, valid over all int32 values. + int32 sec + + # The nanoseconds component, valid in the range [0, 10e9). + uint32 nanosec + "#; + const RAW_MSG: &[u8] = include_bytes!("../../../../../../../tf2_message.bin"); + + let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage".to_owned(), input); + let representation_identifier = + dds::RepresentationIdentifier::from_bytes(RAW_MSG[0..2].try_into().unwrap()).unwrap(); + + let payload = &RAW_MSG[4..]; + let mut deser = CdrDeserializer::::new(payload); + println!("representation_identifier: {representation_identifier:?}"); + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs index d65e7243835e..2e1743d88a4a 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs @@ -7,6 +7,8 @@ pub mod scalar_parser; pub mod sensor_msgs; pub mod std_msgs; +pub mod idl; + /// Trait for ROS2 message parsers that can be constructed with just a row count. pub trait Ros2MessageParser: MessageParser { /// Create a new parser instance. From e39792e811d3b69d69183ab7ff9739c78b71d01d Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 22 Sep 2025 10:30:03 +0200 Subject: [PATCH 02/21] implement parser layer --- crates/utils/re_mcap/src/layers/mod.rs | 7 +- .../re_mcap/src/layers/ros2_reflection.rs | 411 ++++++++++++++++++ .../src/parsers/ros2msg/idl/deserializer.rs | 362 +++++++++++++++ .../re_mcap/src/parsers/ros2msg/idl/mod.rs | 136 +++--- .../src/parsers/ros2msg/sensor_msgs/image.rs | 1 + 5 files changed, 843 insertions(+), 74 deletions(-) create mode 100644 crates/utils/re_mcap/src/layers/ros2_reflection.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs diff --git a/crates/utils/re_mcap/src/layers/mod.rs b/crates/utils/re_mcap/src/layers/mod.rs index bfc3821dd8e7..8e824cc72c08 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::{ @@ -323,7 +325,8 @@ impl LayerRegistry { .register_file_layer::() .register_file_layer::() // message layers (priority order): - .register_message_layer::() + // .register_message_layer::() + .register_message_layer::() .register_message_layer::(); if raw_fallback_enabled { 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..4c4045cfb6e8 --- /dev/null +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -0,0 +1,411 @@ +use std::collections::BTreeMap; + +use arrow::{ + array::{ + ArrayBuilder, BinaryBuilder, BooleanBuilder, FixedSizeListBuilder, Float32Builder, + Float64Builder, Int8Builder, Int16Builder, Int32Builder, Int64Builder, ListBuilder, + StringBuilder, StructBuilder, UInt8Builder, UInt16Builder, UInt32Builder, UInt64Builder, + }, + datatypes::{DataType, Field, Fields}, +}; +use re_chunk::{Chunk, ChunkId}; +use re_types::{ComponentDescriptor, reflection::ComponentDescriptorExt as _}; + +use crate::parsers::ros2msg::idl::{ + ArraySize, ComplexType, MessageSchema, MessageSpec, PrimitiveType, Type, + deserializer::{Value, decode_bytes}, +}; +use crate::parsers::{MessageParser, ParserContext}; +use crate::{Error, LayerIdentifier, MessageLayer}; + +struct Ros2ReflectionMessageParser { + message_schema: MessageSchema, + fields: BTreeMap>>, +} + +#[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("expected type {expected_type}, but found value {value:?}")] + UnexpectedValue { + expected_type: &'static str, + value: Value, + }, + + #[error("type {0} is not supported yet")] + UnsupportedType(&'static str), +} + +impl Ros2ReflectionMessageParser { + fn new(num_rows: usize, message_schema: MessageSchema) -> Self { + let mut fields = BTreeMap::new(); + + // Build Arrow builders for each field in the message + for field in &message_schema.spec.fields { + let name = field.name.clone(); + let builder = arrow_builder_from_type(&field.ty, &message_schema.dependencies); + fields.insert( + name, + FixedSizeListBuilder::with_capacity(builder, 1, num_rows), + ); + re_log::trace!("Added Arrow builder for field: {}", field.name); + } + + Self { + 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.name.clone(), + channel: msg.channel.topic.clone(), + source: err, + } + })?; + + println!("{}: {:#?}", msg.channel.topic, value); + + // if let Value::Message(message_fields) = value { + // // Iterate over all our builders, adding null values for missing fields + // for (field_name, builder) in &mut self.fields { + // if let Some(field_value) = message_fields.get(field_name) { + // append_value(builder.values(), field_value)?; + // builder.append(true); + // } else { + // append_null_value(builder.values(), field_name, &self.message_schema.spec)?; + // 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 { + message_schema, + fields, + } = *self; + + return Ok(vec![]); + } +} + +fn downcast_err<'a, T: std::any::Any>( + builder: &'a mut dyn ArrayBuilder, + val: &Value, +) -> Result<&'a mut T, Ros2ReflectionError> { + builder.as_any_mut().downcast_mut::().ok_or_else(|| { + let type_name = std::any::type_name::(); + Ros2ReflectionError::UnexpectedValue { + expected_type: type_name.strip_suffix("Builder").unwrap_or(type_name), + value: val.clone(), + } + }) +} + +fn append_null_value( + builder: &mut dyn ArrayBuilder, + field_name: &str, + message_spec: &MessageSpec, +) -> Result<(), Ros2ReflectionError> { + // Find the field type in the message spec to determine what kind of null value to append + if let Some(field) = message_spec.fields.iter().find(|f| f.name == field_name) { + append_null_value_for_type(builder, &field.ty)?; + } else { + re_log::warn!("Field {} not found in message spec", field_name); + } + Ok(()) +} + +fn append_null_value_for_type( + builder: &mut dyn ArrayBuilder, + field_type: &Type, +) -> Result<(), Ros2ReflectionError> { + match field_type { + Type::Primitive(p) => match p { + PrimitiveType::Bool => { + downcast_err::(builder, &Value::Bool(false))?.append_null() + } + PrimitiveType::Byte | PrimitiveType::UInt8 => { + downcast_err::(builder, &Value::U8(0))?.append_null() + } + PrimitiveType::Char | PrimitiveType::Int8 => { + downcast_err::(builder, &Value::I8(0))?.append_null() + } + PrimitiveType::Int16 => { + downcast_err::(builder, &Value::I16(0))?.append_null() + } + PrimitiveType::UInt16 => { + downcast_err::(builder, &Value::U16(0))?.append_null() + } + PrimitiveType::Int32 => { + downcast_err::(builder, &Value::I32(0))?.append_null() + } + PrimitiveType::UInt32 => { + downcast_err::(builder, &Value::U32(0))?.append_null() + } + PrimitiveType::Int64 => { + downcast_err::(builder, &Value::I64(0))?.append_null() + } + PrimitiveType::UInt64 => { + downcast_err::(builder, &Value::U64(0))?.append_null() + } + PrimitiveType::Float32 => { + downcast_err::(builder, &Value::F32(0.0))?.append_null() + } + PrimitiveType::Float64 => { + downcast_err::(builder, &Value::F64(0.0))?.append_null() + } + }, + Type::String(_) => { + downcast_err::(builder, &Value::String("".to_string()))?.append_null() + } + Type::Array { .. } => { + // For arrays, append an empty list + let list_builder = + downcast_err::>>(builder, &Value::Array(vec![]))?; + list_builder.append(false); // Empty list + } + Type::Complex(_) => { + // For complex types, just append null - we don't support nested messages anyway + re_log::trace!("Appending null for unsupported complex type"); + } + } + Ok(()) +} + +fn append_value(builder: &mut dyn ArrayBuilder, val: &Value) -> Result<(), Ros2ReflectionError> { + match val { + Value::Bool(x) => downcast_err::(builder, val)?.append_value(*x), + Value::I8(x) => downcast_err::(builder, val)?.append_value(*x), + Value::U8(x) => downcast_err::(builder, val)?.append_value(*x), + Value::I16(x) => downcast_err::(builder, val)?.append_value(*x), + Value::U16(x) => downcast_err::(builder, val)?.append_value(*x), + Value::I32(x) => downcast_err::(builder, val)?.append_value(*x), + Value::U32(x) => downcast_err::(builder, val)?.append_value(*x), + Value::I64(x) => downcast_err::(builder, val)?.append_value(*x), + Value::U64(x) => downcast_err::(builder, val)?.append_value(*x), + Value::F32(x) => downcast_err::(builder, val)?.append_value(*x), + Value::F64(x) => downcast_err::(builder, val)?.append_value(*x), + Value::String(x) => downcast_err::(builder, val)?.append_value(x.clone()), + Value::Message(_message_fields) => { + re_log::error_once!("Nested messages are not supported yet"); + return Ok(()); + } + Value::Array(vec) | Value::Seq(vec) => { + re_log::trace!("Append called on a list with {} elements", vec.len()); + let list_builder = downcast_err::>>(builder, val)?; + + for val in vec { + append_value(list_builder.values(), val)?; + } + list_builder.append(true); + re_log::trace!("Finished append on list with {} elements", vec.len()); + } + } + + Ok(()) +} + +fn struct_builder_from_message_spec( + spec: &MessageSpec, + dependencies: &[MessageSpec], +) -> StructBuilder { + let fields = spec + .fields + .iter() + .map(|f| arrow_field_from_type(&f.ty, &f.name, dependencies)) + .collect::(); + + let field_builders = spec + .fields + .iter() + .map(|f| arrow_builder_from_type(&f.ty, dependencies)) + .collect::>(); + + debug_assert_eq!(fields.len(), field_builders.len()); + + re_log::trace!( + "Created StructBuilder for message {} with fields: {:?}", + spec.name, + fields.iter().map(|f| f.name()).collect::>() + ); + StructBuilder::new(fields, field_builders) +} + +fn arrow_builder_from_type(ty: &Type, dependencies: &[MessageSpec]) -> Box { + let inner: Box = match ty { + Type::Primitive(p) => match p { + PrimitiveType::Bool => Box::new(BooleanBuilder::new()), + PrimitiveType::Byte | PrimitiveType::UInt8 => Box::new(UInt8Builder::new()), + PrimitiveType::Char | PrimitiveType::Int8 => Box::new(Int8Builder::new()), + PrimitiveType::Int16 => Box::new(Int16Builder::new()), + PrimitiveType::UInt16 => Box::new(UInt16Builder::new()), + PrimitiveType::Int32 => Box::new(Int32Builder::new()), + PrimitiveType::UInt32 => Box::new(UInt32Builder::new()), + PrimitiveType::Int64 => Box::new(Int64Builder::new()), + PrimitiveType::UInt64 => Box::new(UInt64Builder::new()), + PrimitiveType::Float32 => Box::new(Float32Builder::new()), + PrimitiveType::Float64 => Box::new(Float64Builder::new()), + }, + Type::String(_) => Box::new(StringBuilder::new()), + Type::Complex(complex_type) => { + // Look up the message spec in dependencies + if let Some(spec) = resolve_complex_type(complex_type, dependencies) { + Box::new(struct_builder_from_message_spec(spec, dependencies)) + } else { + re_log::warn_once!("Could not resolve complex type: {:?}", complex_type); + Box::new(BinaryBuilder::new()) // Fallback to binary + } + } + Type::Array { ty, .. } => match ty.as_ref() { + Type::String(_) => Box::new(ListBuilder::new(StringBuilder::new())), + _ => Box::new(ListBuilder::new(arrow_builder_from_type(ty, dependencies))), + }, + }; + + inner +} + +fn arrow_field_from_type(ty: &Type, name: &str, dependencies: &[MessageSpec]) -> Field { + Field::new(name, datatype_from_type(ty, dependencies), true) +} + +fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { + let inner = match ty { + Type::Primitive(p) => match p { + PrimitiveType::Bool => DataType::Boolean, + PrimitiveType::Byte | PrimitiveType::UInt8 => DataType::UInt8, + PrimitiveType::Char | PrimitiveType::Int8 => DataType::Int8, + PrimitiveType::Int16 => DataType::Int16, + PrimitiveType::UInt16 => DataType::UInt16, + PrimitiveType::Int32 => DataType::Int32, + PrimitiveType::UInt32 => DataType::UInt32, + PrimitiveType::Int64 => DataType::Int64, + PrimitiveType::UInt64 => DataType::UInt64, + PrimitiveType::Float32 => DataType::Float32, + PrimitiveType::Float64 => DataType::Float64, + }, + Type::String(_) => DataType::Utf8, + Type::Complex(complex_type) => { + if let Some(spec) = resolve_complex_type(complex_type, dependencies) { + let fields = spec + .fields + .iter() + .map(|f| arrow_field_from_type(&f.ty, &f.name, dependencies)) + .collect::(); + DataType::Struct(fields) + } else { + DataType::Binary // Fallback + } + } + Type::Array { ty, size } => match size { + ArraySize::Fixed(_) | ArraySize::Bounded(_) | ArraySize::Unbounded => { + DataType::new_list(datatype_from_type(ty, dependencies), true) + } + }, + }; + + inner +} + +fn resolve_complex_type<'a>( + complex_type: &ComplexType, + dependencies: &'a [MessageSpec], +) -> Option<&'a MessageSpec> { + 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 uses the IDL deserializer to provide dynamic parsing of ROS2 messages +/// without requiring pre-compiled message definitions. It results in a direct Arrow +/// representation of the message 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.clone(), &schema_content) + .map_err(|err| Error::InvalidSchema { + schema: schema.name.clone(), + source: err.into(), + })?; + + let found = self + .schemas_per_topic + .insert(channel.topic.clone(), message_schema); + debug_assert!(found.is_none()); + } + + 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; + } + + self.schemas_per_topic.contains_key(&channel.topic) + } + + 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(), + ))) + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs new file mode 100644 index 000000000000..7c6717c436ec --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs @@ -0,0 +1,362 @@ +use anyhow::{Context, bail}; +use cdr_encoding::CdrDeserializer; +use serde::de::{self, DeserializeSeed, Visitor}; +use std::collections::BTreeMap; +use std::fmt; + +use crate::parsers::{ + dds, + ros2msg::idl::{ComplexType, MessageSchema, MessageSpec, Type}, +}; + +pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { + // 4-byte encapsulation header + if buf.len() < 4 { + return bail!("short encapsulation"); + } + + let representation_identifier = dds::RepresentationIdentifier::from_bytes([buf[0], buf[1]]) + .with_context(|| "failed to parse CDR representation identifier")?; + + let mut resolver = std::collections::HashMap::new(); + for dep in &top.dependencies { + resolver.insert(dep.name.clone(), dep); + } + let resolver = MapResolver(resolver); + + let seed = MessageSeed { + spec: &top.spec, + type_resolver: &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") + } +} + +#[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), + Array(Vec), // fixed-size [N] + Seq(Vec), // variable-size [] or [<=N] + Message(BTreeMap), +} + +impl std::fmt::Debug for Value { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Value::Bool(v) => write!(f, "Bool({})", v), + Value::I8(v) => write!(f, "I8({})", v), + Value::U8(v) => write!(f, "U8({})", v), + Value::I16(v) => write!(f, "I16({})", v), + Value::U16(v) => write!(f, "U16({})", v), + Value::I32(v) => write!(f, "I32({})", v), + Value::U32(v) => write!(f, "U32({})", v), + Value::I64(v) => write!(f, "I64({})", v), + Value::U64(v) => write!(f, "U64({})", v), + Value::F32(v) => write!(f, "F32({})", v), + Value::F64(v) => write!(f, "F64({})", v), + Value::String(v) => write!(f, "String({:?})", v), + Value::Array(v) => write!(f, "Array({})", v.len()), + Value::Seq(v) => write!(f, "Seq({})", v.len()), + Value::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, ct: &ComplexType) -> Option<&MessageSpec>; +} + +/// A simple resolver backed by a map of full names ("pkg/Type" and/or "Type"). +pub struct MapResolver<'a>(pub std::collections::HashMap); + +impl TypeResolver for MapResolver<'_> { + fn resolve(&self, ct: &ComplexType) -> Option<&MessageSpec> { + match ct { + ComplexType::Absolute { package, name } => { + self.0.get(&format!("{package}/{name}")).copied() + } + ComplexType::Relative { name } => self.0.get(name).copied(), + } + } +} + +// One value, driven by a Type + resolver. +struct SchemaSeed<'a, R: TypeResolver> { + ty: &'a Type, + r: &'a R, +} +// Whole message (struct) in field order. +struct MessageSeed<'a, R: TypeResolver> { + spec: &'a MessageSpec, + type_resolver: &'a R, +} +// Sequence/array of elements. +struct SeqSeed<'a, R: TypeResolver> { + elem: &'a Type, + fixed_len: Option, + r: &'a R, +} + +struct PrimitiveVisitor(std::marker::PhantomData); + +impl<'de> Visitor<'de> 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) + } +} +macro_rules! impl_primitive_visitor { + ($t:ty, $m:ident) => { + impl<'de> Visitor<'de> 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); + +struct StringVisitor; + +impl<'de> Visitor<'de> 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()) + } +} + +impl<'a, 'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'a, R> { + type Value = Value; + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + use super::ArraySize::*; + use super::PrimitiveType::*; + use super::Type; + + match self.ty { + Type::Primitive(p) => match p { + Bool => de + .deserialize_bool(PrimitiveVisitor::(Default::default())) + .map(Value::Bool), + Byte | UInt8 => de + .deserialize_u8(PrimitiveVisitor::(Default::default())) + .map(Value::U8), // ROS2: octet + Char | Int8 => de + .deserialize_i8(PrimitiveVisitor::(Default::default())) + .map(Value::I8), // ROS2: char (int8) + Float32 => de + .deserialize_f32(PrimitiveVisitor::(Default::default())) + .map(Value::F32), + Float64 => de + .deserialize_f64(PrimitiveVisitor::(Default::default())) + .map(Value::F64), + Int16 => de + .deserialize_i16(PrimitiveVisitor::(Default::default())) + .map(Value::I16), + Int32 => de + .deserialize_i32(PrimitiveVisitor::(Default::default())) + .map(Value::I32), + Int64 => de + .deserialize_i64(PrimitiveVisitor::(Default::default())) + .map(Value::I64), + UInt16 => de + .deserialize_u16(PrimitiveVisitor::(Default::default())) + .map(Value::U16), + UInt32 => de + .deserialize_u32(PrimitiveVisitor::(Default::default())) + .map(Value::U32), + UInt64 => de + .deserialize_u64(PrimitiveVisitor::(Default::default())) + .map(Value::U64), + }, + Type::String(_bound) => de.deserialize_string(StringVisitor).map(Value::String), + Type::Array { ty, size } => match size { + Fixed(n) => { + // CDR: fixed array has NO length prefix; `cdr_encoding` can be driven via tuple. + SeqSeed { + elem: ty, + fixed_len: Some(*n), + r: self.r, + } + .deserialize(de) + .map(Value::Array) + } + Bounded(_) | Unbounded => { + // CDR: length-prefixed sequence; serde side is a seq. + SeqSeed { + elem: ty, + fixed_len: None, + r: self.r, + } + .deserialize(de) + .map(Value::Seq) + } + }, + Type::Complex(complex_ty) => { + let msg = self.r.resolve(complex_ty).ok_or_else(|| { + de::Error::custom(format!("unknown ComplexType: {complex_ty:?}")) + })?; + + MessageSeed { + spec: msg, + type_resolver: self.r, + } + .deserialize(de) + } + } + } +} + +impl<'de, R: TypeResolver> DeserializeSeed<'de> for MessageSeed<'_, R> { + type Value = Value; + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + struct MessageVisitor<'a, R: TypeResolver> { + spec: &'a MessageSpec, + 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 f in &self.spec.fields { + let v = seq + .next_element_seed(SchemaSeed { + ty: &f.ty, + r: self.type_resolver, + })? + .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; + out.insert(f.name.clone(), v); + } + Ok(Value::Message(out)) + } + } + + de.deserialize_tuple( + self.spec.fields.len(), + MessageVisitor { + spec: self.spec, + type_resolver: self.type_resolver, + }, + ) + } +} + +// ---- Sequence/array ---- +impl<'de, R: TypeResolver> DeserializeSeed<'de> for SeqSeed<'_, R> { + type Value = Vec; + fn deserialize(self, de: D) -> Result + where + D: de::Deserializer<'de>, + { + struct SeqVisitor<'a, R: TypeResolver> { + elem: &'a Type, + fixed_len: Option, + type_resolver: &'a R, + } + impl<'de, R: TypeResolver> serde::de::Visitor<'de> for SeqVisitor<'_, R> { + type Value = Vec; + fn expecting(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "cdr seq/array") + } + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let mut out = self.fixed_len.map(Vec::with_capacity).unwrap_or_default(); + while let Some(v) = seq.next_element_seed(SchemaSeed { + ty: self.elem, + r: self.type_resolver, + })? { + out.push(v); + } + Ok(out) + } + } + match self.fixed_len { + Some(n) => de.deserialize_tuple( + n, + SeqVisitor { + elem: self.elem, + fixed_len: Some(n), + type_resolver: self.r, + }, + ), + None => de.deserialize_seq(SeqVisitor { + elem: self.elem, + fixed_len: None, + type_resolver: self.r, + }), + } + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs index 8b9ea57306bd..19d868848165 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs @@ -7,9 +7,14 @@ //! arrays, names, constants, default values). use anyhow::{Context as _, Result, bail}; +pub mod deserializer; + /// A parsed ROS 2 message. #[derive(Debug, Clone, PartialEq)] pub struct MessageSpec { + /// Name of the message type. + pub name: String, + /// Fields that make up the message payload. pub fields: Vec, @@ -18,7 +23,7 @@ pub struct MessageSpec { } impl MessageSpec { - pub fn parse(input: &str) -> anyhow::Result { + pub fn parse(name: &str, input: &str) -> anyhow::Result { let mut fields = Vec::new(); let mut constants = Vec::new(); @@ -43,7 +48,11 @@ impl MessageSpec { } } - Ok(Self { fields, constants }) + Ok(Self { + name: name.to_owned(), + fields, + constants, + }) } } @@ -388,12 +397,12 @@ impl MessageSchema { let main_spec_content = extract_main_msg_spec(input); let specs = extract_msg_specs(input); - let main_spec = MessageSpec::parse(&main_spec_content) + let main_spec = MessageSpec::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 = MessageSpec::parse(&dep_content) + let dep_spec = MessageSpec::parse(&dep_name, &dep_content) .with_context(|| format!("failed to parse dependent message spec `{dep_name}`"))?; dependencies.push(dep_spec); } @@ -465,7 +474,10 @@ mod tests { use cdr_encoding::CdrDeserializer; use serde::Deserializer; - use crate::parsers::dds; + use crate::{ + cdr, + parsers::{dds, ros2msg::definitions::sensor_msgs}, + }; use super::*; @@ -485,95 +497,75 @@ mod tests { uint32 CONST1=42 # inline comment "#; - MessageSpec::parse(input).unwrap(); + MessageSpec::parse("test", input).unwrap(); } #[test] fn test_parse_message_schema() { let input = r#" - geometry_msgs/TransformStamped[] transforms - - ================================================================================ - MSG: geometry_msgs/TransformStamped - # This expresses a transform from coordinate frame header.frame_id - # to the coordinate frame child_frame_id at the time of header.stamp - # - # This message is mostly used by the - # tf2 package. - # See its documentation for more information. - # - # The child_frame_id is necessary in addition to the frame_id - # in the Header to communicate the full reference for the transform - # in a self contained message. - - # The frame id in the header is used as the reference frame of this transform. - std_msgs/Header header - - # The frame id of the child frame to which this transform points. - string child_frame_id - - # Translation and rotation in 3-dimensions of child_frame_id from header.frame_id. - Transform transform +# This message contains an uncompressed image +# (0, 0) is at top-left corner of image - ================================================================================ - MSG: geometry_msgs/Transform - # This represents the transform between two coordinate frames in free space. +std_msgs/Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of cameara + # +x should point to the right in the image + # +y should point down in the image + # +z should point into to plane of the image + # If the frame_id here and the frame_id of the CameraInfo + # message associated with the image conflict + # the behavior is undefined - Vector3 translation - Quaternion rotation +uint32 height # image height, that is, number of rows +uint32 width # image width, that is, number of columns - ================================================================================ - MSG: geometry_msgs/Quaternion - # This represents an orientation in free space in quaternion form. +# The legal values for encoding are in file src/image_encodings.cpp +# If you want to standardize a new string format, join +# ros-users@lists.ros.org and send an email proposing a new encoding. - float64 x 0 - float64 y 0 - float64 z 0 - float64 w 1 +string encoding # Encoding of pixels -- channel meaning, ordering, size + # taken from the list of strings in include/sensor_msgs/image_encodings.hpp - ================================================================================ - MSG: geometry_msgs/Vector3 - # This represents a vector in free space. +uint8 is_bigendian # is this data bigendian? +uint32 step # Full row length in bytes +uint8[] data # actual matrix data, size is (step * rows) - # This is semantically different than a point. - # A vector is always anchored at the origin. - # When a transform is applied to a vector, only the rotational component is applied. +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. - float64 x - float64 y - float64 z +# Two-integer timestamp that is expressed as seconds and nanoseconds. +builtin_interfaces/Time stamp - ================================================================================ - MSG: std_msgs/Header - # Standard metadata for higher-level stamped data types. - # This is generally used to communicate timestamped data - # in a particular coordinate frame. +# Transform frame with which this data is associated. +string frame_id - # Two-integer timestamp that is expressed as seconds and nanoseconds. - builtin_interfaces/Time stamp +================================================================================ +MSG: builtin_interfaces/Time +# This message communicates ROS Time defined here: +# https://design.ros2.org/articles/clock_and_time.html - # Transform frame with which this data is associated. - string frame_id +# The seconds component, valid over all int32 values. +int32 sec - ================================================================================ - MSG: builtin_interfaces/Time - # This message communicates ROS Time defined here: - # https://design.ros2.org/articles/clock_and_time.html +# The nanoseconds component, valid in the range [0, 10e9). +uint32 nanosec - # The seconds component, valid over all int32 values. - int32 sec - - # The nanoseconds component, valid in the range [0, 10e9). - uint32 nanosec "#; - const RAW_MSG: &[u8] = include_bytes!("../../../../../../../tf2_message.bin"); + const RAW_MSG: &[u8] = include_bytes!("../../../../../../../last_image_msg.bin"); - let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage".to_owned(), input); + let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage".to_owned(), input).unwrap(); let representation_identifier = dds::RepresentationIdentifier::from_bytes(RAW_MSG[0..2].try_into().unwrap()).unwrap(); let payload = &RAW_MSG[4..]; - let mut deser = CdrDeserializer::::new(payload); - println!("representation_identifier: {representation_identifier:?}"); + let mut de = CdrDeserializer::::new(payload); + + let mut resolver = std::collections::HashMap::new(); + for dep in &spec.dependencies { + resolver.insert(dep.name.clone(), dep); + } } } 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..11b07630aa0c 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 @@ -63,6 +63,7 @@ impl Ros2MessageParser for ImageMessageParser { impl MessageParser for ImageMessageParser { fn append(&mut self, ctx: &mut ParserContext, msg: &mcap::Message<'_>) -> anyhow::Result<()> { re_tracing::profile_function!(); + // save raw message content for debugging: let sensor_msgs::Image { header, data, From 17bc3f008c7d89a69ac3f4c7f421c9aad0ebad96 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Tue, 23 Sep 2025 13:04:33 +0200 Subject: [PATCH 03/21] debug --- .../re_mcap/src/layers/ros2_reflection.rs | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index 4c4045cfb6e8..f6f3c55b23fb 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -48,6 +48,10 @@ impl Ros2ReflectionMessageParser { // Build Arrow builders for each field in the message for field in &message_schema.spec.fields { + println!( + "Creating builder for field: {} of type {:?}", + field.name, field.ty + ); let name = field.name.clone(); let builder = arrow_builder_from_type(&field.ty, &message_schema.dependencies); fields.insert( @@ -78,20 +82,21 @@ impl MessageParser for Ros2ReflectionMessageParser { println!("{}: {:#?}", msg.channel.topic, value); - // if let Value::Message(message_fields) = value { - // // Iterate over all our builders, adding null values for missing fields - // for (field_name, builder) in &mut self.fields { - // if let Some(field_value) = message_fields.get(field_name) { - // append_value(builder.values(), field_value)?; - // builder.append(true); - // } else { - // append_null_value(builder.values(), field_name, &self.message_schema.spec)?; - // builder.append(false); - // } - // } - // } else { - // return Err(anyhow::anyhow!("Expected message value, got {:?}", value)); - // } + if let Value::Message(message_fields) = value { + // Iterate over all our builders, adding null values for missing fields + for (field_name, builder) in &mut self.fields { + println!("Appending field: {}", field_name); + if let Some(field_value) = message_fields.get(field_name) { + append_value(builder.values(), field_value)?; + builder.append(true); + } else { + append_null_value(builder.values(), field_name, &self.message_schema.spec)?; + builder.append(false); + } + } + } else { + return Err(anyhow::anyhow!("Expected message value, got {:?}", value)); + } Ok(()) } @@ -101,6 +106,8 @@ impl MessageParser for Ros2ReflectionMessageParser { let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); + panic!("done!"); + let Self { message_schema, fields, From ccc8f10d91cc5f4a718f0988ed0c3a1866bc72ab Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Wed, 24 Sep 2025 10:41:12 +0200 Subject: [PATCH 04/21] fix parsing --- .../re_mcap/src/layers/ros2_reflection.rs | 269 ++++++++++-------- .../src/parsers/ros2msg/idl/deserializer.rs | 57 ++-- 2 files changed, 182 insertions(+), 144 deletions(-) diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index f6f3c55b23fb..c0173eae529f 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -1,4 +1,4 @@ -use std::collections::BTreeMap; +use std::collections::HashSet; use arrow::{ array::{ @@ -20,7 +20,7 @@ use crate::{Error, LayerIdentifier, MessageLayer}; struct Ros2ReflectionMessageParser { message_schema: MessageSchema, - fields: BTreeMap>>, + fields: Vec<(String, FixedSizeListBuilder>)>, } #[derive(Debug, thiserror::Error)] @@ -35,7 +35,7 @@ pub enum Ros2ReflectionError { #[error("expected type {expected_type}, but found value {value:?}")] UnexpectedValue { expected_type: &'static str, - value: Value, + value: String, }, #[error("type {0} is not supported yet")] @@ -44,20 +44,18 @@ pub enum Ros2ReflectionError { impl Ros2ReflectionMessageParser { fn new(num_rows: usize, message_schema: MessageSchema) -> Self { - let mut fields = BTreeMap::new(); + let mut fields = Vec::new(); - // Build Arrow builders for each field in the message + println!("{message_schema:#?}"); + + // Build Arrow builders for each field in the message, preserving order for field in &message_schema.spec.fields { - println!( - "Creating builder for field: {} of type {:?}", - field.name, field.ty - ); let name = field.name.clone(); let builder = arrow_builder_from_type(&field.ty, &message_schema.dependencies); - fields.insert( - name, + fields.push(( + name.clone(), FixedSizeListBuilder::with_capacity(builder, 1, num_rows), - ); + )); re_log::trace!("Added Arrow builder for field: {}", field.name); } @@ -80,17 +78,17 @@ impl MessageParser for Ros2ReflectionMessageParser { } })?; - println!("{}: {:#?}", msg.channel.topic, value); - if let Value::Message(message_fields) = value { - // Iterate over all our builders, adding null values for missing fields + // 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 { - println!("Appending field: {}", field_name); if let Some(field_value) = message_fields.get(field_name) { - append_value(builder.values(), field_value)?; + re_log::trace!("Field {} found in message, appending value", field_name); + append_value(builder.values(), field_value, &self.message_schema)?; builder.append(true); + re_log::trace!("Field {}: Finished writing to builders", field_name); } else { - append_null_value(builder.values(), field_name, &self.message_schema.spec)?; + re_log::trace!("Field {} missing in message, appending null", field_name); builder.append(false); } } @@ -106,102 +104,51 @@ impl MessageParser for Ros2ReflectionMessageParser { let entity_path = ctx.entity_path().clone(); let timelines = ctx.build_timelines(); - panic!("done!"); - let Self { message_schema, fields, } = *self; - return Ok(vec![]); + 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(message_schema.name.clone()), + builder.finish().into(), + ) + }) + .collect(), + ) + .map_err(|err| Error::Other(anyhow::anyhow!(err)))?; + + Ok(vec![message_chunk]) } } fn downcast_err<'a, T: std::any::Any>( builder: &'a mut dyn ArrayBuilder, - val: &Value, + _val: &Value, ) -> Result<&'a mut T, Ros2ReflectionError> { + let builder_type_name = std::any::type_name_of_val(builder).to_owned(); builder.as_any_mut().downcast_mut::().ok_or_else(|| { let type_name = std::any::type_name::(); Ros2ReflectionError::UnexpectedValue { expected_type: type_name.strip_suffix("Builder").unwrap_or(type_name), - value: val.clone(), + value: builder_type_name, } }) } -fn append_null_value( +fn append_value( builder: &mut dyn ArrayBuilder, - field_name: &str, - message_spec: &MessageSpec, -) -> Result<(), Ros2ReflectionError> { - // Find the field type in the message spec to determine what kind of null value to append - if let Some(field) = message_spec.fields.iter().find(|f| f.name == field_name) { - append_null_value_for_type(builder, &field.ty)?; - } else { - re_log::warn!("Field {} not found in message spec", field_name); - } - Ok(()) -} - -fn append_null_value_for_type( - builder: &mut dyn ArrayBuilder, - field_type: &Type, + val: &Value, + schema: &MessageSchema, ) -> Result<(), Ros2ReflectionError> { - match field_type { - Type::Primitive(p) => match p { - PrimitiveType::Bool => { - downcast_err::(builder, &Value::Bool(false))?.append_null() - } - PrimitiveType::Byte | PrimitiveType::UInt8 => { - downcast_err::(builder, &Value::U8(0))?.append_null() - } - PrimitiveType::Char | PrimitiveType::Int8 => { - downcast_err::(builder, &Value::I8(0))?.append_null() - } - PrimitiveType::Int16 => { - downcast_err::(builder, &Value::I16(0))?.append_null() - } - PrimitiveType::UInt16 => { - downcast_err::(builder, &Value::U16(0))?.append_null() - } - PrimitiveType::Int32 => { - downcast_err::(builder, &Value::I32(0))?.append_null() - } - PrimitiveType::UInt32 => { - downcast_err::(builder, &Value::U32(0))?.append_null() - } - PrimitiveType::Int64 => { - downcast_err::(builder, &Value::I64(0))?.append_null() - } - PrimitiveType::UInt64 => { - downcast_err::(builder, &Value::U64(0))?.append_null() - } - PrimitiveType::Float32 => { - downcast_err::(builder, &Value::F32(0.0))?.append_null() - } - PrimitiveType::Float64 => { - downcast_err::(builder, &Value::F64(0.0))?.append_null() - } - }, - Type::String(_) => { - downcast_err::(builder, &Value::String("".to_string()))?.append_null() - } - Type::Array { .. } => { - // For arrays, append an empty list - let list_builder = - downcast_err::>>(builder, &Value::Array(vec![]))?; - list_builder.append(false); // Empty list - } - Type::Complex(_) => { - // For complex types, just append null - we don't support nested messages anyway - re_log::trace!("Appending null for unsupported complex type"); - } - } - Ok(()) -} - -fn append_value(builder: &mut dyn ArrayBuilder, val: &Value) -> Result<(), Ros2ReflectionError> { match val { Value::Bool(x) => downcast_err::(builder, val)?.append_value(*x), Value::I8(x) => downcast_err::(builder, val)?.append_value(*x), @@ -214,17 +161,97 @@ fn append_value(builder: &mut dyn ArrayBuilder, val: &Value) -> Result<(), Ros2R Value::U64(x) => downcast_err::(builder, val)?.append_value(*x), Value::F32(x) => downcast_err::(builder, val)?.append_value(*x), Value::F64(x) => downcast_err::(builder, val)?.append_value(*x), - Value::String(x) => downcast_err::(builder, val)?.append_value(x.clone()), - Value::Message(_message_fields) => { - re_log::error_once!("Nested messages are not supported yet"); - return Ok(()); + Value::String(x) => { + re_log::trace!(" - Append called on string with value: {:?}", x); + downcast_err::(builder, val)?.append_value(x.clone()); + } + Value::Message(message_fields) => { + re_log::trace!( + " - Append called on message with fields: {:?}", + message_fields.keys().collect::>() + ); + let struct_builder = downcast_err::(builder, val)?; + re_log::trace!( + " - Retrieved StructBuilder with {} fields", + struct_builder.num_fields() + ); + + // For nested messages, we need to find the matching MessageSpec from dependencies + // Since we don't have type information here, we'll try to match by field names + let mut matching_spec: Option<&MessageSpec> = None; + + // Try to find a MessageSpec that has the same field names as this message + for spec in &schema.dependencies { + let spec_field_names: HashSet<&String> = + spec.fields.iter().map(|f| &f.name).collect(); + let message_field_names: HashSet<&String> = message_fields.keys().collect(); + + if spec_field_names == message_field_names { + matching_spec = Some(spec); + break; + } + } + + if let Some(spec) = matching_spec { + // Use the spec field order to iterate through struct builder fields + for (ith_arrow_field, spec_field) in spec.fields.iter().enumerate() { + if let Some(field_builder) = + struct_builder.field_builders_mut().get_mut(ith_arrow_field) + { + let field_name = &spec_field.name; + re_log::trace!( + " - Processing field {} ({})", + ith_arrow_field, + field_name + ); + + if let Some(field_value) = message_fields.get(field_name) { + re_log::trace!( + " - Found field ({}) with val: {:?}", + field_name, + field_value + ); + append_value(field_builder, field_value, schema)?; + re_log::trace!( + " - Written field ({}) with val: {:?}", + field_name, + field_value + ); + } else { + re_log::trace!( + " - Field {} missing in message, skipping", + field_name + ); + } + } + } + } else { + re_log::warn!( + "Could not find matching MessageSpec for nested message with fields: {:?}", + message_fields.keys().collect::>() + ); + // Fallback: use the order from message_fields.keys() - not ideal but better than crashing + let message_field_names: Vec<&String> = message_fields.keys().collect(); + for (ith_arrow_field, field_builder) in + struct_builder.field_builders_mut().iter_mut().enumerate() + { + if let Some(&field_name) = message_field_names.get(ith_arrow_field) { + if let Some(field_value) = message_fields.get(field_name) { + append_value(field_builder, field_value, schema)?; + } + } + } + } + + struct_builder.append(true); } Value::Array(vec) | Value::Seq(vec) => { re_log::trace!("Append called on a list with {} elements", vec.len()); let list_builder = downcast_err::>>(builder, val)?; + re_log::trace!("Retrieved ListBuilder with values type"); for val in vec { - append_value(list_builder.values(), val)?; + append_value(list_builder.values(), val, schema)?; } list_builder.append(true); re_log::trace!("Finished append on list with {} elements", vec.len()); @@ -241,16 +268,16 @@ fn struct_builder_from_message_spec( let fields = spec .fields .iter() - .map(|f| arrow_field_from_type(&f.ty, &f.name, dependencies)) - .collect::(); - - let field_builders = spec - .fields - .iter() - .map(|f| arrow_builder_from_type(&f.ty, dependencies)) + .map(|f| { + ( + arrow_field_from_type(&f.ty, &f.name, dependencies), + arrow_builder_from_type(&f.ty, dependencies), + ) + }) .collect::>(); - debug_assert_eq!(fields.len(), field_builders.len()); + let (fields, field_builders): (Vec, Vec>) = + fields.into_iter().unzip(); re_log::trace!( "Created StructBuilder for message {} with fields: {:?}", @@ -261,7 +288,7 @@ fn struct_builder_from_message_spec( } fn arrow_builder_from_type(ty: &Type, dependencies: &[MessageSpec]) -> Box { - let inner: Box = match ty { + match ty { Type::Primitive(p) => match p { PrimitiveType::Bool => Box::new(BooleanBuilder::new()), PrimitiveType::Byte | PrimitiveType::UInt8 => Box::new(UInt8Builder::new()), @@ -285,13 +312,10 @@ fn arrow_builder_from_type(ty: &Type, dependencies: &[MessageSpec]) -> Box match ty.as_ref() { - Type::String(_) => Box::new(ListBuilder::new(StringBuilder::new())), - _ => Box::new(ListBuilder::new(arrow_builder_from_type(ty, dependencies))), - }, - }; - - inner + Type::Array { ty, .. } => { + Box::new(ListBuilder::new(arrow_builder_from_type(ty, dependencies))) + } + } } fn arrow_field_from_type(ty: &Type, name: &str, dependencies: &[MessageSpec]) -> Field { @@ -299,7 +323,7 @@ fn arrow_field_from_type(ty: &Type, name: &str, dependencies: &[MessageSpec]) -> } fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { - let inner = match ty { + match ty { Type::Primitive(p) => match p { PrimitiveType::Bool => DataType::Boolean, PrimitiveType::Byte | PrimitiveType::UInt8 => DataType::UInt8, @@ -315,6 +339,11 @@ fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { }, Type::String(_) => DataType::Utf8, Type::Complex(complex_type) => { + println!( + "Resolving complex type: {:?}, resolved to: {:?}", + complex_type, + resolve_complex_type(complex_type, dependencies) + ); if let Some(spec) = resolve_complex_type(complex_type, dependencies) { let fields = spec .fields @@ -331,9 +360,7 @@ fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { DataType::new_list(datatype_from_type(ty, dependencies), true) } }, - }; - - inner + } } fn resolve_complex_type<'a>( @@ -342,10 +369,10 @@ fn resolve_complex_type<'a>( ) -> Option<&'a MessageSpec> { dependencies.iter().find(|spec| match complex_type { ComplexType::Absolute { package, name } => { - spec.name == format!("{}/{}", package, name) || spec.name == *name + spec.name == format!("{package}/{name}") || spec.name == *name } ComplexType::Relative { name } => { - spec.name == *name || spec.name.ends_with(&format!("/{}", name)) + spec.name == *name || spec.name.ends_with(&format!("/{name}")) } }) } @@ -380,7 +407,7 @@ impl MessageLayer for McapRos2ReflectionLayer { let message_schema = MessageSchema::parse(schema.name.clone(), &schema_content) .map_err(|err| Error::InvalidSchema { schema: schema.name.clone(), - source: err.into(), + source: err, })?; let found = self diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs index 7c6717c436ec..165877451847 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs @@ -62,27 +62,27 @@ pub enum Value { impl std::fmt::Debug for Value { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { match self { - Value::Bool(v) => write!(f, "Bool({})", v), - Value::I8(v) => write!(f, "I8({})", v), - Value::U8(v) => write!(f, "U8({})", v), - Value::I16(v) => write!(f, "I16({})", v), - Value::U16(v) => write!(f, "U16({})", v), - Value::I32(v) => write!(f, "I32({})", v), - Value::U32(v) => write!(f, "U32({})", v), - Value::I64(v) => write!(f, "I64({})", v), - Value::U64(v) => write!(f, "U64({})", v), - Value::F32(v) => write!(f, "F32({})", v), - Value::F64(v) => write!(f, "F64({})", v), - Value::String(v) => write!(f, "String({:?})", v), - Value::Array(v) => write!(f, "Array({})", v.len()), - Value::Seq(v) => write!(f, "Seq({})", v.len()), - Value::Message(v) => { + 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::Seq(v) => write!(f, "Seq({})", v.len()), + 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, "{key}: {value:?}")?; } write!(f, "}}") } @@ -104,7 +104,14 @@ impl TypeResolver for MapResolver<'_> { ComplexType::Absolute { package, name } => { self.0.get(&format!("{package}/{name}")).copied() } - ComplexType::Relative { name } => self.0.get(name).copied(), + ComplexType::Relative { name } => { + for (k, v) in &self.0 { + if k.ends_with(&format!("/{name}")) || k == name { + return Some(*v); + } + } + None + } } } } @@ -128,7 +135,7 @@ struct SeqSeed<'a, R: TypeResolver> { struct PrimitiveVisitor(std::marker::PhantomData); -impl<'de> Visitor<'de> for PrimitiveVisitor { +impl Visitor<'_> for PrimitiveVisitor { type Value = bool; fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!(f, "bool") @@ -145,7 +152,7 @@ impl<'de> Visitor<'de> for PrimitiveVisitor { } macro_rules! impl_primitive_visitor { ($t:ty, $m:ident) => { - impl<'de> Visitor<'de> for PrimitiveVisitor<$t> { + impl Visitor<'_> for PrimitiveVisitor<$t> { type Value = $t; fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!(f, stringify!($t)) @@ -169,7 +176,7 @@ impl_primitive_visitor!(f64, visit_f64); struct StringVisitor; -impl<'de> Visitor<'de> for StringVisitor { +impl Visitor<'_> for StringVisitor { type Value = String; fn expecting(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!(f, "string") @@ -185,14 +192,17 @@ impl<'de> Visitor<'de> for StringVisitor { } } -impl<'a, 'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'a, R> { +impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { type Value = Value; fn deserialize(self, de: D) -> Result where D: de::Deserializer<'de>, { - use super::ArraySize::*; - use super::PrimitiveType::*; + use super::ArraySize::{Bounded, Fixed, Unbounded}; + use super::PrimitiveType::{ + Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, UInt8, UInt16, UInt32, + UInt64, + }; use super::Type; match self.ty { @@ -255,6 +265,7 @@ impl<'a, 'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'a, R> { } }, Type::Complex(complex_ty) => { + println!("Resolving complex type: {complex_ty:?}"); let msg = self.r.resolve(complex_ty).ok_or_else(|| { de::Error::custom(format!("unknown ComplexType: {complex_ty:?}")) })?; From 6c9e4f40fcf2223651def251242cf556dca33416 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Wed, 24 Sep 2025 14:32:43 +0200 Subject: [PATCH 05/21] cleanup --- .../re_mcap/src/layers/ros2_reflection.rs | 7 -- .../src/parsers/ros2msg/idl/deserializer.rs | 36 +++++-- .../re_mcap/src/parsers/ros2msg/idl/mod.rs | 98 +++++++++++++------ 3 files changed, 95 insertions(+), 46 deletions(-) diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index c0173eae529f..d0bdc939da38 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -46,8 +46,6 @@ impl Ros2ReflectionMessageParser { fn new(num_rows: usize, message_schema: MessageSchema) -> Self { let mut fields = Vec::new(); - println!("{message_schema:#?}"); - // Build Arrow builders for each field in the message, preserving order for field in &message_schema.spec.fields { let name = field.name.clone(); @@ -339,11 +337,6 @@ fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { }, Type::String(_) => DataType::Utf8, Type::Complex(complex_type) => { - println!( - "Resolving complex type: {:?}, resolved to: {:?}", - complex_type, - resolve_complex_type(complex_type, dependencies) - ); if let Some(spec) = resolve_complex_type(complex_type, dependencies) { let fields = spec .fields diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs index 165877451847..8c3e74143543 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs @@ -1,4 +1,4 @@ -use anyhow::{Context, bail}; +use anyhow::{Context as _, bail}; use cdr_encoding::CdrDeserializer; use serde::de::{self, DeserializeSeed, Visitor}; use std::collections::BTreeMap; @@ -12,7 +12,7 @@ use crate::parsers::{ pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { // 4-byte encapsulation header if buf.len() < 4 { - return bail!("short encapsulation"); + bail!("short encapsulation"); } let representation_identifier = dds::RepresentationIdentifier::from_bytes([buf[0], buf[1]]) @@ -265,7 +265,6 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { } }, Type::Complex(complex_ty) => { - println!("Resolving complex type: {complex_ty:?}"); let msg = self.r.resolve(complex_ty).ok_or_else(|| { de::Error::custom(format!("unknown ComplexType: {complex_ty:?}")) })?; @@ -340,18 +339,35 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SeqSeed<'_, R> { fn expecting(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { write!(f, "cdr seq/array") } + fn visit_seq(self, mut seq: A) -> Result where A: serde::de::SeqAccess<'de>, { - let mut out = self.fixed_len.map(Vec::with_capacity).unwrap_or_default(); - while let Some(v) = seq.next_element_seed(SchemaSeed { - ty: self.elem, - r: self.type_resolver, - })? { - out.push(v); + let cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); + let mut out = Vec::with_capacity(cap); + + if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { + for _ in 0..n { + let v = seq + .next_element_seed(SchemaSeed { + ty: self.elem, + r: 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 { + ty: self.elem, + r: self.type_resolver, + })? { + out.push(v); + } + Ok(out) } - Ok(out) } } match self.fixed_len { diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs index 19d868848165..c8e28f6729a2 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs @@ -65,7 +65,23 @@ fn parse_schema_name(line: &str) -> Option<&str> { line.trim().strip_prefix("MSG: ").map(str::trim) } -/// A message field. +/// 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, @@ -179,7 +195,7 @@ impl Type { let len_str = &s["string<=".len()..s.len() - 1]; let len = len_str .parse::() - .context("failed to parse bounded string length")?; + .with_context(|| "failed to parse bounded string length")?; Ok(Self::String(Some(len))) } else { bail!("invalid string type specifier: `{s}`"); @@ -187,9 +203,22 @@ impl Type { } } +/// A complex (non-primitive) type, possibly qualified with a package path. +/// +/// Examples: +/// ```text +/// // Absolute type with package +/// pkg/Type +/// +/// // Relative type without package +/// Type +/// ``` #[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 }, } @@ -211,12 +240,14 @@ impl ComplexType { "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` or `SomeMessage` format" ); } + Ok(Self::Relative { name: s.to_owned() }) } } } -#[derive(Debug, Clone, PartialEq)] +/// Size specifier for array types. +#[derive(Debug, Clone, PartialEq, Eq)] pub enum ArraySize { Fixed(usize), Bounded(usize), @@ -227,7 +258,8 @@ impl ArraySize { fn parse(array_part: &str) -> Result { let array_part = array_part .strip_suffix(']') - .context("missing closing ']' in array type")?; + .with_context(|| "Missing closing ']' in array type")?; + let array_size = if array_part.is_empty() { Self::Unbounded } else if let Ok(size) = array_part.parse::() { @@ -236,7 +268,7 @@ impl ArraySize { let size_str = &array_part[1..array_part.len() - 1]; let size = size_str .parse::() - .context("failed to parse bounded array size")?; + .with_context(|| "Failed to parse bounded array size")?; Self::Bounded(size) } else { bail!("invalid array size specifier: `{array_part}`"); @@ -245,6 +277,8 @@ impl ArraySize { } } +/// A literal value, used for default values and constant definitions. +/// Can be a primitive, string, or array of literals. #[derive(Debug, Clone, PartialEq)] pub enum Literal { Bool(bool), @@ -272,25 +306,21 @@ impl Literal { | PrimitiveType::Int8 | PrimitiveType::Int16 | PrimitiveType::Int32 - | PrimitiveType::Int64 => { - let v = s - .parse::() - .context("failed to parse integer literal")?; - Ok(Self::Int(v)) - } + | PrimitiveType::Int64 => s + .parse::() + .map(Self::Int) + .with_context(|| "failed to parse integer literal"), PrimitiveType::UInt8 | PrimitiveType::UInt16 | PrimitiveType::UInt32 - | PrimitiveType::UInt64 => { - let v = s - .parse::() - .context("failed to parse unsigned integer literal")?; - Ok(Self::UInt(v)) - } - PrimitiveType::Float32 | PrimitiveType::Float64 => { - let v = s.parse::().context("failed to parse float literal")?; - Ok(Self::Float(v)) - } + | PrimitiveType::UInt64 => s + .parse::() + .map(Self::UInt) + .with_context(|| "failed to parse unsigned integer literal"), + PrimitiveType::Float32 | PrimitiveType::Float64 => s + .parse::() + .map(Self::Float) + .with_context(|| "failed to parse float literal"), }, Type::String(_) => { let s = s.trim_matches('"'); @@ -301,6 +331,7 @@ impl Literal { size: _, } => { let s = s.trim(); + if !s.starts_with('[') || !s.ends_with(']') { bail!("array literal must start with '[' and end with ']': `{s}`"); } @@ -311,6 +342,7 @@ impl Literal { let elem = Self::parse(elem_str, elem_ty)?; elems.push(elem); } + Ok(Self::Array(elems)) } Type::Complex(_) => bail!("cannot parse literal for named type"), @@ -319,6 +351,19 @@ impl Literal { } /// 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, @@ -354,11 +399,11 @@ impl Constant { fn parse(line: &str) -> anyhow::Result { let (type_and_name, value_str) = line .split_once('=') - .context("constant definition missing '='")?; + .with_context(|| "constant definition missing '='")?; let (type_str, name) = type_and_name .trim() .rsplit_once(' ') - .context("constant definition missing space between type and name")?; + .with_context(|| "constant definition missing space between type and name")?; let ty = Type::parse(type_str)?; let value = Literal::parse(value_str.trim(), &ty)?; @@ -471,13 +516,8 @@ fn parse_section(lines: &[&str]) -> Option<(String, String)> { #[cfg(test)] mod tests { + use crate::parsers::dds; use cdr_encoding::CdrDeserializer; - use serde::Deserializer; - - use crate::{ - cdr, - parsers::{dds, ros2msg::definitions::sensor_msgs}, - }; use super::*; From 289d8243be9efea047ea5a43decef70c9a28e386 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Thu, 25 Sep 2025 14:54:45 +0200 Subject: [PATCH 06/21] refactor entire reflection module --- Cargo.lock | 2 + crates/utils/re_mcap/Cargo.toml | 1 + crates/utils/re_mcap/src/layers/ros2.rs | 4 +- .../re_mcap/src/layers/ros2_reflection.rs | 221 ++++--- .../src/parsers/ros2msg/idl/deserializer.rs | 389 ----------- .../re_mcap/src/parsers/ros2msg/idl/mod.rs | 611 ------------------ .../utils/re_mcap/src/parsers/ros2msg/mod.rs | 2 +- .../ros2msg/reflection/deserialize/message.rs | 65 ++ .../ros2msg/reflection/deserialize/mod.rs | 142 ++++ .../reflection/deserialize/primitive.rs | 76 +++ .../reflection/deserialize/primitive_array.rs | 124 ++++ .../ros2msg/reflection/deserialize/schema.rs | 120 ++++ .../reflection/deserialize/sequence.rs | 87 +++ .../ros2msg/reflection/message_spec.rs | 517 +++++++++++++++ .../src/parsers/ros2msg/reflection/mod.rs | 214 ++++++ 15 files changed, 1489 insertions(+), 1086 deletions(-) delete mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs delete mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs create mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs diff --git a/Cargo.lock b/Cargo.lock index fc186cd4fe35..b5bb5ef87f0a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -8986,6 +8986,8 @@ dependencies = [ "mcap", "prost-reflect", "re_chunk", + "re_error", + "re_format_arrow", "re_log", "re_log_types", "re_tracing", diff --git a/crates/utils/re_mcap/Cargo.toml b/crates/utils/re_mcap/Cargo.toml index 8426f5d84513..157bdcc25741 100644 --- a/crates/utils/re_mcap/Cargo.toml +++ b/crates/utils/re_mcap/Cargo.toml @@ -16,6 +16,7 @@ workspace = true [dependencies] re_chunk.workspace = true +re_error.workspace = true re_log.workspace = true re_log_types.workspace = true re_tracing.workspace = true diff --git a/crates/utils/re_mcap/src/layers/ros2.rs b/crates/utils/re_mcap/src/layers/ros2.rs index b7c35c25a980..246a651556ad 100644 --- a/crates/utils/re_mcap/src/layers/ros2.rs +++ b/crates/utils/re_mcap/src/layers/ros2.rs @@ -5,8 +5,8 @@ use crate::parsers::{ MessageParser, ros2msg::{ Ros2MessageParser, - idl::{MessageSchema, MessageSpec}, rcl_interfaces::LogMessageParser, + reflection::MessageSchema, sensor_msgs::{ BatteryStateMessageParser, CameraInfoMessageParser, CompressedImageMessageParser, FluidPressureMessageParser, IlluminanceMessageParser, ImageMessageParser, @@ -100,7 +100,7 @@ impl MessageLayer for McapRos2Layer { } else { let schema = channel.schema.as_ref().unwrap(); let schema_content = String::from_utf8_lossy(schema.data.as_ref()); - if let Ok(message_spec) = MessageSchema::parse(schema.name.clone(), &schema_content) { + if let Ok(message_spec) = MessageSchema::parse(&schema.name, &schema_content) { re_log::warn_once!( "Message schema {:?} is currently not supported, but parsed spec: {message_spec:#?}", schema.name diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index d0bdc939da38..83bab5326051 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -11,9 +11,11 @@ use arrow::{ use re_chunk::{Chunk, ChunkId}; use re_types::{ComponentDescriptor, reflection::ComponentDescriptorExt as _}; -use crate::parsers::ros2msg::idl::{ - ArraySize, ComplexType, MessageSchema, MessageSpec, PrimitiveType, Type, - deserializer::{Value, decode_bytes}, +use crate::parsers::ros2msg::reflection::{ + MessageSchema, + deserialize::primitive_array::PrimitiveArray, + deserialize::{Value, decode_bytes}, + message_spec::{ArraySize, BuiltInType, ComplexType, MessageSpecification, Type}, }; use crate::parsers::{MessageParser, ParserContext}; use crate::{Error, LayerIdentifier, MessageLayer}; @@ -54,7 +56,6 @@ impl Ros2ReflectionMessageParser { name.clone(), FixedSizeListBuilder::with_capacity(builder, 1, num_rows), )); - re_log::trace!("Added Arrow builder for field: {}", field.name); } Self { @@ -70,7 +71,7 @@ impl MessageParser for Ros2ReflectionMessageParser { let value = decode_bytes(&self.message_schema, msg.data.as_ref()).map_err(|err| { Ros2ReflectionError::InvalidMessage { - schema: self.message_schema.name.clone(), + schema: self.message_schema.spec.name.clone(), channel: msg.channel.topic.clone(), source: err, } @@ -81,12 +82,9 @@ impl MessageParser for Ros2ReflectionMessageParser { // 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) { - re_log::trace!("Field {} found in message, appending value", field_name); append_value(builder.values(), field_value, &self.message_schema)?; builder.append(true); - re_log::trace!("Field {}: Finished writing to builders", field_name); } else { - re_log::trace!("Field {} missing in message, appending null", field_name); builder.append(false); } } @@ -116,7 +114,7 @@ impl MessageParser for Ros2ReflectionMessageParser { .map(|(field_name, mut builder)| { ( ComponentDescriptor::partial(field_name) - .with_builtin_archetype(message_schema.name.clone()), + .with_builtin_archetype(message_schema.spec.name.clone()), builder.finish().into(), ) }) @@ -142,6 +140,90 @@ fn downcast_err<'a, T: std::any::Any>( }) } +fn append_primitive_array( + builder: &mut dyn ArrayBuilder, + prim_array: &PrimitiveArray, + val: &Value, +) -> Result<(), Ros2ReflectionError> { + match prim_array { + PrimitiveArray::Bool(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::I8(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::U8(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::I16(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::U16(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::I32(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::U32(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::I64(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::U64(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::F32(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::F64(vec) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + values_builder.append_slice(vec); + list_builder.append(true); + } + PrimitiveArray::String(items) => { + let list_builder = downcast_err::>>(builder, val)?; + let values_builder = downcast_err::(list_builder.values(), val)?; + for item in items { + values_builder.append_value(item); + } + list_builder.append(true); + } + } + Ok(()) +} + fn append_value( builder: &mut dyn ArrayBuilder, val: &Value, @@ -160,23 +242,14 @@ fn append_value( Value::F32(x) => downcast_err::(builder, val)?.append_value(*x), Value::F64(x) => downcast_err::(builder, val)?.append_value(*x), Value::String(x) => { - re_log::trace!(" - Append called on string with value: {:?}", x); downcast_err::(builder, val)?.append_value(x.clone()); } Value::Message(message_fields) => { - re_log::trace!( - " - Append called on message with fields: {:?}", - message_fields.keys().collect::>() - ); let struct_builder = downcast_err::(builder, val)?; - re_log::trace!( - " - Retrieved StructBuilder with {} fields", - struct_builder.num_fields() - ); // For nested messages, we need to find the matching MessageSpec from dependencies // Since we don't have type information here, we'll try to match by field names - let mut matching_spec: Option<&MessageSpec> = None; + let mut matching_spec: Option<&MessageSpecification> = None; // Try to find a MessageSpec that has the same field names as this message for spec in &schema.dependencies { @@ -197,29 +270,11 @@ fn append_value( struct_builder.field_builders_mut().get_mut(ith_arrow_field) { let field_name = &spec_field.name; - re_log::trace!( - " - Processing field {} ({})", - ith_arrow_field, - field_name - ); if let Some(field_value) = message_fields.get(field_name) { - re_log::trace!( - " - Found field ({}) with val: {:?}", - field_name, - field_value - ); append_value(field_builder, field_value, schema)?; - re_log::trace!( - " - Written field ({}) with val: {:?}", - field_name, - field_value - ); } else { - re_log::trace!( - " - Field {} missing in message, skipping", - field_name - ); + //TODO(gijsd): Field is missing in the message, append null } } } @@ -244,15 +299,15 @@ fn append_value( struct_builder.append(true); } Value::Array(vec) | Value::Seq(vec) => { - re_log::trace!("Append called on a list with {} elements", vec.len()); let list_builder = downcast_err::>>(builder, val)?; - re_log::trace!("Retrieved ListBuilder with values type"); for val in vec { append_value(list_builder.values(), val, schema)?; } list_builder.append(true); - re_log::trace!("Finished append on list with {} elements", vec.len()); + } + Value::PrimitiveArray(prim_array) | Value::PrimitiveSeq(prim_array) => { + append_primitive_array(builder, prim_array, val)?; } } @@ -260,8 +315,8 @@ fn append_value( } fn struct_builder_from_message_spec( - spec: &MessageSpec, - dependencies: &[MessageSpec], + spec: &MessageSpecification, + dependencies: &[MessageSpecification], ) -> StructBuilder { let fields = spec .fields @@ -277,30 +332,28 @@ fn struct_builder_from_message_spec( let (fields, field_builders): (Vec, Vec>) = fields.into_iter().unzip(); - re_log::trace!( - "Created StructBuilder for message {} with fields: {:?}", - spec.name, - fields.iter().map(|f| f.name()).collect::>() - ); StructBuilder::new(fields, field_builders) } -fn arrow_builder_from_type(ty: &Type, dependencies: &[MessageSpec]) -> Box { +fn arrow_builder_from_type( + ty: &Type, + dependencies: &[MessageSpecification], +) -> Box { match ty { - Type::Primitive(p) => match p { - PrimitiveType::Bool => Box::new(BooleanBuilder::new()), - PrimitiveType::Byte | PrimitiveType::UInt8 => Box::new(UInt8Builder::new()), - PrimitiveType::Char | PrimitiveType::Int8 => Box::new(Int8Builder::new()), - PrimitiveType::Int16 => Box::new(Int16Builder::new()), - PrimitiveType::UInt16 => Box::new(UInt16Builder::new()), - PrimitiveType::Int32 => Box::new(Int32Builder::new()), - PrimitiveType::UInt32 => Box::new(UInt32Builder::new()), - PrimitiveType::Int64 => Box::new(Int64Builder::new()), - PrimitiveType::UInt64 => Box::new(UInt64Builder::new()), - PrimitiveType::Float32 => Box::new(Float32Builder::new()), - PrimitiveType::Float64 => Box::new(Float64Builder::new()), + 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::String(_) => Box::new(StringBuilder::new()), Type::Complex(complex_type) => { // Look up the message spec in dependencies if let Some(spec) = resolve_complex_type(complex_type, dependencies) { @@ -316,26 +369,26 @@ fn arrow_builder_from_type(ty: &Type, dependencies: &[MessageSpec]) -> Box Field { +fn arrow_field_from_type(ty: &Type, name: &str, dependencies: &[MessageSpecification]) -> Field { Field::new(name, datatype_from_type(ty, dependencies), true) } -fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { +fn datatype_from_type(ty: &Type, dependencies: &[MessageSpecification]) -> DataType { match ty { - Type::Primitive(p) => match p { - PrimitiveType::Bool => DataType::Boolean, - PrimitiveType::Byte | PrimitiveType::UInt8 => DataType::UInt8, - PrimitiveType::Char | PrimitiveType::Int8 => DataType::Int8, - PrimitiveType::Int16 => DataType::Int16, - PrimitiveType::UInt16 => DataType::UInt16, - PrimitiveType::Int32 => DataType::Int32, - PrimitiveType::UInt32 => DataType::UInt32, - PrimitiveType::Int64 => DataType::Int64, - PrimitiveType::UInt64 => DataType::UInt64, - PrimitiveType::Float32 => DataType::Float32, - PrimitiveType::Float64 => DataType::Float64, + 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::String(_) => DataType::Utf8, Type::Complex(complex_type) => { if let Some(spec) = resolve_complex_type(complex_type, dependencies) { let fields = spec @@ -358,8 +411,8 @@ fn datatype_from_type(ty: &Type, dependencies: &[MessageSpec]) -> DataType { fn resolve_complex_type<'a>( complex_type: &ComplexType, - dependencies: &'a [MessageSpec], -) -> Option<&'a MessageSpec> { + 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 @@ -397,10 +450,12 @@ impl MessageLayer for McapRos2ReflectionLayer { } let schema_content = String::from_utf8_lossy(schema.data.as_ref()); - let message_schema = MessageSchema::parse(schema.name.clone(), &schema_content) - .map_err(|err| Error::InvalidSchema { - schema: schema.name.clone(), - source: err, + let message_schema = + MessageSchema::parse(&schema.name, &schema_content).map_err(|err| { + Error::InvalidSchema { + schema: schema.name.clone(), + source: err, + } })?; let found = self diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs deleted file mode 100644 index 8c3e74143543..000000000000 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/deserializer.rs +++ /dev/null @@ -1,389 +0,0 @@ -use anyhow::{Context as _, bail}; -use cdr_encoding::CdrDeserializer; -use serde::de::{self, DeserializeSeed, Visitor}; -use std::collections::BTreeMap; -use std::fmt; - -use crate::parsers::{ - dds, - ros2msg::idl::{ComplexType, MessageSchema, MessageSpec, Type}, -}; - -pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { - // 4-byte encapsulation header - if buf.len() < 4 { - bail!("short encapsulation"); - } - - let representation_identifier = dds::RepresentationIdentifier::from_bytes([buf[0], buf[1]]) - .with_context(|| "failed to parse CDR representation identifier")?; - - let mut resolver = std::collections::HashMap::new(); - for dep in &top.dependencies { - resolver.insert(dep.name.clone(), dep); - } - let resolver = MapResolver(resolver); - - let seed = MessageSeed { - spec: &top.spec, - type_resolver: &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") - } -} - -#[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), - Array(Vec), // fixed-size [N] - Seq(Vec), // variable-size [] or [<=N] - Message(BTreeMap), -} - -impl std::fmt::Debug for Value { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> 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::Seq(v) => write!(f, "Seq({})", v.len()), - 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, ct: &ComplexType) -> Option<&MessageSpec>; -} - -/// A simple resolver backed by a map of full names ("pkg/Type" and/or "Type"). -pub struct MapResolver<'a>(pub std::collections::HashMap); - -impl TypeResolver for MapResolver<'_> { - fn resolve(&self, ct: &ComplexType) -> Option<&MessageSpec> { - match ct { - ComplexType::Absolute { package, name } => { - self.0.get(&format!("{package}/{name}")).copied() - } - ComplexType::Relative { name } => { - for (k, v) in &self.0 { - if k.ends_with(&format!("/{name}")) || k == name { - return Some(*v); - } - } - None - } - } - } -} - -// One value, driven by a Type + resolver. -struct SchemaSeed<'a, R: TypeResolver> { - ty: &'a Type, - r: &'a R, -} -// Whole message (struct) in field order. -struct MessageSeed<'a, R: TypeResolver> { - spec: &'a MessageSpec, - type_resolver: &'a R, -} -// Sequence/array of elements. -struct SeqSeed<'a, R: TypeResolver> { - elem: &'a Type, - fixed_len: Option, - r: &'a R, -} - -struct PrimitiveVisitor(std::marker::PhantomData); - -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) - } -} -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); - -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()) - } -} - -impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { - type Value = Value; - fn deserialize(self, de: D) -> Result - where - D: de::Deserializer<'de>, - { - use super::ArraySize::{Bounded, Fixed, Unbounded}; - use super::PrimitiveType::{ - Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, UInt8, UInt16, UInt32, - UInt64, - }; - use super::Type; - - match self.ty { - Type::Primitive(p) => match p { - Bool => de - .deserialize_bool(PrimitiveVisitor::(Default::default())) - .map(Value::Bool), - Byte | UInt8 => de - .deserialize_u8(PrimitiveVisitor::(Default::default())) - .map(Value::U8), // ROS2: octet - Char | Int8 => de - .deserialize_i8(PrimitiveVisitor::(Default::default())) - .map(Value::I8), // ROS2: char (int8) - Float32 => de - .deserialize_f32(PrimitiveVisitor::(Default::default())) - .map(Value::F32), - Float64 => de - .deserialize_f64(PrimitiveVisitor::(Default::default())) - .map(Value::F64), - Int16 => de - .deserialize_i16(PrimitiveVisitor::(Default::default())) - .map(Value::I16), - Int32 => de - .deserialize_i32(PrimitiveVisitor::(Default::default())) - .map(Value::I32), - Int64 => de - .deserialize_i64(PrimitiveVisitor::(Default::default())) - .map(Value::I64), - UInt16 => de - .deserialize_u16(PrimitiveVisitor::(Default::default())) - .map(Value::U16), - UInt32 => de - .deserialize_u32(PrimitiveVisitor::(Default::default())) - .map(Value::U32), - UInt64 => de - .deserialize_u64(PrimitiveVisitor::(Default::default())) - .map(Value::U64), - }, - Type::String(_bound) => de.deserialize_string(StringVisitor).map(Value::String), - Type::Array { ty, size } => match size { - Fixed(n) => { - // CDR: fixed array has NO length prefix; `cdr_encoding` can be driven via tuple. - SeqSeed { - elem: ty, - fixed_len: Some(*n), - r: self.r, - } - .deserialize(de) - .map(Value::Array) - } - Bounded(_) | Unbounded => { - // CDR: length-prefixed sequence; serde side is a seq. - SeqSeed { - elem: ty, - fixed_len: None, - r: self.r, - } - .deserialize(de) - .map(Value::Seq) - } - }, - Type::Complex(complex_ty) => { - let msg = self.r.resolve(complex_ty).ok_or_else(|| { - de::Error::custom(format!("unknown ComplexType: {complex_ty:?}")) - })?; - - MessageSeed { - spec: msg, - type_resolver: self.r, - } - .deserialize(de) - } - } - } -} - -impl<'de, R: TypeResolver> DeserializeSeed<'de> for MessageSeed<'_, R> { - type Value = Value; - fn deserialize(self, de: D) -> Result - where - D: de::Deserializer<'de>, - { - struct MessageVisitor<'a, R: TypeResolver> { - spec: &'a MessageSpec, - 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 f in &self.spec.fields { - let v = seq - .next_element_seed(SchemaSeed { - ty: &f.ty, - r: self.type_resolver, - })? - .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; - out.insert(f.name.clone(), v); - } - Ok(Value::Message(out)) - } - } - - de.deserialize_tuple( - self.spec.fields.len(), - MessageVisitor { - spec: self.spec, - type_resolver: self.type_resolver, - }, - ) - } -} - -// ---- Sequence/array ---- -impl<'de, R: TypeResolver> DeserializeSeed<'de> for SeqSeed<'_, R> { - type Value = Vec; - fn deserialize(self, de: D) -> Result - where - D: de::Deserializer<'de>, - { - struct SeqVisitor<'a, R: TypeResolver> { - elem: &'a Type, - fixed_len: Option, - type_resolver: &'a R, - } - impl<'de, R: TypeResolver> serde::de::Visitor<'de> for SeqVisitor<'_, R> { - type Value = Vec; - fn expecting(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "cdr seq/array") - } - - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - let cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); - let mut out = Vec::with_capacity(cap); - - if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { - for _ in 0..n { - let v = seq - .next_element_seed(SchemaSeed { - ty: self.elem, - r: 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 { - ty: self.elem, - r: self.type_resolver, - })? { - out.push(v); - } - Ok(out) - } - } - } - match self.fixed_len { - Some(n) => de.deserialize_tuple( - n, - SeqVisitor { - elem: self.elem, - fixed_len: Some(n), - type_resolver: self.r, - }, - ), - None => de.deserialize_seq(SeqVisitor { - elem: self.elem, - fixed_len: None, - type_resolver: self.r, - }), - } - } -} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs deleted file mode 100644 index c8e28f6729a2..000000000000 --- a/crates/utils/re_mcap/src/parsers/ros2msg/idl/mod.rs +++ /dev/null @@ -1,611 +0,0 @@ -//! 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 _, Result, bail}; - -pub mod deserializer; - -/// A parsed ROS 2 message. -#[derive(Debug, Clone, PartialEq)] -pub struct MessageSpec { - /// 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 MessageSpec { - pub fn parse(name: &str, input: &str) -> anyhow::Result { - let mut fields = Vec::new(); - let mut constants = Vec::new(); - - for (line_num, line) in input.lines().enumerate() { - let line = strip_comment(line).trim(); - if line.is_empty() { - continue; - } - - if is_schema_separator(line) { - continue; - } - - if Constant::is_constant_line(line) { - let constant = Constant::parse(line) - .with_context(|| format!("failed to parse constant on line {line_num}"))?; - constants.push(constant); - } else { - let field = Field::parse(line) - .with_context(|| format!("failed to parse field on line {line_num}"))?; - fields.push(field); - } - } - - Ok(Self { - name: name.to_owned(), - fields, - constants, - }) - } -} - -fn is_schema_separator(line: &str) -> bool { - let line = line.trim(); - line.len() >= 79 && line.chars().all(|c| c == '=') -} - -fn parse_schema_name(line: &str) -> Option<&str> { - line.trim().strip_prefix("MSG: ").map(str::trim) -} - -/// 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(line: &str) -> anyhow::Result { - let line = line.trim(); - let mut parts = line.split_whitespace(); - - let type_str = parts - .next() - .with_context(|| format!("field definition (`{line}`) missing type"))?; - - let name = parts - .next() - .with_context(|| format!("field definition (`{line}`) missing name"))?; - - let optional_default = parts.next(); - - if parts.next().is_some() { - bail!("field definition (`{line}`) has too many parts"); - } - - let ty = Type::parse(type_str).with_context(|| { - format!("failed to parse type `{type_str}` in field definition `{line}`") - })?; - - let default = if let Some(default_str) = optional_default { - Some(Literal::parse(default_str, &ty).with_context(|| { - format!( - "failed to parse default value `{default_str}` for type `{ty:?}` in field definition `{line}`" - ) - })?) - } else { - None - }; - - Ok(Self { - ty, - name: name.to_owned(), - default, - }) - } -} - -#[derive(Debug, Clone, PartialEq, Eq)] -pub enum PrimitiveType { - Bool, - Byte, - Char, - Float32, - Float64, - Int8, - Int16, - Int32, - Int64, - UInt8, - UInt16, - UInt32, - UInt64, -} - -#[derive(Debug, Clone, PartialEq)] -pub enum Type { - Primitive(PrimitiveType), - String(Option), // Optional max length for bounded strings. - 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) -> anyhow::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::Primitive(PrimitiveType::Bool)), - "byte" => Ok(Self::Primitive(PrimitiveType::Byte)), - "char" => Ok(Self::Primitive(PrimitiveType::Char)), - "float32" => Ok(Self::Primitive(PrimitiveType::Float32)), - "float64" => Ok(Self::Primitive(PrimitiveType::Float64)), - "int8" => Ok(Self::Primitive(PrimitiveType::Int8)), - "int16" => Ok(Self::Primitive(PrimitiveType::Int16)), - "int32" => Ok(Self::Primitive(PrimitiveType::Int32)), - "int64" => Ok(Self::Primitive(PrimitiveType::Int64)), - "uint8" => Ok(Self::Primitive(PrimitiveType::UInt8)), - "uint16" => Ok(Self::Primitive(PrimitiveType::UInt16)), - "uint32" => Ok(Self::Primitive(PrimitiveType::UInt32)), - "uint64" => Ok(Self::Primitive(PrimitiveType::UInt64)), - "string" => Ok(Self::String(None)), - s if s.starts_with("string") => Self::parse_bounded_string(s), // e.g. `string<=10` - s => ComplexType::parse(s).map(Type::Complex), - } - } - } - - fn parse_bounded_string(s: &str) -> anyhow::Result { - if s.starts_with("string<=") { - let len_str = &s["string<=".len()..s.len() - 1]; - let len = len_str - .parse::() - .with_context(|| "failed to parse bounded string length")?; - Ok(Self::String(Some(len))) - } else { - bail!("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 -/// Type -/// ``` -#[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) -> anyhow::Result { - if let Some((package, name)) = s.rsplit_once('/') { - if package.is_empty() || name.is_empty() { - bail!( - "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` format" - ); - } - Ok(Self::Absolute { - package: package.to_owned(), - name: name.to_owned(), - }) - } else { - if s.is_empty() { - bail!( - "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` or `SomeMessage` format" - ); - } - - Ok(Self::Relative { name: s.to_owned() }) - } - } -} - -/// Size specifier for array types. -#[derive(Debug, Clone, PartialEq, Eq)] -pub enum ArraySize { - Fixed(usize), - Bounded(usize), - Unbounded, -} - -impl ArraySize { - fn parse(array_part: &str) -> Result { - let array_part = array_part - .strip_suffix(']') - .with_context(|| "Missing closing ']' in array type")?; - - let array_size = if array_part.is_empty() { - Self::Unbounded - } else if let Ok(size) = array_part.parse::() { - Self::Fixed(size) - } else if array_part.ends_with('>') && array_part.starts_with('<') { - let size_str = &array_part[1..array_part.len() - 1]; - let size = size_str - .parse::() - .with_context(|| "Failed to parse bounded array size")?; - Self::Bounded(size) - } else { - bail!("invalid array size specifier: `{array_part}`"); - }; - Ok(array_size) - } -} - -/// A literal value, used for default values and constant definitions. -/// Can be a primitive, string, or array of literals. -#[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 { - match ty { - Type::Primitive(p) => match p { - PrimitiveType::Bool => { - let v = match s { - "true" => true, - "false" => false, - _ => bail!("invalid boolean literal: `{s}`"), - }; - Ok(Self::Bool(v)) - } - PrimitiveType::Byte - | PrimitiveType::Char - | PrimitiveType::Int8 - | PrimitiveType::Int16 - | PrimitiveType::Int32 - | PrimitiveType::Int64 => s - .parse::() - .map(Self::Int) - .with_context(|| "failed to parse integer literal"), - PrimitiveType::UInt8 - | PrimitiveType::UInt16 - | PrimitiveType::UInt32 - | PrimitiveType::UInt64 => s - .parse::() - .map(Self::UInt) - .with_context(|| "failed to parse unsigned integer literal"), - PrimitiveType::Float32 | PrimitiveType::Float64 => s - .parse::() - .map(Self::Float) - .with_context(|| "failed to parse float literal"), - }, - Type::String(_) => { - let s = s.trim_matches('"'); - Ok(Self::String(s.to_owned())) - } - Type::Array { - ty: elem_ty, - size: _, - } => { - let s = s.trim(); - - if !s.starts_with('[') || !s.ends_with(']') { - bail!("array literal must start with '[' and end with ']': `{s}`"); - } - let inner = &s[1..s.len() - 1]; - let elems_str = inner.split(',').map(|e| e.trim()).filter(|e| !e.is_empty()); - let mut elems = Vec::new(); - for elem_str in elems_str { - let elem = Self::parse(elem_str, elem_ty)?; - elems.push(elem); - } - - Ok(Self::Array(elems)) - } - Type::Complex(_) => bail!("cannot parse literal for named type"), - } - } -} - -/// 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 { - /// Determine if a line is a constant definition or a field definition. - /// - /// A constant definition has the following structure: ` =` - /// where `NAME` is all-caps with digits and underscores only, and `` is not an array. - /// - /// We look for the first `=` that is not inside quotes or brackets to make this determination. - fn is_constant_line(line: &str) -> bool { - let mut in_quote = false; - let mut bracket = 0usize; - for c in line.chars() { - match c { - '"' | '\'' => in_quote = !in_quote, - '[' => bracket += 1, - ']' => { - bracket = bracket.saturating_sub(1); - } - '=' if !in_quote && bracket == 0 => return true, - _ => {} - } - } - - false - } - - fn parse(line: &str) -> anyhow::Result { - let (type_and_name, value_str) = line - .split_once('=') - .with_context(|| "constant definition missing '='")?; - let (type_str, name) = type_and_name - .trim() - .rsplit_once(' ') - .with_context(|| "constant definition missing space between type and name")?; - - let ty = Type::parse(type_str)?; - let value = Literal::parse(value_str.trim(), &ty)?; - - if matches!(ty, Type::Array { .. }) { - bail!("constant type cannot be an array"); - } - if !name - .chars() - .all(|c| c.is_ascii_uppercase() || c.is_ascii_digit() || c == '_') - { - bail!("constant name must be all-caps alphanumeric and underscores only, got `{name}`"); - } - - Ok(Self { - ty, - name: name.to_owned(), - value, - }) - } -} - -fn strip_comment(s: &str) -> &str { - s.split_once('#').map(|(before, _)| before).unwrap_or(s) -} - -#[derive(Debug, Clone, PartialEq)] -pub struct MessageSchema { - pub name: String, - pub spec: MessageSpec, - pub dependencies: Vec, // Other message types referenced by this one. -} - -impl MessageSchema { - pub fn parse(name: String, input: &str) -> anyhow::Result { - let main_spec_content = extract_main_msg_spec(input); - let specs = extract_msg_specs(input); - - let main_spec = MessageSpec::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 = MessageSpec::parse(&dep_name, &dep_content) - .with_context(|| format!("failed to parse dependent message spec `{dep_name}`"))?; - dependencies.push(dep_spec); - } - - Ok(Self { - name, - spec: main_spec, - dependencies, - }) - } -} - -/// 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)) -} - -#[cfg(test)] -mod tests { - use crate::parsers::dds; - use cdr_encoding::CdrDeserializer; - - use super::*; - - #[test] - fn test_parse_message_spec() { - let input = r#" - # This is a comment - std_msgs/Header header - - int32 field1 - float64 field2 3.14 - string field3 "hello" - uint8[] field4 - - geometry_msgs/Point[] field5 - - uint32 CONST1=42 # inline comment - "#; - - MessageSpec::parse("test", input).unwrap(); - } - - #[test] - fn test_parse_message_schema() { - let input = r#" -# This message contains an uncompressed image -# (0, 0) is at top-left corner of image - -std_msgs/Header header # Header timestamp should be acquisition time of image - # Header frame_id should be optical frame of camera - # origin of frame should be optical center of cameara - # +x should point to the right in the image - # +y should point down in the image - # +z should point into to plane of the image - # If the frame_id here and the frame_id of the CameraInfo - # message associated with the image conflict - # the behavior is undefined - -uint32 height # image height, that is, number of rows -uint32 width # image width, that is, number of columns - -# The legal values for encoding are in file src/image_encodings.cpp -# If you want to standardize a new string format, join -# ros-users@lists.ros.org and send an email proposing a new encoding. - -string encoding # Encoding of pixels -- channel meaning, ordering, size - # taken from the list of strings in include/sensor_msgs/image_encodings.hpp - -uint8 is_bigendian # is this data bigendian? -uint32 step # Full row length in bytes -uint8[] data # actual matrix data, size is (step * rows) - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. - -# Two-integer timestamp that is expressed as seconds and nanoseconds. -builtin_interfaces/Time stamp - -# Transform frame with which this data is associated. -string frame_id - -================================================================================ -MSG: builtin_interfaces/Time -# This message communicates ROS Time defined here: -# https://design.ros2.org/articles/clock_and_time.html - -# The seconds component, valid over all int32 values. -int32 sec - -# The nanoseconds component, valid in the range [0, 10e9). -uint32 nanosec - - "#; - const RAW_MSG: &[u8] = include_bytes!("../../../../../../../last_image_msg.bin"); - - let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage".to_owned(), input).unwrap(); - let representation_identifier = - dds::RepresentationIdentifier::from_bytes(RAW_MSG[0..2].try_into().unwrap()).unwrap(); - - let payload = &RAW_MSG[4..]; - let mut de = CdrDeserializer::::new(payload); - - let mut resolver = std::collections::HashMap::new(); - for dep in &spec.dependencies { - resolver.insert(dep.name.clone(), dep); - } - } -} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs index 2e1743d88a4a..c87271f3da92 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs @@ -7,7 +7,7 @@ pub mod scalar_parser; pub mod sensor_msgs; pub mod std_msgs; -pub mod idl; +pub mod reflection; /// Trait for ROS2 message parsers that can be constructed with just a row count. pub trait Ros2MessageParser: MessageParser { diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs new file mode 100644 index 000000000000..75bdfc36845e --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs @@ -0,0 +1,65 @@ +use serde::de::{self, DeserializeSeed}; + +use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; + +use super::schema::SchemaSeed; +use super::{TypeResolver, Value}; + +// Whole message (struct) in field order. +pub(super) struct MessageSeed<'a, R: TypeResolver> { + spec: &'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 { + 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.spec.fields.len(), + MessageVisitor { + spec: self.spec, + 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 f in &self.spec.fields { + let v = seq + .next_element_seed(SchemaSeed::new(&f.ty, self.type_resolver))? + .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; + out.insert(f.name.clone(), v); + } + Ok(Value::Message(out)) + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs new file mode 100644 index 000000000000..a14bedb03c1f --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs @@ -0,0 +1,142 @@ +use std::collections::{BTreeMap, HashMap}; + +use anyhow::Context as _; +use cdr_encoding::CdrDeserializer; +use serde::de::DeserializeSeed as _; + +use crate::parsers::{ + dds, + ros2msg::reflection::{ + MessageSchema, + message_spec::{ComplexType, MessageSpecification}, + }, +}; + +pub mod message; +pub mod primitive; +pub mod primitive_array; +pub mod schema; +pub mod sequence; + +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 = message::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") + } +} + +#[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), + Array(Vec), // fixed-size [N] - for complex types + Seq(Vec), // variable-size [] or [<=N] - for complex types + PrimitiveArray(primitive_array::PrimitiveArray), // fixed-size [N] - for primitives + PrimitiveSeq(primitive_array::PrimitiveArray), // variable-size [] or [<=N] - for primitives + 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::Seq(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, ct: &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, ct: &ComplexType) -> Option<&MessageSpecification> { + match ct { + ComplexType::Absolute { package, name } => { + let full_name = format!("{package}/{name}"); + self.absolute.get(&full_name).copied() + } + ComplexType::Relative { name } => self.relative.get(name).copied(), + } + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs new file mode 100644 index 000000000000..6cf91d11e54d --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs @@ -0,0 +1,76 @@ +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_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs new file mode 100644 index 000000000000..50becbcbf6a6 --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs @@ -0,0 +1,124 @@ +use crate::parsers::ros2msg::reflection::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_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs new file mode 100644 index 000000000000..1f37a0671c20 --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs @@ -0,0 +1,120 @@ +use serde::de::{self, DeserializeSeed}; + +use crate::parsers::ros2msg::reflection::message_spec::Type; + +use super::message::MessageSeed; +use super::primitive::{PrimitiveVisitor, StringVisitor}; +use super::primitive_array::PrimitiveArraySeed; +use super::sequence::SequenceSeed; +use super::{TypeResolver, Value}; + +/// 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::parsers::ros2msg::reflection::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(p) => match p { + 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) => de.deserialize_string(StringVisitor).map(Value::String), + WString(_) => Err(de::Error::custom("wstring not supported")), + }, + Type::Array { ty, size } => match size { + Fixed(n) => { + // 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(*n), + } + .deserialize(de) + .map(Value::PrimitiveArray) + } else { + // CDR: fixed array has NO length prefix; `cdr_encoding` can be driven via tuple. + SequenceSeed::new(ty, Some(*n), 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::Seq) + } + } + }, + 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) + } + } + } +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs new file mode 100644 index 000000000000..00ce9fc55e8b --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs @@ -0,0 +1,87 @@ +use serde::de::{self, DeserializeSeed}; + +use crate::parsers::ros2msg::reflection::message_spec::Type; + +use super::{TypeResolver, Value, schema::SchemaSeed}; + +// 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, + } + } +} + +// ---- Sequence/array ---- +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(n) => de.deserialize_tuple( + n, + SequenceVisitor { + elem: self.elem, + fixed_len: Some(n), + 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 seq/array") + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); + let mut out = Vec::with_capacity(cap); + + if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { + for _ in 0..n { + 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_mcap/src/parsers/ros2msg/reflection/message_spec.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs new file mode 100644 index 000000000000..ddbfa699814f --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs @@ -0,0 +1,517 @@ +use crate::parsers::ros2msg::reflection::Ros2IdlError; + +/// 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; + } + + if Constant::is_constant_line(line) { + let constant = Constant::parse(line)?; + constants.push(constant); + } else { + let field = Field::parse(line)?; + 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 { + /// Determine if a line is a constant definition or a field definition. + /// + /// A constant definition has the following structure: ` =` + /// where `NAME` is all-caps with digits and underscores only, and `` is not an array. + /// + /// We look for the first `=` that is not inside quotes or brackets to make this determination. + fn is_constant_line(line: &str) -> bool { + let mut in_quote = false; + let mut bracket = 0usize; + for c in line.chars() { + match c { + '"' | '\'' => in_quote = !in_quote, + '[' => bracket += 1, + ']' => { + bracket = bracket.saturating_sub(1); + } + '=' if !in_quote && bracket == 0 => return true, + _ => {} + } + } + + false + } + + fn parse(line: &str) -> Result { + let (type_and_name, value_str) = line + .split_once('=') + .ok_or_else(|| Ros2IdlError::Parse("constant definition missing '='".to_owned()))?; + + let (type_str, name) = type_and_name.trim().rsplit_once(' ').ok_or_else(|| { + Ros2IdlError::Parse( + "constant definition missing space between type and name".to_owned(), + ) + })?; + + let ty = Type::parse(type_str)?; + let value = Literal::parse(value_str.trim(), &ty)?; + + if matches!(ty, Type::Array { .. }) { + return Err(Ros2IdlError::Parse( + "constant type cannot be an array".to_owned(), + )); + } + + if !Self::is_valid_constant_name(name) { + return Err(Ros2IdlError::Parse(format!( + "constant name must be all-caps alphanumeric and underscores only, got `{name}`" + ))); + } + + 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(line: &str) -> Result { + let line = line.trim(); + + // Parse first two whitespace-delimited tokens (type and name) with indices + fn next_token_bounds(s: &str, start: usize) -> Option<(usize, usize)> { + let bytes = s.as_bytes(); + let mut i = start; + while i < bytes.len() && bytes[i].is_ascii_whitespace() { + i += 1; + } + if i >= bytes.len() { + return None; + } + let start = i; + while i < bytes.len() && !bytes[i].is_ascii_whitespace() { + i += 1; + } + Some((start, i)) + } + + let (ty_start, ty_end) = next_token_bounds(line, 0).ok_or_else(|| { + Ros2IdlError::Parse(format!("field definition (`{line}`) missing type")) + })?; + let type_str = &line[ty_start..ty_end]; + + let (name_start, name_end) = next_token_bounds(line, ty_end).ok_or_else(|| { + Ros2IdlError::Parse(format!("field definition (`{line}`) missing name")) + })?; + let name = &line[name_start..name_end]; + + let rest = line[name_end..].trim(); + let optional_default = if rest.is_empty() { None } else { Some(rest) }; + + let ty = Type::parse(type_str).map_err(|_e| { + Ros2IdlError::Parse(format!( + "failed to parse type `{type_str}` in field definition `{line}`" + )) + })?; + + let default = if let Some(default_str) = optional_default { + Some(Literal::parse(default_str, &ty).map_err(|_e| { + Ros2IdlError::Parse(format!( + "failed to parse default value `{default_str}` for type `{ty:?}` in field definition `{line}`" + )) + })?) + } else { + None + }; + + 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))), + s if s.starts_with("string<=") => Self::parse_bounded_string(s), // e.g. `string<=10` + s if s.starts_with("wstring<=") => Err(Ros2IdlError::Parse( + "wstring types are not supported yet".to_owned(), + )), // TODO(gijsd): Support utf16 strings. + s => ComplexType::parse(s).map(Type::Complex), + } + } + } + + fn parse_bounded_string(s: &str) -> Result { + if let Some(len_str) = s.strip_prefix("string<=") { + let len = len_str.parse::().map_err(|e| { + Ros2IdlError::Parse(format!("failed to parse bounded string length: {e}")) + })?; + Ok(Self::BuiltIn(BuiltInType::String(Some(len)))) + } else { + Err(Ros2IdlError::Parse(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(Ros2IdlError::Parse(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(Ros2IdlError::Parse(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(|| Ros2IdlError::Parse("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(|e| { + Ros2IdlError::Parse(format!("Failed to parse bounded array size: {e}")) + })?; + Self::Bounded(size) + } else { + return Err(Ros2IdlError::Parse(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(Ros2IdlError::Parse(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(|e| { + Ros2IdlError::Parse(format!("failed to parse integer literal `{s}`: {e}")) + }) + } + // Byte is an unsigned 8-bit integer. + Byte | UInt8 | UInt16 | UInt32 | UInt64 => { + s.parse::().map(Self::UInt).map_err(|e| { + Ros2IdlError::Parse(format!( + "failed to parse unsigned integer literal `{s}`: {e}" + )) + }) + } + Float32 | Float64 => s.parse::().map(Self::Float).map_err(|e| { + Ros2IdlError::Parse(format!("failed to parse float literal `{s}`: {e}")) + }), + String(_) => { + let s = s.trim_matches('"'); + Ok(Self::String(s.to_owned())) + } + WString(_) => Err(Ros2IdlError::Parse( + "wstring literals are not supported yet".to_owned(), + )), // TODO(gijsd): Support utf16 strings. + }, + Type::Array { + ty: elem_ty, + size: _, + } => { + let s = s.trim(); + + if !s.starts_with('[') || !s.ends_with(']') { + Err(Ros2IdlError::Parse(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::, Ros2IdlError>>()?; + + Ok(Self::Array(elems)) + } + } + Type::Complex(_) => Err(Ros2IdlError::Parse( + "literals of complex types are not supported".to_owned(), + )), + } + } +} + +/// 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 +} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs new file mode 100644 index 000000000000..95d79a50416d --- /dev/null +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs @@ -0,0 +1,214 @@ +//! 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 thiserror::Error; + +use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; + +pub mod deserialize; +pub mod message_spec; + +#[derive(Error, Debug)] +pub enum Ros2IdlError { + #[error("ROS2 message spec parse error: {0}")] + Parse(String), + + #[error("{}", re_error::format(.0))] + Other(#[from] anyhow::Error), +} + +/// 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)) +} + +#[cfg(test)] +mod tests { + use crate::parsers::dds; + use cdr_encoding::CdrDeserializer; + + use super::*; + + #[test] + fn test_parse_message_spec() { + let input = r#" + # This is a comment + std_msgs/Header header + + int32 field1 + float64 field2 3.14 + string field3 "hello" + uint8[] field4 + + geometry_msgs/Point[] field5 + + uint32 CONST1=42 # inline comment + "#; + + MessageSpecification::parse("test", input).unwrap(); + } + + #[test] + fn test_parse_message_schema() { + let input = r#" +# This message contains an uncompressed image +# (0, 0) is at top-left corner of image + +std_msgs/Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of cameara + # +x should point to the right in the image + # +y should point down in the image + # +z should point into to plane of the image + # If the frame_id here and the frame_id of the CameraInfo + # message associated with the image conflict + # the behavior is undefined + +uint32 height # image height, that is, number of rows +uint32 width # image width, that is, number of columns + +# The legal values for encoding are in file src/image_encodings.cpp +# If you want to standardize a new string format, join +# ros-users@lists.ros.org and send an email proposing a new encoding. + +string encoding # Encoding of pixels -- channel meaning, ordering, size + # taken from the list of strings in include/sensor_msgs/image_encodings.hpp + +uint8 is_bigendian # is this data bigendian? +uint32 step # Full row length in bytes +uint8[] data # actual matrix data, size is (step * rows) + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. + +# Two-integer timestamp that is expressed as seconds and nanoseconds. +builtin_interfaces/Time stamp + +# Transform frame with which this data is associated. +string frame_id + +================================================================================ +MSG: builtin_interfaces/Time +# This message communicates ROS Time defined here: +# https://design.ros2.org/articles/clock_and_time.html + +# The seconds component, valid over all int32 values. +int32 sec + +# The nanoseconds component, valid in the range [0, 10e9). +uint32 nanosec + + "#; + const RAW_MSG: &[u8] = include_bytes!("../../../../../../../last_image_msg.bin"); + + let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage", input).unwrap(); + let representation_identifier = + dds::RepresentationIdentifier::from_bytes(RAW_MSG[0..2].try_into().unwrap()).unwrap(); + + let payload = &RAW_MSG[4..]; + let mut de = CdrDeserializer::::new(payload); + + let _resolver = deserialize::MapResolver::new( + spec.dependencies.iter().map(|dep| (dep.name.clone(), dep)), + ); + } +} From 1f5bb9837ff69deac8aad0222c52986de38fb1f6 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Thu, 25 Sep 2025 16:51:12 +0200 Subject: [PATCH 07/21] add more test cases --- .../ros2msg/reflection/message_spec.rs | 437 ++++++++++++++++-- .../src/parsers/ros2msg/reflection/mod.rs | 9 - 2 files changed, 395 insertions(+), 51 deletions(-) diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs index ddbfa699814f..11d613322048 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs @@ -1,4 +1,27 @@ -use crate::parsers::ros2msg::reflection::Ros2IdlError; +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), + + /// Deliberately unsupported features + #[error("unsupported: {0}")] + Unsupported(String), +} /// A parsed ROS 2 message specification, including fields and constants. /// @@ -25,7 +48,7 @@ pub struct MessageSpecification { } impl MessageSpecification { - pub(super) fn parse(name: &str, input: &str) -> Result { + pub(super) fn parse(name: &str, input: &str) -> Result { let mut fields = Vec::new(); let mut constants = Vec::new(); @@ -86,15 +109,23 @@ impl Constant { /// We look for the first `=` that is not inside quotes or brackets to make this determination. fn is_constant_line(line: &str) -> bool { let mut in_quote = false; - let mut bracket = 0usize; - for c in line.chars() { + let mut bracket_depth = 0usize; + let mut chars = line.chars().peekable(); + + while let Some(c) = chars.next() { match c { '"' | '\'' => in_quote = !in_quote, - '[' => bracket += 1, - ']' => { - bracket = bracket.saturating_sub(1); + '[' if !in_quote => bracket_depth += 1, + ']' if !in_quote => { + bracket_depth = bracket_depth.saturating_sub(1); + } + '<' if !in_quote && bracket_depth == 0 => { + // Check if this is part of a <= in a bounded string type + if chars.peek() == Some(&'=') { + chars.next(); // consume the '=' + } } - '=' if !in_quote && bracket == 0 => return true, + '=' if !in_quote && bracket_depth == 0 => return true, _ => {} } } @@ -102,28 +133,28 @@ impl Constant { false } - fn parse(line: &str) -> Result { + fn parse(line: &str) -> Result { let (type_and_name, value_str) = line .split_once('=') - .ok_or_else(|| Ros2IdlError::Parse("constant definition missing '='".to_owned()))?; + .ok_or_else(|| ParseError::Syntax("constant definition missing '='".to_owned()))?; let (type_str, name) = type_and_name.trim().rsplit_once(' ').ok_or_else(|| { - Ros2IdlError::Parse( - "constant definition missing space between type and name".to_owned(), - ) + ParseError::Syntax(format!( + "constant definition `{type_and_name}` missing space between type and name" + )) })?; let ty = Type::parse(type_str)?; let value = Literal::parse(value_str.trim(), &ty)?; if matches!(ty, Type::Array { .. }) { - return Err(Ros2IdlError::Parse( + return Err(ParseError::Type( "constant type cannot be an array".to_owned(), )); } if !Self::is_valid_constant_name(name) { - return Err(Ros2IdlError::Parse(format!( + return Err(ParseError::Validate(format!( "constant name must be all-caps alphanumeric and underscores only, got `{name}`" ))); } @@ -181,7 +212,7 @@ pub struct Field { } impl Field { - fn parse(line: &str) -> Result { + fn parse(line: &str) -> Result { let line = line.trim(); // Parse first two whitespace-delimited tokens (type and name) with indices @@ -202,12 +233,12 @@ impl Field { } let (ty_start, ty_end) = next_token_bounds(line, 0).ok_or_else(|| { - Ros2IdlError::Parse(format!("field definition (`{line}`) missing type")) + ParseError::Syntax(format!("field definition (`{line}`) missing type")) })?; let type_str = &line[ty_start..ty_end]; let (name_start, name_end) = next_token_bounds(line, ty_end).ok_or_else(|| { - Ros2IdlError::Parse(format!("field definition (`{line}`) missing name")) + ParseError::Syntax(format!("field definition (`{line}`) missing name")) })?; let name = &line[name_start..name_end]; @@ -215,14 +246,14 @@ impl Field { let optional_default = if rest.is_empty() { None } else { Some(rest) }; let ty = Type::parse(type_str).map_err(|_e| { - Ros2IdlError::Parse(format!( + ParseError::Type(format!( "failed to parse type `{type_str}` in field definition `{line}`" )) })?; let default = if let Some(default_str) = optional_default { Some(Literal::parse(default_str, &ty).map_err(|_e| { - Ros2IdlError::Parse(format!( + ParseError::Value(format!( "failed to parse default value `{default_str}` for type `{ty:?}` in field definition `{line}`" )) })?) @@ -269,7 +300,7 @@ pub enum Type { impl Type { /// Parse a type definition, e.g. `int32`, `string<=10`, `float32[3]`, `pkg/Type[]`, `pkg/Type[<=5]`, etc. - fn parse(s: &str) -> Result { + fn parse(s: &str) -> Result { let s = s.trim(); if let Some((base, array_part)) = s.split_once('[') { @@ -297,7 +328,7 @@ impl Type { "uint64" => Ok(Self::BuiltIn(BuiltInType::UInt64)), "string" => Ok(Self::BuiltIn(BuiltInType::String(None))), s if s.starts_with("string<=") => Self::parse_bounded_string(s), // e.g. `string<=10` - s if s.starts_with("wstring<=") => Err(Ros2IdlError::Parse( + s if s.starts_with("wstring<=") => Err(ParseError::Unsupported( "wstring types are not supported yet".to_owned(), )), // TODO(gijsd): Support utf16 strings. s => ComplexType::parse(s).map(Type::Complex), @@ -305,14 +336,14 @@ impl Type { } } - fn parse_bounded_string(s: &str) -> Result { + fn parse_bounded_string(s: &str) -> Result { if let Some(len_str) = s.strip_prefix("string<=") { let len = len_str.parse::().map_err(|e| { - Ros2IdlError::Parse(format!("failed to parse bounded string length: {e}")) + ParseError::Type(format!("failed to parse bounded string length: {e}")) })?; Ok(Self::BuiltIn(BuiltInType::String(Some(len)))) } else { - Err(Ros2IdlError::Parse(format!( + Err(ParseError::Type(format!( "invalid string type specifier: `{s}`" ))) } @@ -339,10 +370,10 @@ pub enum ComplexType { } impl ComplexType { - fn parse(s: &str) -> Result { + fn parse(s: &str) -> Result { if let Some((package, name)) = s.rsplit_once('/') { if package.is_empty() || name.is_empty() { - Err(Ros2IdlError::Parse(format!( + Err(ParseError::Type(format!( "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` format" ))) } else { @@ -352,7 +383,7 @@ impl ComplexType { }) } } else if s.is_empty() { - Err(Ros2IdlError::Parse(format!( + Err(ParseError::Type(format!( "invalid complex type specifier: `{s}`, expected `some_package/SomeMessage` or `SomeMessage` format" ))) } else { @@ -375,10 +406,10 @@ pub enum ArraySize { } impl ArraySize { - fn parse(array_part: &str) -> Result { + fn parse(array_part: &str) -> Result { let array_part = array_part .strip_suffix(']') - .ok_or_else(|| Ros2IdlError::Parse("Missing closing ']' in array type".to_owned()))?; + .ok_or_else(|| ParseError::Syntax("Missing closing ']' in array type".to_owned()))?; let array_size = if array_part.is_empty() { Self::Unbounded @@ -386,11 +417,11 @@ impl ArraySize { Self::Fixed(size) } else if let Some(n) = array_part.strip_prefix("<=") { let size = n.parse::().map_err(|e| { - Ros2IdlError::Parse(format!("Failed to parse bounded array size: {e}")) + ParseError::Value(format!("Failed to parse bounded array size: {e}")) })?; Self::Bounded(size) } else { - return Err(Ros2IdlError::Parse(format!( + return Err(ParseError::Value(format!( "invalid array size specifier: `{array_part}`" ))); }; @@ -412,7 +443,7 @@ pub enum Literal { } impl Literal { - fn parse(s: &str, ty: &Type) -> Result { + fn parse(s: &str, ty: &Type) -> Result { use BuiltInType::{ Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, String, UInt8, UInt16, UInt32, UInt64, WString, @@ -423,32 +454,40 @@ impl Literal { Bool => match s { "true" => Ok(Self::Bool(true)), "false" => Ok(Self::Bool(false)), - _ => Err(Ros2IdlError::Parse(format!( + _ => 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(|e| { - Ros2IdlError::Parse(format!("failed to parse integer literal `{s}`: {e}")) + ParseError::Value(format!("failed to parse integer literal `{s}`: {e}")) }) } // Byte is an unsigned 8-bit integer. Byte | UInt8 | UInt16 | UInt32 | UInt64 => { s.parse::().map(Self::UInt).map_err(|e| { - Ros2IdlError::Parse(format!( + ParseError::Value(format!( "failed to parse unsigned integer literal `{s}`: {e}" )) }) } Float32 | Float64 => s.parse::().map(Self::Float).map_err(|e| { - Ros2IdlError::Parse(format!("failed to parse float literal `{s}`: {e}")) + ParseError::Value(format!("failed to parse float literal `{s}`: {e}")) }), String(_) => { - let s = s.trim_matches('"'); + 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())) } - WString(_) => Err(Ros2IdlError::Parse( + WString(_) => Err(ParseError::Unsupported( "wstring literals are not supported yet".to_owned(), )), // TODO(gijsd): Support utf16 strings. }, @@ -459,7 +498,7 @@ impl Literal { let s = s.trim(); if !s.starts_with('[') || !s.ends_with(']') { - Err(Ros2IdlError::Parse(format!( + Err(ParseError::Value(format!( "array literal must start with '[' and end with ']': `{s}`" ))) } else { @@ -469,12 +508,12 @@ impl Literal { .map(|e| e.trim()) .filter(|e| !e.is_empty()) .map(|elem_str| Self::parse(elem_str, elem_ty)) - .collect::, Ros2IdlError>>()?; + .collect::, ParseError>>()?; Ok(Self::Array(elems)) } } - Type::Complex(_) => Err(Ros2IdlError::Parse( + Type::Complex(_) => Err(ParseError::Value( "literals of complex types are not supported".to_owned(), )), } @@ -515,3 +554,317 @@ fn strip_comment(s: &str) -> &str { } s } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn valid_constant() { + assert_eq!( + Constant::parse("int32 CONST_NAME=42").unwrap(), + Constant { + ty: Type::BuiltIn(BuiltInType::Int32), + name: "CONST_NAME".to_owned(), + value: Literal::Int(42) + } + ); + + assert_eq!( + Constant::parse("string 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("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("int32=42").is_err()); // missing name + assert!(Constant::parse("CONST_NAME=42").is_err()); // missing type + assert!(Constant::parse("int32 CONST_NAME").is_err()); // missing '=' and value + assert!(Constant::parse("int32 CONST_NAME=abc").is_err()); // invalid int value + assert!(Constant::parse("int32 CONST_NAME[]=42").is_err()); // array type not allowed + assert!(Constant::parse("int32 const_name=42").is_err()); // invalid name (not all-caps) + assert!(Constant::parse("int32 CONST__NAME=42").is_err()); // invalid name (consecutive underscores + assert!(Constant::parse("int32 _CONSTNAME=42").is_err()); // invalid name (doesn't start with letter) + assert!(Constant::parse("int32 CONSTNAME_=42").is_err()); // invalid name (ends with underscore) + } + + #[test] + fn valid_field() { + assert_eq!( + Field::parse("int32 field_name").unwrap(), + Field { + ty: Type::BuiltIn(BuiltInType::Int32), + name: "field_name".to_owned(), + default: None + } + ); + + assert_eq!( + Field::parse("string<=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("float64[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("geometry_msgs/Point[] 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("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("int32").is_err()); // missing name + assert!(Field::parse("field_name").is_err()); // missing type + assert!(Field::parse("int32 field_name extra token").is_err()); // extra token + assert!(Field::parse("string<=abc name").is_err()); // invalid bounded string + assert!(Field::parse("float64[abc] position").is_err()); // invalid array size + assert!(Field::parse("geometry_msgs/ Point[] points").is_err()); // invalid complex type + assert!(Field::parse("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/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs index 95d79a50416d..41c400630f58 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs @@ -13,15 +13,6 @@ use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; pub mod deserialize; pub mod message_spec; -#[derive(Error, Debug)] -pub enum Ros2IdlError { - #[error("ROS2 message spec parse error: {0}")] - Parse(String), - - #[error("{}", re_error::format(.0))] - Other(#[from] anyhow::Error), -} - /// 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) From 082a90961a18c6919bf1883e2ba2368ad3eb98da Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 29 Sep 2025 11:47:50 +0200 Subject: [PATCH 08/21] Deserialization + correct archetype --- crates/utils/re_mcap/src/layers/mod.rs | 2 +- crates/utils/re_mcap/src/layers/ros2.rs | 24 +- .../re_mcap/src/layers/ros2_reflection.rs | 245 +++++++--------- .../ros2msg/reflection/deserialize/message.rs | 65 ----- .../ros2msg/reflection/deserialize/mod.rs | 263 +++++++++++++++++- .../ros2msg/reflection/deserialize/schema.rs | 120 -------- .../reflection/deserialize/sequence.rs | 87 ------ .../ros2msg/reflection/message_spec.rs | 33 ++- .../src/parsers/ros2msg/reflection/mod.rs | 72 ----- 9 files changed, 378 insertions(+), 533 deletions(-) delete mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs delete mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs delete mode 100644 crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs diff --git a/crates/utils/re_mcap/src/layers/mod.rs b/crates/utils/re_mcap/src/layers/mod.rs index 8e824cc72c08..c550fc392d54 100644 --- a/crates/utils/re_mcap/src/layers/mod.rs +++ b/crates/utils/re_mcap/src/layers/mod.rs @@ -325,7 +325,7 @@ impl LayerRegistry { .register_file_layer::() .register_file_layer::() // message layers (priority order): - // .register_message_layer::() + .register_message_layer::() .register_message_layer::() .register_message_layer::(); diff --git a/crates/utils/re_mcap/src/layers/ros2.rs b/crates/utils/re_mcap/src/layers/ros2.rs index 246a651556ad..a44e07a6e4dc 100644 --- a/crates/utils/re_mcap/src/layers/ros2.rs +++ b/crates/utils/re_mcap/src/layers/ros2.rs @@ -6,7 +6,6 @@ use crate::parsers::{ ros2msg::{ Ros2MessageParser, rcl_interfaces::LogMessageParser, - reflection::MessageSchema, sensor_msgs::{ BatteryStateMessageParser, CameraInfoMessageParser, CompressedImageMessageParser, FluidPressureMessageParser, IlluminanceMessageParser, ImageMessageParser, @@ -93,28 +92,9 @@ impl MessageLayer for McapRos2Layer { } fn supports_channel(&self, channel: &mcap::Channel<'_>) -> bool { - if channel.schema.as_ref().is_some_and(|s| { + channel.schema.as_ref().is_some_and(|s| { s.encoding.as_str() == Self::ENCODING && self.registry.contains_key(&s.name) - }) { - true - } else { - let schema = channel.schema.as_ref().unwrap(); - let schema_content = String::from_utf8_lossy(schema.data.as_ref()); - if let Ok(message_spec) = MessageSchema::parse(&schema.name, &schema_content) { - re_log::warn_once!( - "Message schema {:?} is currently not supported, but parsed spec: {message_spec:#?}", - schema.name - ); - } else { - re_log::warn_once!( - "Message schema {:?} is currently not supported, and failed to parse spec", - schema.name - ); - re_log::warn_once!("Schema content:\n{}", schema_content); - } - - false - } + }) } fn message_parser( diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index 83bab5326051..f705736c6ae3 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -1,12 +1,14 @@ -use std::collections::HashSet; - use arrow::{ array::{ - ArrayBuilder, BinaryBuilder, BooleanBuilder, FixedSizeListBuilder, Float32Builder, - Float64Builder, Int8Builder, Int16Builder, Int32Builder, Int64Builder, ListBuilder, - StringBuilder, StructBuilder, UInt8Builder, UInt16Builder, UInt32Builder, UInt64Builder, + ArrayBuilder, ArrowPrimitiveType, BinaryBuilder, 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, }, - datatypes::{DataType, Field, Fields}, }; use re_chunk::{Chunk, ChunkId}; use re_types::{ComponentDescriptor, reflection::ComponentDescriptorExt as _}; @@ -27,21 +29,15 @@ struct Ros2ReflectionMessageParser { #[derive(Debug, thiserror::Error)] pub enum Ros2ReflectionError { - #[error("invalid message on channel {channel} for schema {schema}: {source}")] + #[error("Invalid message on channel {channel} for schema {schema}: {source}")] InvalidMessage { schema: String, channel: String, source: anyhow::Error, }, - #[error("expected type {expected_type}, but found value {value:?}")] - UnexpectedValue { - expected_type: &'static str, - value: String, - }, - - #[error("type {0} is not supported yet")] - UnsupportedType(&'static str), + #[error("Failed to downcast builder to expected type: {0}")] + Downcast(&'static str), } impl Ros2ReflectionMessageParser { @@ -105,6 +101,7 @@ impl MessageParser for Ros2ReflectionMessageParser { fields, } = *self; + let archetype_name = message_schema.spec.name.clone().replace('/', "."); let message_chunk = Chunk::from_auto_row_ids( ChunkId::new(), entity_path, @@ -114,7 +111,7 @@ impl MessageParser for Ros2ReflectionMessageParser { .map(|(field_name, mut builder)| { ( ComponentDescriptor::partial(field_name) - .with_builtin_archetype(message_schema.spec.name.clone()), + .with_builtin_archetype(archetype_name.clone()), builder.finish().into(), ) }) @@ -126,102 +123,63 @@ impl MessageParser for Ros2ReflectionMessageParser { } } -fn downcast_err<'a, T: std::any::Any>( - builder: &'a mut dyn ArrayBuilder, - _val: &Value, -) -> Result<&'a mut T, Ros2ReflectionError> { - let builder_type_name = std::any::type_name_of_val(builder).to_owned(); +fn downcast_err( + 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::UnexpectedValue { - expected_type: type_name.strip_suffix("Builder").unwrap_or(type_name), - value: builder_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_err::>>(builder)?; + let values_builder = downcast_err::>(list_builder.values())?; + values_builder.append_slice(vec); + list_builder.append(true); + Ok(()) +} + fn append_primitive_array( builder: &mut dyn ArrayBuilder, prim_array: &PrimitiveArray, - val: &Value, ) -> Result<(), Ros2ReflectionError> { match prim_array { PrimitiveArray::Bool(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::I8(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::U8(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::I16(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::U16(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::I32(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::U32(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::I64(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::U64(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::F32(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; - values_builder.append_slice(vec); - list_builder.append(true); - } - PrimitiveArray::F64(vec) => { - let list_builder = downcast_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; + // `Bool` is a special case since Arrow doesn't have a primitive boolean array + let list_builder = downcast_err::>>(builder)?; + let values_builder = downcast_err::(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_err::>>(builder, val)?; - let values_builder = downcast_err::(list_builder.values(), val)?; + let list_builder = downcast_err::>>(builder)?; + let values_builder = downcast_err::(list_builder.values())?; for item in items { values_builder.append_value(item); } list_builder.append(true); + Ok(()) } } - Ok(()) } fn append_value( @@ -230,76 +188,53 @@ fn append_value( schema: &MessageSchema, ) -> Result<(), Ros2ReflectionError> { match val { - Value::Bool(x) => downcast_err::(builder, val)?.append_value(*x), - Value::I8(x) => downcast_err::(builder, val)?.append_value(*x), - Value::U8(x) => downcast_err::(builder, val)?.append_value(*x), - Value::I16(x) => downcast_err::(builder, val)?.append_value(*x), - Value::U16(x) => downcast_err::(builder, val)?.append_value(*x), - Value::I32(x) => downcast_err::(builder, val)?.append_value(*x), - Value::U32(x) => downcast_err::(builder, val)?.append_value(*x), - Value::I64(x) => downcast_err::(builder, val)?.append_value(*x), - Value::U64(x) => downcast_err::(builder, val)?.append_value(*x), - Value::F32(x) => downcast_err::(builder, val)?.append_value(*x), - Value::F64(x) => downcast_err::(builder, val)?.append_value(*x), + Value::Bool(x) => downcast_err::(builder)?.append_value(*x), + Value::I8(x) => downcast_err::(builder)?.append_value(*x), + Value::U8(x) => downcast_err::(builder)?.append_value(*x), + Value::I16(x) => downcast_err::(builder)?.append_value(*x), + Value::U16(x) => downcast_err::(builder)?.append_value(*x), + Value::I32(x) => downcast_err::(builder)?.append_value(*x), + Value::U32(x) => downcast_err::(builder)?.append_value(*x), + Value::I64(x) => downcast_err::(builder)?.append_value(*x), + Value::U64(x) => downcast_err::(builder)?.append_value(*x), + Value::F32(x) => downcast_err::(builder)?.append_value(*x), + Value::F64(x) => downcast_err::(builder)?.append_value(*x), Value::String(x) => { - downcast_err::(builder, val)?.append_value(x.clone()); + downcast_err::(builder)?.append_value(x.clone()); } Value::Message(message_fields) => { - let struct_builder = downcast_err::(builder, val)?; + let struct_builder = downcast_err::(builder)?; - // For nested messages, we need to find the matching MessageSpec from dependencies + // 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 mut matching_spec: Option<&MessageSpecification> = None; - - // Try to find a MessageSpec that has the same field names as this message - for spec in &schema.dependencies { - let spec_field_names: HashSet<&String> = - spec.fields.iter().map(|f| &f.name).collect(); - let message_field_names: HashSet<&String> = message_fields.keys().collect(); - - if spec_field_names == message_field_names { - matching_spec = Some(spec); - break; - } - } + let matching_spec = find_matching_message_spec(schema, message_fields); if let Some(spec) = matching_spec { - // Use the spec field order to iterate through struct builder fields - for (ith_arrow_field, spec_field) in spec.fields.iter().enumerate() { - if let Some(field_builder) = - struct_builder.field_builders_mut().get_mut(ith_arrow_field) - { - let field_name = &spec_field.name; - - if let Some(field_value) = message_fields.get(field_name) { + // 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!( - "Could not find matching MessageSpec for nested message with fields: {:?}", + re_log::warn_once!( + "Could not find matching message specification for nested message with fields: {:?}", message_fields.keys().collect::>() ); - // Fallback: use the order from message_fields.keys() - not ideal but better than crashing - let message_field_names: Vec<&String> = message_fields.keys().collect(); - for (ith_arrow_field, field_builder) in - struct_builder.field_builders_mut().iter_mut().enumerate() - { - if let Some(&field_name) = message_field_names.get(ith_arrow_field) { - if let Some(field_value) = message_fields.get(field_name) { - append_value(field_builder, field_value, schema)?; - } - } - } } struct_builder.append(true); } Value::Array(vec) | Value::Seq(vec) => { - let list_builder = downcast_err::>>(builder, val)?; + let list_builder = downcast_err::>>(builder)?; for val in vec { append_value(list_builder.values(), val, schema)?; @@ -307,13 +242,26 @@ fn append_value( list_builder.append(true); } Value::PrimitiveArray(prim_array) | Value::PrimitiveSeq(prim_array) => { - append_primitive_array(builder, prim_array, val)?; + 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], @@ -425,9 +373,8 @@ fn resolve_complex_type<'a>( /// Provides reflection-based conversion of ROS2-encoded MCAP messages. /// -/// This layer uses the IDL deserializer to provide dynamic parsing of ROS2 messages -/// without requiring pre-compiled message definitions. It results in a direct Arrow -/// representation of the message fields, similar to the protobuf layer. +/// 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, @@ -476,7 +423,15 @@ impl MessageLayer for McapRos2ReflectionLayer { return false; } - self.schemas_per_topic.contains_key(&channel.topic) + // 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( diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs deleted file mode 100644 index 75bdfc36845e..000000000000 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/message.rs +++ /dev/null @@ -1,65 +0,0 @@ -use serde::de::{self, DeserializeSeed}; - -use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; - -use super::schema::SchemaSeed; -use super::{TypeResolver, Value}; - -// Whole message (struct) in field order. -pub(super) struct MessageSeed<'a, R: TypeResolver> { - spec: &'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 { - 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.spec.fields.len(), - MessageVisitor { - spec: self.spec, - 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 f in &self.spec.fields { - let v = seq - .next_element_seed(SchemaSeed::new(&f.ty, self.type_resolver))? - .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; - out.insert(f.name.clone(), v); - } - Ok(Value::Message(out)) - } -} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs index a14bedb03c1f..86df341aa7f9 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs @@ -2,21 +2,21 @@ use std::collections::{BTreeMap, HashMap}; use anyhow::Context as _; use cdr_encoding::CdrDeserializer; -use serde::de::DeserializeSeed as _; +use serde::de::{self, DeserializeSeed}; use crate::parsers::{ dds, ros2msg::reflection::{ MessageSchema, - message_spec::{ComplexType, MessageSpecification}, + deserialize::primitive_array::PrimitiveArraySeed, + message_spec::{ComplexType, MessageSpecification, Type}, }, }; -pub mod message; pub mod primitive; pub mod primitive_array; -pub mod schema; -pub mod sequence; + +use primitive::{PrimitiveVisitor, StringVisitor}; pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { // 4-byte encapsulation header @@ -29,7 +29,7 @@ pub fn decode_bytes(top: &MessageSchema, buf: &[u8]) -> anyhow::Result { let resolver = MapResolver::new(top.dependencies.iter().map(|dep| (dep.name.clone(), dep))); - let seed = message::MessageSeed::new(&top.spec, &resolver); + let seed = MessageSeed::new(&top.spec, &resolver); if representation_identifier.is_big_endian() { let mut de = CdrDeserializer::::new(&buf[4..]); @@ -140,3 +140,254 @@ impl TypeResolver for MapResolver<'_> { } } } + +// Whole message (struct) in field order. +pub(super) struct MessageSeed<'a, R: TypeResolver> { + spec: &'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 { + 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.spec.fields.len(), + MessageVisitor { + spec: self.spec, + 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 f in &self.spec.fields { + let v = seq + .next_element_seed(SchemaSeed::new(&f.ty, self.type_resolver))? + .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; + out.insert(f.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::parsers::ros2msg::reflection::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(p) => match p { + 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(n) => { + // 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(*n), + } + .deserialize(de) + .map(Value::PrimitiveArray) + } else { + SequenceSeed::new(ty, Some(*n), 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::Seq) + } + } + }, + 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(n) => de.deserialize_tuple( + n, + SequenceVisitor { + elem: self.elem, + fixed_len: Some(n), + 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 cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); + let mut out = Vec::with_capacity(cap); + + if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { + for _ in 0..n { + 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_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs deleted file mode 100644 index 1f37a0671c20..000000000000 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/schema.rs +++ /dev/null @@ -1,120 +0,0 @@ -use serde::de::{self, DeserializeSeed}; - -use crate::parsers::ros2msg::reflection::message_spec::Type; - -use super::message::MessageSeed; -use super::primitive::{PrimitiveVisitor, StringVisitor}; -use super::primitive_array::PrimitiveArraySeed; -use super::sequence::SequenceSeed; -use super::{TypeResolver, Value}; - -/// 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::parsers::ros2msg::reflection::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(p) => match p { - 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) => de.deserialize_string(StringVisitor).map(Value::String), - WString(_) => Err(de::Error::custom("wstring not supported")), - }, - Type::Array { ty, size } => match size { - Fixed(n) => { - // 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(*n), - } - .deserialize(de) - .map(Value::PrimitiveArray) - } else { - // CDR: fixed array has NO length prefix; `cdr_encoding` can be driven via tuple. - SequenceSeed::new(ty, Some(*n), 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::Seq) - } - } - }, - 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) - } - } - } -} diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs deleted file mode 100644 index 00ce9fc55e8b..000000000000 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/sequence.rs +++ /dev/null @@ -1,87 +0,0 @@ -use serde::de::{self, DeserializeSeed}; - -use crate::parsers::ros2msg::reflection::message_spec::Type; - -use super::{TypeResolver, Value, schema::SchemaSeed}; - -// 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, - } - } -} - -// ---- Sequence/array ---- -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(n) => de.deserialize_tuple( - n, - SequenceVisitor { - elem: self.elem, - fixed_len: Some(n), - 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 seq/array") - } - - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - let cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); - let mut out = Vec::with_capacity(cap); - - if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { - for _ in 0..n { - 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_mcap/src/parsers/ros2msg/reflection/message_spec.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs index 11d613322048..2d394ba1a5db 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs @@ -17,10 +17,6 @@ pub enum ParseError { /// Cross-entry checks: duplicates, bounds exceeded, defaults not allowed, naming rules #[error("validation error: {0}")] Validate(String), - - /// Deliberately unsupported features - #[error("unsupported: {0}")] - Unsupported(String), } /// A parsed ROS 2 message specification, including fields and constants. @@ -327,21 +323,31 @@ impl Type { "uint32" => Ok(Self::BuiltIn(BuiltInType::UInt32)), "uint64" => Ok(Self::BuiltIn(BuiltInType::UInt64)), "string" => Ok(Self::BuiltIn(BuiltInType::String(None))), - s if s.starts_with("string<=") => Self::parse_bounded_string(s), // e.g. `string<=10` - s if s.starts_with("wstring<=") => Err(ParseError::Unsupported( - "wstring types are not supported yet".to_owned(), - )), // TODO(gijsd): Support utf16 strings. + "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(s: &str) -> Result { - if let Some(len_str) = s.strip_prefix("string<=") { + 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(|e| { ParseError::Type(format!("failed to parse bounded string length: {e}")) })?; - Ok(Self::BuiltIn(BuiltInType::String(Some(len)))) + + Ok(len) } else { Err(ParseError::Type(format!( "invalid string type specifier: `{s}`" @@ -475,7 +481,7 @@ impl Literal { Float32 | Float64 => s.parse::().map(Self::Float).map_err(|e| { ParseError::Value(format!("failed to parse float literal `{s}`: {e}")) }), - String(_) => { + String(_) | WString(_) => { let s = if (s.starts_with('"') && s.ends_with('"')) || (s.starts_with('\'') && s.ends_with('\'')) { @@ -487,9 +493,6 @@ impl Literal { }; Ok(Self::String(s.to_owned())) } - WString(_) => Err(ParseError::Unsupported( - "wstring literals are not supported yet".to_owned(), - )), // TODO(gijsd): Support utf16 strings. }, Type::Array { ty: elem_ty, diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs index 41c400630f58..75d6b6ea2072 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs @@ -6,7 +6,6 @@ //! used to parse unknown types and still extract semantic meaning (types, //! arrays, names, constants, default values). use anyhow::Context as _; -use thiserror::Error; use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; @@ -111,9 +110,6 @@ fn parse_section(lines: &[&str]) -> Option<(String, String)> { #[cfg(test)] mod tests { - use crate::parsers::dds; - use cdr_encoding::CdrDeserializer; - use super::*; #[test] @@ -134,72 +130,4 @@ mod tests { MessageSpecification::parse("test", input).unwrap(); } - - #[test] - fn test_parse_message_schema() { - let input = r#" -# This message contains an uncompressed image -# (0, 0) is at top-left corner of image - -std_msgs/Header header # Header timestamp should be acquisition time of image - # Header frame_id should be optical frame of camera - # origin of frame should be optical center of cameara - # +x should point to the right in the image - # +y should point down in the image - # +z should point into to plane of the image - # If the frame_id here and the frame_id of the CameraInfo - # message associated with the image conflict - # the behavior is undefined - -uint32 height # image height, that is, number of rows -uint32 width # image width, that is, number of columns - -# The legal values for encoding are in file src/image_encodings.cpp -# If you want to standardize a new string format, join -# ros-users@lists.ros.org and send an email proposing a new encoding. - -string encoding # Encoding of pixels -- channel meaning, ordering, size - # taken from the list of strings in include/sensor_msgs/image_encodings.hpp - -uint8 is_bigendian # is this data bigendian? -uint32 step # Full row length in bytes -uint8[] data # actual matrix data, size is (step * rows) - -================================================================================ -MSG: std_msgs/Header -# Standard metadata for higher-level stamped data types. -# This is generally used to communicate timestamped data -# in a particular coordinate frame. - -# Two-integer timestamp that is expressed as seconds and nanoseconds. -builtin_interfaces/Time stamp - -# Transform frame with which this data is associated. -string frame_id - -================================================================================ -MSG: builtin_interfaces/Time -# This message communicates ROS Time defined here: -# https://design.ros2.org/articles/clock_and_time.html - -# The seconds component, valid over all int32 values. -int32 sec - -# The nanoseconds component, valid in the range [0, 10e9). -uint32 nanosec - - "#; - const RAW_MSG: &[u8] = include_bytes!("../../../../../../../last_image_msg.bin"); - - let spec = MessageSchema::parse("tf2_msgs/msg/TFMessage", input).unwrap(); - let representation_identifier = - dds::RepresentationIdentifier::from_bytes(RAW_MSG[0..2].try_into().unwrap()).unwrap(); - - let payload = &RAW_MSG[4..]; - let mut de = CdrDeserializer::::new(payload); - - let _resolver = deserialize::MapResolver::new( - spec.dependencies.iter().map(|dep| (dep.name.clone(), dep)), - ); - } } From 9a4ef3f7e0b05c4b627901b7fcff74bb5e0407fe Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 29 Sep 2025 12:16:54 +0200 Subject: [PATCH 09/21] fmt --- .../ros2msg/reflection/deserialize/mod.rs | 1 + .../reflection/deserialize/primitive.rs | 2 ++ .../ros2msg/reflection/message_spec.rs | 20 ++++++++-------- .../src/parsers/ros2msg/reflection/mod.rs | 24 ------------------- 4 files changed, 13 insertions(+), 34 deletions(-) diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs index 86df341aa7f9..62057bccdc8a 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs @@ -330,6 +330,7 @@ impl<'a, R: TypeResolver> SequenceSeed<'a, R> { impl<'de, R: TypeResolver> DeserializeSeed<'de> for SequenceSeed<'_, R> { type Value = Vec; + fn deserialize(self, de: D) -> Result where D: de::Deserializer<'de>, diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs index 6cf91d11e54d..29bf9a8be034 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs @@ -14,9 +14,11 @@ 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) } diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs index 2d394ba1a5db..c7109c8f580c 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs @@ -343,8 +343,8 @@ impl Type { 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(|e| { - ParseError::Type(format!("failed to parse bounded string length: {e}")) + let len = len_str.parse::().map_err(|err| { + ParseError::Type(format!("failed to parse bounded string length: {err}")) })?; Ok(len) @@ -422,8 +422,8 @@ impl ArraySize { } 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(|e| { - ParseError::Value(format!("Failed to parse bounded array size: {e}")) + let size = n.parse::().map_err(|err| { + ParseError::Value(format!("Failed to parse bounded array size: {err}")) })?; Self::Bounded(size) } else { @@ -466,20 +466,20 @@ impl Literal { }, // Char is a signed 8-bit integer representing an ASCII character. Char | Int8 | Int16 | Int32 | Int64 => { - s.parse::().map(Self::Int).map_err(|e| { - ParseError::Value(format!("failed to parse integer literal `{s}`: {e}")) + 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(|e| { + s.parse::().map(Self::UInt).map_err(|err| { ParseError::Value(format!( - "failed to parse unsigned integer literal `{s}`: {e}" + "failed to parse unsigned integer literal `{s}`: {err}" )) }) } - Float32 | Float64 => s.parse::().map(Self::Float).map_err(|e| { - ParseError::Value(format!("failed to parse float literal `{s}`: {e}")) + 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('"')) diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs index 75d6b6ea2072..bb5048b88dca 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs @@ -107,27 +107,3 @@ fn parse_section(lines: &[&str]) -> Option<(String, String)> { Some((name.to_owned(), body)) } - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_parse_message_spec() { - let input = r#" - # This is a comment - std_msgs/Header header - - int32 field1 - float64 field2 3.14 - string field3 "hello" - uint8[] field4 - - geometry_msgs/Point[] field5 - - uint32 CONST1=42 # inline comment - "#; - - MessageSpecification::parse("test", input).unwrap(); - } -} From 337521ffd29db340fe5007ba04ff78b41e8923ca Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 29 Sep 2025 12:25:02 +0200 Subject: [PATCH 10/21] remove re_error dependency --- Cargo.lock | 1 - crates/utils/re_mcap/Cargo.toml | 1 - 2 files changed, 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b5bb5ef87f0a..bde155f84eab 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -8986,7 +8986,6 @@ dependencies = [ "mcap", "prost-reflect", "re_chunk", - "re_error", "re_format_arrow", "re_log", "re_log_types", diff --git a/crates/utils/re_mcap/Cargo.toml b/crates/utils/re_mcap/Cargo.toml index 157bdcc25741..8426f5d84513 100644 --- a/crates/utils/re_mcap/Cargo.toml +++ b/crates/utils/re_mcap/Cargo.toml @@ -16,7 +16,6 @@ workspace = true [dependencies] re_chunk.workspace = true -re_error.workspace = true re_log.workspace = true re_log_types.workspace = true re_tracing.workspace = true From 65cab245601a8f7f177a68b7d193f0552cd533c7 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 29 Sep 2025 18:47:35 +0200 Subject: [PATCH 11/21] Fix empty fields --- Cargo.lock | 1 - .../re_mcap/src/layers/ros2_reflection.rs | 27 +++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/Cargo.lock b/Cargo.lock index bde155f84eab..fc186cd4fe35 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -8986,7 +8986,6 @@ dependencies = [ "mcap", "prost-reflect", "re_chunk", - "re_format_arrow", "re_log", "re_log_types", "re_tracing", diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index f705736c6ae3..2c1fb49d51c7 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -23,6 +23,7 @@ use crate::parsers::{MessageParser, ParserContext}; use crate::{Error, LayerIdentifier, MessageLayer}; struct Ros2ReflectionMessageParser { + num_rows: usize, message_schema: MessageSchema, fields: Vec<(String, FixedSizeListBuilder>)>, } @@ -55,6 +56,7 @@ impl Ros2ReflectionMessageParser { } Self { + num_rows, message_schema, fields, } @@ -97,11 +99,36 @@ impl MessageParser for Ros2ReflectionMessageParser { 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() { + // Create a list array with `num_rows` entries, where each entry is an empty list + let empty_list = arrow::array::ListArray::new_null( + std::sync::Arc::new(Field::new("empty", DataType::Null, 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.clone()), + empty_list, + )) + .collect(), + ) + .map_err(|err| Error::Other(anyhow::anyhow!(err)))?; + + return Ok(vec![chunk]); + } + let message_chunk = Chunk::from_auto_row_ids( ChunkId::new(), entity_path, From 329415bac370f590479f66f034cabf1ff3fb7225 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Fri, 3 Oct 2025 11:14:46 +0200 Subject: [PATCH 12/21] re_ros_msg --- ARCHITECTURE.md | 1 + Cargo.lock | 18 ++ Cargo.toml | 1 + crates/utils/re_mcap/Cargo.toml | 1 + .../re_mcap/src/layers/ros2_reflection.rs | 40 ++- .../utils/re_mcap/src/parsers/ros2msg/mod.rs | 2 - crates/utils/re_ros_msg/Cargo.toml | 32 +++ .../src}/deserialize/mod.rs | 109 +++----- .../src}/deserialize/primitive.rs | 0 .../src}/deserialize/primitive_array.rs | 2 +- .../mod.rs => re_ros_msg/src/lib.rs} | 2 +- .../src}/message_spec.rs | 260 ++++++++++-------- 12 files changed, 272 insertions(+), 196 deletions(-) create mode 100644 crates/utils/re_ros_msg/Cargo.toml rename crates/utils/{re_mcap/src/parsers/ros2msg/reflection => re_ros_msg/src}/deserialize/mod.rs (79%) rename crates/utils/{re_mcap/src/parsers/ros2msg/reflection => re_ros_msg/src}/deserialize/primitive.rs (100%) rename crates/utils/{re_mcap/src/parsers/ros2msg/reflection => re_ros_msg/src}/deserialize/primitive_array.rs (98%) rename crates/utils/{re_mcap/src/parsers/ros2msg/reflection/mod.rs => re_ros_msg/src/lib.rs} (97%) rename crates/utils/{re_mcap/src/parsers/ros2msg/reflection => re_ros_msg/src}/message_spec.rs (80%) 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..142496346901 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,23 @@ dependencies = [ "winit", ] +[[package]] +name = "re_ros_msg" +version = "0.26.0-alpha.1+dev" +dependencies = [ + "ahash", + "anyhow", + "byteorder", + "cdr-encoding", + "insta", + "re_format_arrow", + "re_log", + "re_tracing", + "serde", + "serde_bytes", + "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/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/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index 2c1fb49d51c7..0f3b2dd3c08f 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -1,3 +1,4 @@ +use anyhow::Context as _; use arrow::{ array::{ ArrayBuilder, ArrowPrimitiveType, BinaryBuilder, BooleanBuilder, FixedSizeListBuilder, @@ -10,18 +11,43 @@ use arrow::{ Int64Type, UInt8Type, UInt16Type, UInt32Type, UInt64Type, }, }; +use cdr_encoding::CdrDeserializer; use re_chunk::{Chunk, ChunkId}; -use re_types::{ComponentDescriptor, reflection::ComponentDescriptorExt as _}; - -use crate::parsers::ros2msg::reflection::{ +use re_ros_msg::{ MessageSchema, - deserialize::primitive_array::PrimitiveArray, - deserialize::{Value, decode_bytes}, + deserialize::{MapResolver, MessageSeed, Value, primitive_array::PrimitiveArray}, message_spec::{ArraySize, BuiltInType, ComplexType, MessageSpecification, Type}, }; -use crate::parsers::{MessageParser, ParserContext}; +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, @@ -260,7 +286,7 @@ fn append_value( struct_builder.append(true); } - Value::Array(vec) | Value::Seq(vec) => { + Value::Array(vec) | Value::Sequence(vec) => { let list_builder = downcast_err::>>(builder)?; for val in vec { diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs index c87271f3da92..d65e7243835e 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs +++ b/crates/utils/re_mcap/src/parsers/ros2msg/mod.rs @@ -7,8 +7,6 @@ pub mod scalar_parser; pub mod sensor_msgs; pub mod std_msgs; -pub mod reflection; - /// Trait for ROS2 message parsers that can be constructed with just a row count. pub trait Ros2MessageParser: MessageParser { /// Create a new parser instance. diff --git a/crates/utils/re_ros_msg/Cargo.toml b/crates/utils/re_ros_msg/Cargo.toml new file mode 100644 index 000000000000..7c2ec72d2a0b --- /dev/null +++ b/crates/utils/re_ros_msg/Cargo.toml @@ -0,0 +1,32 @@ +[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] +re_log.workspace = true +re_tracing.workspace = true + +ahash.workspace = true +anyhow.workspace = true +byteorder.workspace = true +cdr-encoding.workspace = true +serde.workspace = true +serde_bytes.workspace = true +thiserror.workspace = true + +[dev-dependencies] +re_format_arrow.workspace = true + +insta = { workspace = true, features = ["filters", "redactions"] } diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs b/crates/utils/re_ros_msg/src/deserialize/mod.rs similarity index 79% rename from crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs rename to crates/utils/re_ros_msg/src/deserialize/mod.rs index 62057bccdc8a..19ef44fdc508 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/mod.rs +++ b/crates/utils/re_ros_msg/src/deserialize/mod.rs @@ -1,16 +1,10 @@ use std::collections::{BTreeMap, HashMap}; -use anyhow::Context as _; -use cdr_encoding::CdrDeserializer; use serde::de::{self, DeserializeSeed}; -use crate::parsers::{ - dds, - ros2msg::reflection::{ - MessageSchema, - deserialize::primitive_array::PrimitiveArraySeed, - message_spec::{ComplexType, MessageSpecification, Type}, - }, +use crate::{ + deserialize::primitive_array::PrimitiveArraySeed, + message_spec::{ComplexType, MessageSpecification, Type}, }; pub mod primitive; @@ -18,30 +12,7 @@ pub mod primitive_array; use primitive::{PrimitiveVisitor, StringVisitor}; -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") - } -} - +/// A single deserialized value of any type that can appear in a ROS message. #[derive(Clone, PartialEq)] pub enum Value { Bool(bool), @@ -56,10 +27,20 @@ pub enum Value { F32(f32), F64(f64), String(String), - Array(Vec), // fixed-size [N] - for complex types - Seq(Vec), // variable-size [] or [<=N] - for complex types - PrimitiveArray(primitive_array::PrimitiveArray), // fixed-size [N] - for primitives - PrimitiveSeq(primitive_array::PrimitiveArray), // variable-size [] or [<=N] - for primitives + + /// 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), } @@ -79,7 +60,7 @@ impl std::fmt::Debug for Value { Self::F64(v) => write!(f, "F64({v})"), Self::String(v) => write!(f, "String({v:?})"), Self::Array(v) => write!(f, "Array({})", v.len()), - Self::Seq(v) => write!(f, "Seq({})", 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({{")?; @@ -97,7 +78,7 @@ impl std::fmt::Debug for Value { /// How we resolve a [`ComplexType`] at runtime. pub trait TypeResolver { - fn resolve(&self, ct: &ComplexType) -> Option<&MessageSpecification>; + fn resolve(&self, ty: &ComplexType) -> Option<&MessageSpecification>; } /// Efficient type resolver with separate maps for absolute and relative lookups. @@ -130,8 +111,8 @@ impl<'a> MapResolver<'a> { } impl TypeResolver for MapResolver<'_> { - fn resolve(&self, ct: &ComplexType) -> Option<&MessageSpecification> { - match ct { + 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() @@ -141,16 +122,16 @@ impl TypeResolver for MapResolver<'_> { } } -// Whole message (struct) in field order. -pub(super) struct MessageSeed<'a, R: TypeResolver> { - spec: &'a MessageSpecification, +/// 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 { - spec, + specification: spec, type_resolver, } } @@ -164,9 +145,9 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for MessageSeed<'_, R> { D: de::Deserializer<'de>, { de.deserialize_tuple( - self.spec.fields.len(), + self.specification.fields.len(), MessageVisitor { - spec: self.spec, + spec: self.specification, type_resolver: self.type_resolver, }, ) @@ -190,17 +171,17 @@ impl<'de, R: TypeResolver> serde::de::Visitor<'de> for MessageVisitor<'_, R> { A: serde::de::SeqAccess<'de>, { let mut out = std::collections::BTreeMap::new(); - for f in &self.spec.fields { + for field in &self.spec.fields { let v = seq - .next_element_seed(SchemaSeed::new(&f.ty, self.type_resolver))? + .next_element_seed(SchemaSeed::new(&field.ty, self.type_resolver))? .ok_or_else(|| serde::de::Error::custom("missing struct field"))?; - out.insert(f.name.clone(), v); + out.insert(field.name.clone(), v); } Ok(Value::Message(out)) } } -/// One value, driven by a Type + resolver. +/// One value, driven by a [`Type`] + resolver. pub(super) struct SchemaSeed<'a, R: TypeResolver> { ty: &'a Type, resolver: &'a R, @@ -219,7 +200,7 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { where D: de::Deserializer<'de>, { - use crate::parsers::ros2msg::reflection::message_spec::{ + use crate::message_spec::{ ArraySize::{Bounded, Fixed, Unbounded}, BuiltInType::{ Bool, Byte, Char, Float32, Float64, Int8, Int16, Int32, Int64, String, UInt8, @@ -229,7 +210,7 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { }; match self.ty { - Type::BuiltIn(p) => match p { + Type::BuiltIn(primitive_type) => match primitive_type { Bool => de .deserialize_bool(PrimitiveVisitor::::new()) .map(Value::Bool), @@ -268,17 +249,17 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { } }, Type::Array { ty, size } => match size { - Fixed(n) => { + 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(*n), + fixed_len: Some(*len), } .deserialize(de) .map(Value::PrimitiveArray) } else { - SequenceSeed::new(ty, Some(*n), self.resolver) + SequenceSeed::new(ty, Some(*len), self.resolver) .deserialize(de) .map(Value::Array) } @@ -296,7 +277,7 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SchemaSeed<'_, R> { // CDR: length-prefixed sequence; serde side is a seq. SequenceSeed::new(ty, None, self.resolver) .deserialize(de) - .map(Value::Seq) + .map(Value::Sequence) } } }, @@ -336,11 +317,11 @@ impl<'de, R: TypeResolver> DeserializeSeed<'de> for SequenceSeed<'_, R> { D: de::Deserializer<'de>, { match self.fixed_len { - Some(n) => de.deserialize_tuple( - n, + Some(len) => de.deserialize_tuple( + len, SequenceVisitor { elem: self.elem, - fixed_len: Some(n), + fixed_len: Some(len), type_resolver: self.resolver, }, ), @@ -370,11 +351,11 @@ impl<'de, R: TypeResolver> serde::de::Visitor<'de> for SequenceVisitor<'_, R> { where A: serde::de::SeqAccess<'de>, { - let cap = self.fixed_len.or_else(|| seq.size_hint()).unwrap_or(0); - let mut out = Vec::with_capacity(cap); + let len = self.fixed_len.or_else(|| seq.size_hint()); + let mut out = Vec::with_capacity(len.unwrap_or(0)); - if let Some(n) = self.fixed_len.or_else(|| seq.size_hint()) { - for _ in 0..n { + 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"))?; diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs b/crates/utils/re_ros_msg/src/deserialize/primitive.rs similarity index 100% rename from crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive.rs rename to crates/utils/re_ros_msg/src/deserialize/primitive.rs diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs b/crates/utils/re_ros_msg/src/deserialize/primitive_array.rs similarity index 98% rename from crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs rename to crates/utils/re_ros_msg/src/deserialize/primitive_array.rs index 50becbcbf6a6..5ece14e4a045 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/deserialize/primitive_array.rs +++ b/crates/utils/re_ros_msg/src/deserialize/primitive_array.rs @@ -1,4 +1,4 @@ -use crate::parsers::ros2msg::reflection::message_spec::BuiltInType; +use crate::message_spec::BuiltInType; use serde::de::{self, DeserializeSeed, Visitor}; use std::fmt; diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs b/crates/utils/re_ros_msg/src/lib.rs similarity index 97% rename from crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs rename to crates/utils/re_ros_msg/src/lib.rs index bb5048b88dca..892c2a7ea116 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/mod.rs +++ b/crates/utils/re_ros_msg/src/lib.rs @@ -7,7 +7,7 @@ //! arrays, names, constants, default values). use anyhow::Context as _; -use crate::parsers::ros2msg::reflection::message_spec::MessageSpecification; +use crate::message_spec::MessageSpecification; pub mod deserialize; pub mod message_spec; diff --git a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs b/crates/utils/re_ros_msg/src/message_spec.rs similarity index 80% rename from crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs rename to crates/utils/re_ros_msg/src/message_spec.rs index c7109c8f580c..ba4b23bd42b4 100644 --- a/crates/utils/re_mcap/src/parsers/ros2msg/reflection/message_spec.rs +++ b/crates/utils/re_ros_msg/src/message_spec.rs @@ -58,11 +58,24 @@ impl MessageSpecification { continue; } - if Constant::is_constant_line(line) { - let constant = Constant::parse(line)?; + // 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(line)?; + let field = Field::parse(ty, name, rest)?; fields.push(field); } } @@ -97,64 +110,33 @@ pub struct Constant { } impl Constant { - /// Determine if a line is a constant definition or a field definition. - /// - /// A constant definition has the following structure: ` =` - /// where `NAME` is all-caps with digits and underscores only, and `` is not an array. - /// - /// We look for the first `=` that is not inside quotes or brackets to make this determination. - fn is_constant_line(line: &str) -> bool { - let mut in_quote = false; - let mut bracket_depth = 0usize; - let mut chars = line.chars().peekable(); - - while let Some(c) = chars.next() { - match c { - '"' | '\'' => in_quote = !in_quote, - '[' if !in_quote => bracket_depth += 1, - ']' if !in_quote => { - bracket_depth = bracket_depth.saturating_sub(1); - } - '<' if !in_quote && bracket_depth == 0 => { - // Check if this is part of a <= in a bounded string type - if chars.peek() == Some(&'=') { - chars.next(); // consume the '=' - } - } - '=' if !in_quote && bracket_depth == 0 => return true, - _ => {} - } + 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}`" + ))); } - false - } - - fn parse(line: &str) -> Result { - let (type_and_name, value_str) = line - .split_once('=') - .ok_or_else(|| ParseError::Syntax("constant definition missing '='".to_owned()))?; - - let (type_str, name) = type_and_name.trim().rsplit_once(' ').ok_or_else(|| { - ParseError::Syntax(format!( - "constant definition `{type_and_name}` missing space between type and name" - )) - })?; - - let ty = Type::parse(type_str)?; - let value = Literal::parse(value_str.trim(), &ty)?; - if matches!(ty, Type::Array { .. }) { return Err(ParseError::Type( "constant type cannot be an array".to_owned(), )); } - 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::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(), @@ -208,53 +190,11 @@ pub struct Field { } impl Field { - fn parse(line: &str) -> Result { - let line = line.trim(); - - // Parse first two whitespace-delimited tokens (type and name) with indices - fn next_token_bounds(s: &str, start: usize) -> Option<(usize, usize)> { - let bytes = s.as_bytes(); - let mut i = start; - while i < bytes.len() && bytes[i].is_ascii_whitespace() { - i += 1; - } - if i >= bytes.len() { - return None; - } - let start = i; - while i < bytes.len() && !bytes[i].is_ascii_whitespace() { - i += 1; - } - Some((start, i)) - } - - let (ty_start, ty_end) = next_token_bounds(line, 0).ok_or_else(|| { - ParseError::Syntax(format!("field definition (`{line}`) missing type")) - })?; - let type_str = &line[ty_start..ty_end]; - - let (name_start, name_end) = next_token_bounds(line, ty_end).ok_or_else(|| { - ParseError::Syntax(format!("field definition (`{line}`) missing name")) - })?; - let name = &line[name_start..name_end]; - - let rest = line[name_end..].trim(); - let optional_default = if rest.is_empty() { None } else { Some(rest) }; - - let ty = Type::parse(type_str).map_err(|_e| { - ParseError::Type(format!( - "failed to parse type `{type_str}` in field definition `{line}`" - )) - })?; - - let default = if let Some(default_str) = optional_default { - Some(Literal::parse(default_str, &ty).map_err(|_e| { - ParseError::Value(format!( - "failed to parse default value `{default_str}` for type `{ty:?}` in field definition `{line}`" - )) - })?) - } else { + fn parse(ty: Type, name: &str, rest: &str) -> Result { + let default = if rest.is_empty() { None + } else { + Some(Literal::parse(rest, &ty)?) }; Ok(Self { @@ -523,6 +463,53 @@ impl Literal { } } +/// 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; @@ -565,7 +552,7 @@ mod tests { #[test] fn valid_constant() { assert_eq!( - Constant::parse("int32 CONST_NAME=42").unwrap(), + Constant::parse(Type::BuiltIn(BuiltInType::Int32), "CONST_NAME", "=42").unwrap(), Constant { ty: Type::BuiltIn(BuiltInType::Int32), name: "CONST_NAME".to_owned(), @@ -574,7 +561,12 @@ mod tests { ); assert_eq!( - Constant::parse("string CONST_STR=\"hello\"").unwrap(), + Constant::parse( + Type::BuiltIn(BuiltInType::String(None)), + "CONST_STR", + "=\"hello\"" + ) + .unwrap(), Constant { ty: Type::BuiltIn(BuiltInType::String(None)), name: "CONST_STR".to_owned(), @@ -583,7 +575,7 @@ mod tests { ); assert_eq!( - Constant::parse("float64 CONST_FLOAT=3.1").unwrap(), + Constant::parse(Type::BuiltIn(BuiltInType::Float64), "CONST_FLOAT", "=3.1").unwrap(), Constant { ty: Type::BuiltIn(BuiltInType::Float64), name: "CONST_FLOAT".to_owned(), @@ -594,21 +586,29 @@ mod tests { #[test] fn invalid_constant() { - assert!(Constant::parse("int32=42").is_err()); // missing name - assert!(Constant::parse("CONST_NAME=42").is_err()); // missing type - assert!(Constant::parse("int32 CONST_NAME").is_err()); // missing '=' and value - assert!(Constant::parse("int32 CONST_NAME=abc").is_err()); // invalid int value - assert!(Constant::parse("int32 CONST_NAME[]=42").is_err()); // array type not allowed - assert!(Constant::parse("int32 const_name=42").is_err()); // invalid name (not all-caps) - assert!(Constant::parse("int32 CONST__NAME=42").is_err()); // invalid name (consecutive underscores - assert!(Constant::parse("int32 _CONSTNAME=42").is_err()); // invalid name (doesn't start with letter) - assert!(Constant::parse("int32 CONSTNAME_=42").is_err()); // invalid name (ends with underscore) + 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("int32 field_name").unwrap(), + Field::parse(Type::BuiltIn(BuiltInType::Int32), "field_name", "").unwrap(), Field { ty: Type::BuiltIn(BuiltInType::Int32), name: "field_name".to_owned(), @@ -617,7 +617,12 @@ mod tests { ); assert_eq!( - Field::parse("string<=10 name \"default\"").unwrap(), + Field::parse( + Type::BuiltIn(BuiltInType::String(Some(10))), + "name", + "\"default\"" + ) + .unwrap(), Field { name: "name".to_owned(), ty: Type::BuiltIn(BuiltInType::String(Some(10))), @@ -626,7 +631,15 @@ mod tests { ); assert_eq!( - Field::parse("float64[3] position [0.0, 1.0, 2.0]").unwrap(), + 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 { @@ -642,7 +655,18 @@ mod tests { ); assert_eq!( - Field::parse("geometry_msgs/Point[] points").unwrap(), + 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 { @@ -657,7 +681,7 @@ mod tests { ); assert_eq!( - Field::parse("bool enabled true").unwrap(), + Field::parse(Type::BuiltIn(BuiltInType::Bool), "enabled", "true").unwrap(), Field { ty: Type::BuiltIn(BuiltInType::Bool), name: "enabled".to_owned(), @@ -668,13 +692,7 @@ mod tests { #[test] fn invalid_field() { - assert!(Field::parse("int32").is_err()); // missing name - assert!(Field::parse("field_name").is_err()); // missing type - assert!(Field::parse("int32 field_name extra token").is_err()); // extra token - assert!(Field::parse("string<=abc name").is_err()); // invalid bounded string - assert!(Field::parse("float64[abc] position").is_err()); // invalid array size - assert!(Field::parse("geometry_msgs/ Point[] points").is_err()); // invalid complex type - assert!(Field::parse("bool enabled maybe").is_err()); // invalid bool literal + assert!(Field::parse(Type::BuiltIn(BuiltInType::Bool), "enabled", "maybe").is_err()); // invalid bool literal } #[test] From 4ef2fc6dd2d5337289d1e2d63d28ad9c7c4dc665 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Fri, 3 Oct 2025 13:55:34 +0200 Subject: [PATCH 13/21] Remove unused dependencies --- Cargo.lock | 8 -------- crates/utils/re_ros_msg/Cargo.toml | 12 ------------ 2 files changed, 20 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 142496346901..2692629f0c49 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -9305,16 +9305,8 @@ dependencies = [ name = "re_ros_msg" version = "0.26.0-alpha.1+dev" dependencies = [ - "ahash", "anyhow", - "byteorder", - "cdr-encoding", - "insta", - "re_format_arrow", - "re_log", - "re_tracing", "serde", - "serde_bytes", "thiserror 1.0.69", ] diff --git a/crates/utils/re_ros_msg/Cargo.toml b/crates/utils/re_ros_msg/Cargo.toml index 7c2ec72d2a0b..5655ed95ceda 100644 --- a/crates/utils/re_ros_msg/Cargo.toml +++ b/crates/utils/re_ros_msg/Cargo.toml @@ -15,18 +15,6 @@ version.workspace = true workspace = true [dependencies] -re_log.workspace = true -re_tracing.workspace = true - -ahash.workspace = true anyhow.workspace = true -byteorder.workspace = true -cdr-encoding.workspace = true serde.workspace = true -serde_bytes.workspace = true thiserror.workspace = true - -[dev-dependencies] -re_format_arrow.workspace = true - -insta = { workspace = true, features = ["filters", "redactions"] } From e7fc5b4f9b117bbb57d304e70f68c8b76b0ef19d Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 11:00:48 +0200 Subject: [PATCH 14/21] update snapshot --- .../tests/snapshots/test_mcap_loader__tests__ros2.snap | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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..cfa374b048bf 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:6e27f8b4edf093402405d748659d04bf753a00bdd339f3ab0d1768edd0ad8952 +size 472210 From de4791ab5463c30c631c930ac372a46b78fc0cae Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 11:21:53 +0200 Subject: [PATCH 15/21] remove fallback --- .../re_mcap/src/layers/ros2_reflection.rs | 199 ++++++++++-------- 1 file changed, 111 insertions(+), 88 deletions(-) diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index 0f3b2dd3c08f..28a90c74f323 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -1,10 +1,12 @@ +use std::sync::Arc; + use anyhow::Context as _; use arrow::{ array::{ - ArrayBuilder, ArrowPrimitiveType, BinaryBuilder, BooleanBuilder, FixedSizeListBuilder, - Float32Builder, Float64Builder, Int8Builder, Int16Builder, Int32Builder, Int64Builder, - ListBuilder, PrimitiveBuilder, StringBuilder, StructBuilder, UInt8Builder, UInt16Builder, - UInt32Builder, UInt64Builder, + 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, @@ -12,7 +14,9 @@ use arrow::{ }, }; use cdr_encoding::CdrDeserializer; -use re_chunk::{Chunk, ChunkId}; +use re_chunk::{ + Chunk, ChunkId, EntityPath, TimeColumn, TimelineName, external::nohash_hasher::IntMap, +}; use re_ros_msg::{ MessageSchema, deserialize::{MapResolver, MessageSeed, Value, primitive_array::PrimitiveArray}, @@ -68,24 +72,24 @@ pub enum Ros2ReflectionError { } impl Ros2ReflectionMessageParser { - fn new(num_rows: usize, message_schema: MessageSchema) -> Self { + 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); + let builder = arrow_builder_from_type(&field.ty, &message_schema.dependencies)?; fields.push(( name.clone(), FixedSizeListBuilder::with_capacity(builder, 1, num_rows), )); } - Self { + Ok(Self { num_rows, message_schema, fields, - } + }) } } @@ -133,26 +137,8 @@ impl MessageParser for Ros2ReflectionMessageParser { let archetype_name = message_schema.spec.name.clone().replace('/', "."); if fields.is_empty() { - // Create a list array with `num_rows` entries, where each entry is an empty list - let empty_list = arrow::array::ListArray::new_null( - std::sync::Arc::new(Field::new("empty", DataType::Null, 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.clone()), - empty_list, - )) - .collect(), - ) - .map_err(|err| Error::Other(anyhow::anyhow!(err)))?; - - return Ok(vec![chunk]); + return create_empty_message_chunk(entity_path, timelines, num_rows, &archetype_name) + .map(|chunk| vec![chunk]); } let message_chunk = Chunk::from_auto_row_ids( @@ -176,7 +162,36 @@ impl MessageParser for Ros2ReflectionMessageParser { } } -fn downcast_err( +fn create_empty_message_chunk( + entity_path: EntityPath, + timelines: IntMap, + num_rows: usize, + archetype_name: &str, +) -> 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(|| { @@ -193,8 +208,8 @@ where T: ArrowPrimitiveType, PrimitiveBuilder: 'static, { - let list_builder = downcast_err::>>(builder)?; - let values_builder = downcast_err::>(list_builder.values())?; + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::>(list_builder.values())?; values_builder.append_slice(vec); list_builder.append(true); Ok(()) @@ -207,8 +222,8 @@ fn append_primitive_array( match prim_array { PrimitiveArray::Bool(vec) => { // `Bool` is a special case since Arrow doesn't have a primitive boolean array - let list_builder = downcast_err::>>(builder)?; - let values_builder = downcast_err::(list_builder.values())?; + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::(list_builder.values())?; values_builder.append_slice(vec); list_builder.append(true); Ok(()) @@ -224,8 +239,8 @@ fn append_primitive_array( 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_err::>>(builder)?; - let values_builder = downcast_err::(list_builder.values())?; + let list_builder = downcast_builder::>>(builder)?; + let values_builder = downcast_builder::(list_builder.values())?; for item in items { values_builder.append_value(item); } @@ -241,22 +256,22 @@ fn append_value( schema: &MessageSchema, ) -> Result<(), Ros2ReflectionError> { match val { - Value::Bool(x) => downcast_err::(builder)?.append_value(*x), - Value::I8(x) => downcast_err::(builder)?.append_value(*x), - Value::U8(x) => downcast_err::(builder)?.append_value(*x), - Value::I16(x) => downcast_err::(builder)?.append_value(*x), - Value::U16(x) => downcast_err::(builder)?.append_value(*x), - Value::I32(x) => downcast_err::(builder)?.append_value(*x), - Value::U32(x) => downcast_err::(builder)?.append_value(*x), - Value::I64(x) => downcast_err::(builder)?.append_value(*x), - Value::U64(x) => downcast_err::(builder)?.append_value(*x), - Value::F32(x) => downcast_err::(builder)?.append_value(*x), - Value::F64(x) => downcast_err::(builder)?.append_value(*x), + 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_err::(builder)?.append_value(x.clone()); + downcast_builder::(builder)?.append_value(x.clone()); } Value::Message(message_fields) => { - let struct_builder = downcast_err::(builder)?; + 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 @@ -287,7 +302,7 @@ fn append_value( struct_builder.append(true); } Value::Array(vec) | Value::Sequence(vec) => { - let list_builder = downcast_err::>>(builder)?; + let list_builder = downcast_builder::>>(builder)?; for val in vec { append_value(list_builder.values(), val, schema)?; @@ -318,29 +333,29 @@ fn find_matching_message_spec<'a>( fn struct_builder_from_message_spec( spec: &MessageSpecification, dependencies: &[MessageSpecification], -) -> StructBuilder { +) -> anyhow::Result { let fields = spec .fields .iter() .map(|f| { - ( - arrow_field_from_type(&f.ty, &f.name, dependencies), - arrow_builder_from_type(&f.ty, dependencies), - ) + Ok(( + arrow_field_from_type(&f.ty, &f.name, dependencies)?, + arrow_builder_from_type(&f.ty, dependencies)?, + )) }) - .collect::>(); + .collect::>>()?; let (fields, field_builders): (Vec, Vec>) = fields.into_iter().unzip(); - StructBuilder::new(fields, field_builders) + Ok(StructBuilder::new(fields, field_builders)) } fn arrow_builder_from_type( ty: &Type, dependencies: &[MessageSpecification], -) -> Box { - match ty { +) -> anyhow::Result> { + Ok(match ty { Type::BuiltIn(p) => match p { BuiltInType::Bool => Box::new(BooleanBuilder::new()), BuiltInType::Byte | BuiltInType::UInt8 => Box::new(UInt8Builder::new()), @@ -357,25 +372,30 @@ fn arrow_builder_from_type( }, Type::Complex(complex_type) => { // Look up the message spec in dependencies - if let Some(spec) = resolve_complex_type(complex_type, dependencies) { - Box::new(struct_builder_from_message_spec(spec, dependencies)) - } else { - re_log::warn_once!("Could not resolve complex type: {:?}", complex_type); - Box::new(BinaryBuilder::new()) // Fallback to binary - } + 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))) + Box::new(ListBuilder::new(arrow_builder_from_type(ty, dependencies)?)) } - } + }) } -fn arrow_field_from_type(ty: &Type, name: &str, dependencies: &[MessageSpecification]) -> Field { - Field::new(name, datatype_from_type(ty, dependencies), true) +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]) -> DataType { - match ty { +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, @@ -391,23 +411,22 @@ fn datatype_from_type(ty: &Type, dependencies: &[MessageSpecification]) -> DataT BuiltInType::String(_) | BuiltInType::WString(_) => DataType::Utf8, // No wstring in Arrow }, Type::Complex(complex_type) => { - if let Some(spec) = resolve_complex_type(complex_type, dependencies) { - let fields = spec - .fields - .iter() - .map(|f| arrow_field_from_type(&f.ty, &f.name, dependencies)) - .collect::(); - DataType::Struct(fields) - } else { - DataType::Binary // Fallback - } + 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) + DataType::new_list(datatype_from_type(ty, dependencies)?, true) } }, - } + }) } fn resolve_complex_type<'a>( @@ -461,7 +480,12 @@ impl MessageLayer for McapRos2ReflectionLayer { let found = self .schemas_per_topic .insert(channel.topic.clone(), message_schema); - debug_assert!(found.is_none()); + + debug_assert!( + found.is_none(), + "Duplicate schema for topic {}", + channel.topic + ); } Ok(()) @@ -493,9 +517,8 @@ impl MessageLayer for McapRos2ReflectionLayer { num_rows: usize, ) -> Option> { let message_schema = self.schemas_per_topic.get(&channel.topic)?; - Some(Box::new(Ros2ReflectionMessageParser::new( - num_rows, - message_schema.clone(), - ))) + Some(Box::new( + Ros2ReflectionMessageParser::new(num_rows, message_schema.clone()).ok()?, + )) } } From a9034bb2ee3936ec8d52b8020aa2cce0847e852d Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 11:30:51 +0200 Subject: [PATCH 16/21] Remove arrow data parsing from semantic extractors --- .../ros2msg/sensor_msgs/camera_info.rs | 208 +----------------- .../ros2msg/sensor_msgs/compressed_image.rs | 37 +--- .../src/parsers/ros2msg/sensor_msgs/image.rs | 61 +---- .../src/parsers/ros2msg/sensor_msgs/imu.rs | 37 ---- .../ros2msg/sensor_msgs/magnetic_field.rs | 43 +--- .../ros2msg/sensor_msgs/nav_sat_fix.rs | 48 +--- 6 files changed, 20 insertions(+), 414 deletions(-) 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 11b07630aa0c..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, } } @@ -63,15 +38,13 @@ impl Ros2MessageParser for ImageMessageParser { impl MessageParser for ImageMessageParser { fn append(&mut self, ctx: &mut ParserContext, msg: &mcap::Message<'_>) -> anyhow::Result<()> { re_tracing::profile_function!(); - // save raw message content for debugging: let sensor_msgs::Image { header, data, height, width, encoding, - is_bigendian, - step, + .. } = cdr::try_decode_message::>(&msg.data) .context("Failed to decode sensor_msgs::Image message from CDR data")?; @@ -91,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(()) } @@ -116,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) @@ -141,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(), From 0f46c16f1fe2a410f6c1f65b9cb6889bd4c97048 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 11:35:31 +0200 Subject: [PATCH 17/21] rust-analyzer refactor artifact --- crates/utils/re_mcap/src/layers/ros2_reflection.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crates/utils/re_mcap/src/layers/ros2_reflection.rs b/crates/utils/re_mcap/src/layers/ros2_reflection.rs index 28a90c74f323..a7192faf01d3 100644 --- a/crates/utils/re_mcap/src/layers/ros2_reflection.rs +++ b/crates/utils/re_mcap/src/layers/ros2_reflection.rs @@ -167,7 +167,7 @@ fn create_empty_message_chunk( timelines: IntMap, num_rows: usize, archetype_name: &str, -) -> Result { +) -> anyhow::Result { let empty_list = arrow::array::ListArray::new_null( std::sync::Arc::new(Field::new( "empty", From 40456d427f6f5b40346c9de82dd7d11c1097ee2b Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 11:59:37 +0200 Subject: [PATCH 18/21] Update snapshots/snippets --- .../tests/snapshots/test_mcap_loader__tests__ros2.snap | 4 ++-- tests/assets/rrd/snippets/howto/load_mcap.rrd | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 cfa374b048bf..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:6e27f8b4edf093402405d748659d04bf753a00bdd339f3ab0d1768edd0ad8952 -size 472210 +oid sha256:b1a4ac6fa84a2a951884291e6e4ea971186c23dd6369b4f4d0538b030ed0b77a +size 449123 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 From eb762706ac50b29ef5bef1d599d55776576d5f0c Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 12:22:19 +0200 Subject: [PATCH 19/21] escape --- crates/utils/re_ros_msg/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crates/utils/re_ros_msg/src/lib.rs b/crates/utils/re_ros_msg/src/lib.rs index 892c2a7ea116..ca7d18202487 100644 --- a/crates/utils/re_ros_msg/src/lib.rs +++ b/crates/utils/re_ros_msg/src/lib.rs @@ -95,7 +95,7 @@ fn extract_msg_specs(input: &str) -> Vec<(String, String)> { /// Parse a section of lines into a (name, body) pair. /// -/// The first line should contain "MSG: " and subsequent lines form the message body. +/// 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; From be990abed1a7c72db80b6424f37780616f4689cc Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 13:14:25 +0200 Subject: [PATCH 20/21] escape pt. 2 --- crates/utils/re_ros_msg/src/lib.rs | 2 +- new_changelog.md | 133 +++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 1 deletion(-) create mode 100644 new_changelog.md diff --git a/crates/utils/re_ros_msg/src/lib.rs b/crates/utils/re_ros_msg/src/lib.rs index ca7d18202487..1508c7beb952 100644 --- a/crates/utils/re_ros_msg/src/lib.rs +++ b/crates/utils/re_ros_msg/src/lib.rs @@ -66,7 +66,7 @@ fn extract_main_msg_spec(input: &str) -> String { .join("\n") } -/// Find "MSG: " and take the rest as content +/// 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. diff --git a/new_changelog.md b/new_changelog.md new file mode 100644 index 000000000000..eb2cdf59e8e5 --- /dev/null +++ b/new_changelog.md @@ -0,0 +1,133 @@ + +TODO: add link to release video + +๐Ÿ“– Release blogpost: TODO: add link + +๐Ÿงณ Migration guide: TODO: add link + +### โœจ Overview & highlights +TODO: fill in + +### โš ๏ธ Breaking changes +TODO: fill in +๐Ÿงณ Migration guide: TODO: add link (yes, again) + +### ๐Ÿ”Ž Details + +#### ๐Ÿ Python API +- Add module definition to all `pyclasses` [#11268](https://github.com/rerun-io/rerun/pull/11268) +- Python SDK: Add `timeout_sec` argument to `flush` [#11295](https://github.com/rerun-io/rerun/pull/11295) +- Python SDK: remove `blocking` argument of `flush` [#11314](https://github.com/rerun-io/rerun/pull/11314) +- Fix instances of `newest_first` not working correctly [#11326](https://github.com/rerun-io/rerun/pull/11326) +- Update Schema to make query view requirements clearer [#11287](https://github.com/rerun-io/rerun/pull/11287) +- Fix error when logging `AnyValues` with empty columns [#11322](https://github.com/rerun-io/rerun/pull/11322) +- Include "outer nullability" when we format column datatypes [#11339](https://github.com/rerun-io/rerun/pull/11339) +- Fix error message for what package to install [#11398](https://github.com/rerun-io/rerun/pull/11398) +- Py-SDK: More kw-args [#11384](https://github.com/rerun-io/rerun/pull/11384) +- Add meaningful equality comparisons to many rust wrappers [#11401](https://github.com/rerun-io/rerun/pull/11401) + +#### ๐Ÿชณ Bug fixes +- Fix edge case for parsing videos with constant frame size [#11226](https://github.com/rerun-io/rerun/pull/11226) +- Fix title bar height on macOS Tahoe [#11241](https://github.com/rerun-io/rerun/pull/11241) +- Blueprint panel can now always be resized [#11046](https://github.com/rerun-io/rerun/pull/11046) +- Fix rare issue where video stream sample indices would be determined incorrectly, breaking video inspection UI & playback [#11308](https://github.com/rerun-io/rerun/pull/11308) +- Fix links to custom timelines [#11333](https://github.com/rerun-io/rerun/pull/11333) +- Fix url edit warning spam [#11330](https://github.com/rerun-io/rerun/pull/11330) +- Fix race condition for ui adjustments while loading recordings from redap client [#11365](https://github.com/rerun-io/rerun/pull/11365) +- Fix formatting and parsing of component paths in URLs [#11364](https://github.com/rerun-io/rerun/pull/11364) +- Share button on notebook no longer uses the current base url for web viewer urls [#11379](https://github.com/rerun-io/rerun/pull/11379) +- Enable "Close current recording" only when there's a recording [#11353](https://github.com/rerun-io/rerun/pull/11353) + +#### ๐ŸŒ Viewer improvements +- Keep last paused time in web-viewer url [#11246](https://github.com/rerun-io/rerun/pull/11246) +- Add limited support for out-of-order video stream samples [#11307](https://github.com/rerun-io/rerun/pull/11307) +- Better video stream errors for missing samples & key frames [#11310](https://github.com/rerun-io/rerun/pull/11310) +- Add optional viewer url parameter to web app options [#11296](https://github.com/rerun-io/rerun/pull/11296) +- Add spectral colormap [#11298](https://github.com/rerun-io/rerun/pull/11298) +- Better gRPC errors [#11335](https://github.com/rerun-io/rerun/pull/11335) + +#### ๐Ÿ—„๏ธ OSS server +- Add table support to OSS server [#11356](https://github.com/rerun-io/rerun/pull/11356) + +#### ๐Ÿ“š Docs +- Add how to connect the server to the CLI command output [#11400](https://github.com/rerun-io/rerun/pull/11400) + +#### ๐Ÿ–ผ UI improvements +- Add copy-button to the recording link table item [#11242](https://github.com/rerun-io/rerun/pull/11242) +- Add copy link context menu to server entries [#11235](https://github.com/rerun-io/rerun/pull/11235) +- Add support for displaying timezone with our timestamps [#11234](https://github.com/rerun-io/rerun/pull/11234) +- Show loading screen when opening link [#11270](https://github.com/rerun-io/rerun/pull/11270) +- Support filtering by timestamp in table view [#11227](https://github.com/rerun-io/rerun/pull/11227) +- Add `starts with`/`ends with` string filters to table [#11341](https://github.com/rerun-io/rerun/pull/11341) +- Mark valid data ranges in timeline when loading data via range-limited URL [#11340](https://github.com/rerun-io/rerun/pull/11340) +- Add `does not contain` operator to string column filtering [#11349](https://github.com/rerun-io/rerun/pull/11349) +- Copy button on some selection items [#11337](https://github.com/rerun-io/rerun/pull/11337) +- Add `is not` to timestamp filtering [#11366](https://github.com/rerun-io/rerun/pull/11366) +- Add `is not` to nullable boolean filter [#11371](https://github.com/rerun-io/rerun/pull/11371) +- Treat`!=` filtering of numerical column as the inverse of `==` (aka. outer-NOT and ALL semantics) [#11372](https://github.com/rerun-io/rerun/pull/11372) +- Add context menu button to copy partition name [#11378](https://github.com/rerun-io/rerun/pull/11378) +- Store (and display) recordings in insertion order [#11415](https://github.com/rerun-io/rerun/pull/11415) + +#### ๐ŸŽจ Renderer improvements +- Export `BindGroupEntry` type to re_renderer rust dependents [#11406](https://github.com/rerun-io/rerun/pull/11406) (thanks [@Weijun-H](https://github.com/Weijun-H)!) + +#### ๐Ÿงข MCAP +- Add support for `enum` in protobuf MCAP messages [#11280](https://github.com/rerun-io/rerun/pull/11280) + +#### ๐Ÿง‘โ€๐Ÿ’ป Dev-experience +- Improve rrd loading errors by checking FourCC first [#11265](https://github.com/rerun-io/rerun/pull/11265) + +#### ๐Ÿ“ฆ Dependencies +- Update to wgpu 26 [#11300](https://github.com/rerun-io/rerun/pull/11300) +- Update DataFusion to version 49.0.2 [#11291](https://github.com/rerun-io/rerun/pull/11291) + + +#### Chronological changes (don't include these) +- Fix edge case for parsing videos with constant frame size [#11226](https://github.com/rerun-io/rerun/pull/11226) ce2bfd71c92a7b86ac5e53fb12929b64f44c0809 +- Fix title bar height on macOS Tahoe [#11241](https://github.com/rerun-io/rerun/pull/11241) fb3590a5b506101bd5947716de4b938c33efc589 +- Add copy-button to the recording link table item [#11242](https://github.com/rerun-io/rerun/pull/11242) 72ac4cbc8dbfacd2c80fb61ae8fe26a62ce8ee94 +- Add copy link context menu to server entries [#11235](https://github.com/rerun-io/rerun/pull/11235) e1d3a8ba8c4221b63181457a7372387db097fe61 +- Keep last paused time in web-viewer url [#11246](https://github.com/rerun-io/rerun/pull/11246) 094b8734116e9cf302b3cb9086923dca5530f3fc +- Add support for displaying timezone with our timestamps [#11234](https://github.com/rerun-io/rerun/pull/11234) 6935b54319c1b76e43a9a8e408047d0011578403 +- Blueprint panel can now always be resized [#11046](https://github.com/rerun-io/rerun/pull/11046) effa0a2f23d47f100c49f11ee93c8f6832e2ba09 +- Improve rrd loading errors by checking FourCC first [#11265](https://github.com/rerun-io/rerun/pull/11265) 2d0794a271549f40380655b3ec81c788426037ec +- Add support for `enum` in protobuf MCAP messages [#11280](https://github.com/rerun-io/rerun/pull/11280) 3d17d7f9096e8ffbfa39fb7c1881856f5e775134 +- Update to wgpu 26 [#11300](https://github.com/rerun-io/rerun/pull/11300) 65c4dddf73a8bb36babf0f34114feb35eca0a451 +- Fix rare issue where video stream sample indices would be determined incorrectly, breaking video inspection UI & playback [#11308](https://github.com/rerun-io/rerun/pull/11308) a339212ea48b24b3c9b28765fcdf6387036e115e +- Add module definition to all `pyclasses` [#11268](https://github.com/rerun-io/rerun/pull/11268) 29893f54b450b6fb40c98f68813a31ee00148235 +- Add limited support for out-of-order video stream samples [#11307](https://github.com/rerun-io/rerun/pull/11307) eee28a7fa51503ad8ea13529cd1c654f879878dd +- Better video stream errors for missing samples & key frames [#11310](https://github.com/rerun-io/rerun/pull/11310) c0794d9cdf84d2a441a205b2d6e592aa9594d826 +- Python SDK: Add `timeout_sec` argument to `flush` [#11295](https://github.com/rerun-io/rerun/pull/11295) 3dde2726ed56151765dfe359bd8eb547a02f1457 +- Show loading screen when opening link [#11270](https://github.com/rerun-io/rerun/pull/11270) 436ea5ca618ead9a6b26171552673900fb6f4ed3 +- Python SDK: remove `blocking` argument of `flush` [#11314](https://github.com/rerun-io/rerun/pull/11314) b5c3d7add12c02c3f5a6dd60dfebae8e85d10395 +- Add optional viewer url parameter to web app options [#11296](https://github.com/rerun-io/rerun/pull/11296) 4aa1ff21f3e65dcdd32ee5c4e7da93d51095d8f4 +- Add spectral colormap [#11298](https://github.com/rerun-io/rerun/pull/11298) 5c8022f6bdfd5018ce12bf814f0cf579c78c1fff +- Fix instances of `newest_first` not working correctly [#11326](https://github.com/rerun-io/rerun/pull/11326) fc7b7cac782e6ea6c342703da81c1b3eaf63dbf5 +- Update Schema to make query view requirements clearer [#11287](https://github.com/rerun-io/rerun/pull/11287) bb3fb6fa1a70d1e6884c752336c0724bf3e6907a +- Fix error when logging `AnyValues` with empty columns [#11322](https://github.com/rerun-io/rerun/pull/11322) 0f7f7ba339693ab95de68619a30a1a69e0638d9d +- Update DataFusion to version 49.0.2 [#11291](https://github.com/rerun-io/rerun/pull/11291) 7b5245fd6d4071a9281d622639de01a9bb96f341 +- Support filtering by timestamp in table view [#11227](https://github.com/rerun-io/rerun/pull/11227) 7d13956ad9dfed23f0e9e54751935434e3ddb3d4 +- Fix links to custom timelines [#11333](https://github.com/rerun-io/rerun/pull/11333) 76629b919e9035e02716aa21783283bb36de2d15 +- Fix url edit warning spam [#11330](https://github.com/rerun-io/rerun/pull/11330) 6808754dc17939154949d46663174511cfb64dfc +- Include "outer nullability" when we format column datatypes [#11339](https://github.com/rerun-io/rerun/pull/11339) 9869eaec8c1d8fd3328cbd9fb8ca066fb780a194 +- Add `starts with`/`ends with` string filters to table [#11341](https://github.com/rerun-io/rerun/pull/11341) 5710c79827bd0c7518826abd0de41cb147f98407 +- Better gRPC errors [#11335](https://github.com/rerun-io/rerun/pull/11335) bb7d135aeb8f396f2825c85e31c4399e8a86a967 +- Mark valid data ranges in timeline when loading data via range-limited URL [#11340](https://github.com/rerun-io/rerun/pull/11340) 6c651ab064794c932271768dbf1cb13399a4a823 +- Add `does not contain` operator to string column filtering [#11349](https://github.com/rerun-io/rerun/pull/11349) a17073721f2be045aa3befec84edc205e4f55727 +- Copy button on some selection items [#11337](https://github.com/rerun-io/rerun/pull/11337) b1c64773503336f753f73d1fc33500f8c8513f29 +- Add table support to OSS server [#11356](https://github.com/rerun-io/rerun/pull/11356) 21753bbae37b7593e7c2423a4c23b74419492701 +- Add `is not` to timestamp filtering [#11366](https://github.com/rerun-io/rerun/pull/11366) de829816c0bf9e4bafc4ad7946551628163ee70d +- Fix race condition for ui adjustments while loading recordings from redap client [#11365](https://github.com/rerun-io/rerun/pull/11365) 61534320456bb83aef54254990fed12ef0941855 +- Add `is not` to nullable boolean filter [#11371](https://github.com/rerun-io/rerun/pull/11371) 5e62eac4dbe3c5ad159143e947022312f72732dc +- Treat`!=` filtering of numerical column as the inverse of `==` (aka. outer-NOT and ALL semantics) [#11372](https://github.com/rerun-io/rerun/pull/11372) 8d338d0f531976cdff0a1e4a62c7c1af5ad68acc +- Fix formatting and parsing of component paths in URLs [#11364](https://github.com/rerun-io/rerun/pull/11364) 812a72cf370b5b1a9e1056c409cb5eb4212914ff +- Add context menu button to copy partition name [#11378](https://github.com/rerun-io/rerun/pull/11378) 76681413ab84c62e2ec389a2ca0753835d2cec40 +- Share button on notebook no longer uses the current base url for web viewer urls [#11379](https://github.com/rerun-io/rerun/pull/11379) be0afb138d3b2e004c92233a3370e8a2840ff307 +- Enable "Close current recording" only when there's a recording [#11353](https://github.com/rerun-io/rerun/pull/11353) 9b453bedfa3b0c9bba85dff73448abf083d36abb +- Fix error message for what package to install [#11398](https://github.com/rerun-io/rerun/pull/11398) 90479b300bd739b5e73da49b68f02fef4fcba217 +- Py-SDK: More kw-args [#11384](https://github.com/rerun-io/rerun/pull/11384) 296a5744421ed1472838bea9043311e2f43d62f7 +- Export `BindGroupEntry` type to re_renderer rust dependents [#11406](https://github.com/rerun-io/rerun/pull/11406) 38023e9da5194f8c2e552a8efcb546894fb4d901 +- Add meaningful equality comparisons to many rust wrappers [#11401](https://github.com/rerun-io/rerun/pull/11401) 4590f7f97331acf037f4e80f9c2e1eeab47f0292 +- Store (and display) recordings in insertion order [#11415](https://github.com/rerun-io/rerun/pull/11415) a9436a8efa9c16f81137aef63e464f337b5c84fb +- Add how to connect the server to the CLI command output [#11400](https://github.com/rerun-io/rerun/pull/11400) 02dabe991bd3c2cb7343ca90ba71b0165b8c1fea + From 0ff050b347c2bf332f38712602867d345f120586 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Mon, 6 Oct 2025 13:20:45 +0200 Subject: [PATCH 21/21] remove sneaky file --- new_changelog.md | 133 ----------------------------------------------- 1 file changed, 133 deletions(-) delete mode 100644 new_changelog.md diff --git a/new_changelog.md b/new_changelog.md deleted file mode 100644 index eb2cdf59e8e5..000000000000 --- a/new_changelog.md +++ /dev/null @@ -1,133 +0,0 @@ - -TODO: add link to release video - -๐Ÿ“– Release blogpost: TODO: add link - -๐Ÿงณ Migration guide: TODO: add link - -### โœจ Overview & highlights -TODO: fill in - -### โš ๏ธ Breaking changes -TODO: fill in -๐Ÿงณ Migration guide: TODO: add link (yes, again) - -### ๐Ÿ”Ž Details - -#### ๐Ÿ Python API -- Add module definition to all `pyclasses` [#11268](https://github.com/rerun-io/rerun/pull/11268) -- Python SDK: Add `timeout_sec` argument to `flush` [#11295](https://github.com/rerun-io/rerun/pull/11295) -- Python SDK: remove `blocking` argument of `flush` [#11314](https://github.com/rerun-io/rerun/pull/11314) -- Fix instances of `newest_first` not working correctly [#11326](https://github.com/rerun-io/rerun/pull/11326) -- Update Schema to make query view requirements clearer [#11287](https://github.com/rerun-io/rerun/pull/11287) -- Fix error when logging `AnyValues` with empty columns [#11322](https://github.com/rerun-io/rerun/pull/11322) -- Include "outer nullability" when we format column datatypes [#11339](https://github.com/rerun-io/rerun/pull/11339) -- Fix error message for what package to install [#11398](https://github.com/rerun-io/rerun/pull/11398) -- Py-SDK: More kw-args [#11384](https://github.com/rerun-io/rerun/pull/11384) -- Add meaningful equality comparisons to many rust wrappers [#11401](https://github.com/rerun-io/rerun/pull/11401) - -#### ๐Ÿชณ Bug fixes -- Fix edge case for parsing videos with constant frame size [#11226](https://github.com/rerun-io/rerun/pull/11226) -- Fix title bar height on macOS Tahoe [#11241](https://github.com/rerun-io/rerun/pull/11241) -- Blueprint panel can now always be resized [#11046](https://github.com/rerun-io/rerun/pull/11046) -- Fix rare issue where video stream sample indices would be determined incorrectly, breaking video inspection UI & playback [#11308](https://github.com/rerun-io/rerun/pull/11308) -- Fix links to custom timelines [#11333](https://github.com/rerun-io/rerun/pull/11333) -- Fix url edit warning spam [#11330](https://github.com/rerun-io/rerun/pull/11330) -- Fix race condition for ui adjustments while loading recordings from redap client [#11365](https://github.com/rerun-io/rerun/pull/11365) -- Fix formatting and parsing of component paths in URLs [#11364](https://github.com/rerun-io/rerun/pull/11364) -- Share button on notebook no longer uses the current base url for web viewer urls [#11379](https://github.com/rerun-io/rerun/pull/11379) -- Enable "Close current recording" only when there's a recording [#11353](https://github.com/rerun-io/rerun/pull/11353) - -#### ๐ŸŒ Viewer improvements -- Keep last paused time in web-viewer url [#11246](https://github.com/rerun-io/rerun/pull/11246) -- Add limited support for out-of-order video stream samples [#11307](https://github.com/rerun-io/rerun/pull/11307) -- Better video stream errors for missing samples & key frames [#11310](https://github.com/rerun-io/rerun/pull/11310) -- Add optional viewer url parameter to web app options [#11296](https://github.com/rerun-io/rerun/pull/11296) -- Add spectral colormap [#11298](https://github.com/rerun-io/rerun/pull/11298) -- Better gRPC errors [#11335](https://github.com/rerun-io/rerun/pull/11335) - -#### ๐Ÿ—„๏ธ OSS server -- Add table support to OSS server [#11356](https://github.com/rerun-io/rerun/pull/11356) - -#### ๐Ÿ“š Docs -- Add how to connect the server to the CLI command output [#11400](https://github.com/rerun-io/rerun/pull/11400) - -#### ๐Ÿ–ผ UI improvements -- Add copy-button to the recording link table item [#11242](https://github.com/rerun-io/rerun/pull/11242) -- Add copy link context menu to server entries [#11235](https://github.com/rerun-io/rerun/pull/11235) -- Add support for displaying timezone with our timestamps [#11234](https://github.com/rerun-io/rerun/pull/11234) -- Show loading screen when opening link [#11270](https://github.com/rerun-io/rerun/pull/11270) -- Support filtering by timestamp in table view [#11227](https://github.com/rerun-io/rerun/pull/11227) -- Add `starts with`/`ends with` string filters to table [#11341](https://github.com/rerun-io/rerun/pull/11341) -- Mark valid data ranges in timeline when loading data via range-limited URL [#11340](https://github.com/rerun-io/rerun/pull/11340) -- Add `does not contain` operator to string column filtering [#11349](https://github.com/rerun-io/rerun/pull/11349) -- Copy button on some selection items [#11337](https://github.com/rerun-io/rerun/pull/11337) -- Add `is not` to timestamp filtering [#11366](https://github.com/rerun-io/rerun/pull/11366) -- Add `is not` to nullable boolean filter [#11371](https://github.com/rerun-io/rerun/pull/11371) -- Treat`!=` filtering of numerical column as the inverse of `==` (aka. outer-NOT and ALL semantics) [#11372](https://github.com/rerun-io/rerun/pull/11372) -- Add context menu button to copy partition name [#11378](https://github.com/rerun-io/rerun/pull/11378) -- Store (and display) recordings in insertion order [#11415](https://github.com/rerun-io/rerun/pull/11415) - -#### ๐ŸŽจ Renderer improvements -- Export `BindGroupEntry` type to re_renderer rust dependents [#11406](https://github.com/rerun-io/rerun/pull/11406) (thanks [@Weijun-H](https://github.com/Weijun-H)!) - -#### ๐Ÿงข MCAP -- Add support for `enum` in protobuf MCAP messages [#11280](https://github.com/rerun-io/rerun/pull/11280) - -#### ๐Ÿง‘โ€๐Ÿ’ป Dev-experience -- Improve rrd loading errors by checking FourCC first [#11265](https://github.com/rerun-io/rerun/pull/11265) - -#### ๐Ÿ“ฆ Dependencies -- Update to wgpu 26 [#11300](https://github.com/rerun-io/rerun/pull/11300) -- Update DataFusion to version 49.0.2 [#11291](https://github.com/rerun-io/rerun/pull/11291) - - -#### Chronological changes (don't include these) -- Fix edge case for parsing videos with constant frame size [#11226](https://github.com/rerun-io/rerun/pull/11226) ce2bfd71c92a7b86ac5e53fb12929b64f44c0809 -- Fix title bar height on macOS Tahoe [#11241](https://github.com/rerun-io/rerun/pull/11241) fb3590a5b506101bd5947716de4b938c33efc589 -- Add copy-button to the recording link table item [#11242](https://github.com/rerun-io/rerun/pull/11242) 72ac4cbc8dbfacd2c80fb61ae8fe26a62ce8ee94 -- Add copy link context menu to server entries [#11235](https://github.com/rerun-io/rerun/pull/11235) e1d3a8ba8c4221b63181457a7372387db097fe61 -- Keep last paused time in web-viewer url [#11246](https://github.com/rerun-io/rerun/pull/11246) 094b8734116e9cf302b3cb9086923dca5530f3fc -- Add support for displaying timezone with our timestamps [#11234](https://github.com/rerun-io/rerun/pull/11234) 6935b54319c1b76e43a9a8e408047d0011578403 -- Blueprint panel can now always be resized [#11046](https://github.com/rerun-io/rerun/pull/11046) effa0a2f23d47f100c49f11ee93c8f6832e2ba09 -- Improve rrd loading errors by checking FourCC first [#11265](https://github.com/rerun-io/rerun/pull/11265) 2d0794a271549f40380655b3ec81c788426037ec -- Add support for `enum` in protobuf MCAP messages [#11280](https://github.com/rerun-io/rerun/pull/11280) 3d17d7f9096e8ffbfa39fb7c1881856f5e775134 -- Update to wgpu 26 [#11300](https://github.com/rerun-io/rerun/pull/11300) 65c4dddf73a8bb36babf0f34114feb35eca0a451 -- Fix rare issue where video stream sample indices would be determined incorrectly, breaking video inspection UI & playback [#11308](https://github.com/rerun-io/rerun/pull/11308) a339212ea48b24b3c9b28765fcdf6387036e115e -- Add module definition to all `pyclasses` [#11268](https://github.com/rerun-io/rerun/pull/11268) 29893f54b450b6fb40c98f68813a31ee00148235 -- Add limited support for out-of-order video stream samples [#11307](https://github.com/rerun-io/rerun/pull/11307) eee28a7fa51503ad8ea13529cd1c654f879878dd -- Better video stream errors for missing samples & key frames [#11310](https://github.com/rerun-io/rerun/pull/11310) c0794d9cdf84d2a441a205b2d6e592aa9594d826 -- Python SDK: Add `timeout_sec` argument to `flush` [#11295](https://github.com/rerun-io/rerun/pull/11295) 3dde2726ed56151765dfe359bd8eb547a02f1457 -- Show loading screen when opening link [#11270](https://github.com/rerun-io/rerun/pull/11270) 436ea5ca618ead9a6b26171552673900fb6f4ed3 -- Python SDK: remove `blocking` argument of `flush` [#11314](https://github.com/rerun-io/rerun/pull/11314) b5c3d7add12c02c3f5a6dd60dfebae8e85d10395 -- Add optional viewer url parameter to web app options [#11296](https://github.com/rerun-io/rerun/pull/11296) 4aa1ff21f3e65dcdd32ee5c4e7da93d51095d8f4 -- Add spectral colormap [#11298](https://github.com/rerun-io/rerun/pull/11298) 5c8022f6bdfd5018ce12bf814f0cf579c78c1fff -- Fix instances of `newest_first` not working correctly [#11326](https://github.com/rerun-io/rerun/pull/11326) fc7b7cac782e6ea6c342703da81c1b3eaf63dbf5 -- Update Schema to make query view requirements clearer [#11287](https://github.com/rerun-io/rerun/pull/11287) bb3fb6fa1a70d1e6884c752336c0724bf3e6907a -- Fix error when logging `AnyValues` with empty columns [#11322](https://github.com/rerun-io/rerun/pull/11322) 0f7f7ba339693ab95de68619a30a1a69e0638d9d -- Update DataFusion to version 49.0.2 [#11291](https://github.com/rerun-io/rerun/pull/11291) 7b5245fd6d4071a9281d622639de01a9bb96f341 -- Support filtering by timestamp in table view [#11227](https://github.com/rerun-io/rerun/pull/11227) 7d13956ad9dfed23f0e9e54751935434e3ddb3d4 -- Fix links to custom timelines [#11333](https://github.com/rerun-io/rerun/pull/11333) 76629b919e9035e02716aa21783283bb36de2d15 -- Fix url edit warning spam [#11330](https://github.com/rerun-io/rerun/pull/11330) 6808754dc17939154949d46663174511cfb64dfc -- Include "outer nullability" when we format column datatypes [#11339](https://github.com/rerun-io/rerun/pull/11339) 9869eaec8c1d8fd3328cbd9fb8ca066fb780a194 -- Add `starts with`/`ends with` string filters to table [#11341](https://github.com/rerun-io/rerun/pull/11341) 5710c79827bd0c7518826abd0de41cb147f98407 -- Better gRPC errors [#11335](https://github.com/rerun-io/rerun/pull/11335) bb7d135aeb8f396f2825c85e31c4399e8a86a967 -- Mark valid data ranges in timeline when loading data via range-limited URL [#11340](https://github.com/rerun-io/rerun/pull/11340) 6c651ab064794c932271768dbf1cb13399a4a823 -- Add `does not contain` operator to string column filtering [#11349](https://github.com/rerun-io/rerun/pull/11349) a17073721f2be045aa3befec84edc205e4f55727 -- Copy button on some selection items [#11337](https://github.com/rerun-io/rerun/pull/11337) b1c64773503336f753f73d1fc33500f8c8513f29 -- Add table support to OSS server [#11356](https://github.com/rerun-io/rerun/pull/11356) 21753bbae37b7593e7c2423a4c23b74419492701 -- Add `is not` to timestamp filtering [#11366](https://github.com/rerun-io/rerun/pull/11366) de829816c0bf9e4bafc4ad7946551628163ee70d -- Fix race condition for ui adjustments while loading recordings from redap client [#11365](https://github.com/rerun-io/rerun/pull/11365) 61534320456bb83aef54254990fed12ef0941855 -- Add `is not` to nullable boolean filter [#11371](https://github.com/rerun-io/rerun/pull/11371) 5e62eac4dbe3c5ad159143e947022312f72732dc -- Treat`!=` filtering of numerical column as the inverse of `==` (aka. outer-NOT and ALL semantics) [#11372](https://github.com/rerun-io/rerun/pull/11372) 8d338d0f531976cdff0a1e4a62c7c1af5ad68acc -- Fix formatting and parsing of component paths in URLs [#11364](https://github.com/rerun-io/rerun/pull/11364) 812a72cf370b5b1a9e1056c409cb5eb4212914ff -- Add context menu button to copy partition name [#11378](https://github.com/rerun-io/rerun/pull/11378) 76681413ab84c62e2ec389a2ca0753835d2cec40 -- Share button on notebook no longer uses the current base url for web viewer urls [#11379](https://github.com/rerun-io/rerun/pull/11379) be0afb138d3b2e004c92233a3370e8a2840ff307 -- Enable "Close current recording" only when there's a recording [#11353](https://github.com/rerun-io/rerun/pull/11353) 9b453bedfa3b0c9bba85dff73448abf083d36abb -- Fix error message for what package to install [#11398](https://github.com/rerun-io/rerun/pull/11398) 90479b300bd739b5e73da49b68f02fef4fcba217 -- Py-SDK: More kw-args [#11384](https://github.com/rerun-io/rerun/pull/11384) 296a5744421ed1472838bea9043311e2f43d62f7 -- Export `BindGroupEntry` type to re_renderer rust dependents [#11406](https://github.com/rerun-io/rerun/pull/11406) 38023e9da5194f8c2e552a8efcb546894fb4d901 -- Add meaningful equality comparisons to many rust wrappers [#11401](https://github.com/rerun-io/rerun/pull/11401) 4590f7f97331acf037f4e80f9c2e1eeab47f0292 -- Store (and display) recordings in insertion order [#11415](https://github.com/rerun-io/rerun/pull/11415) a9436a8efa9c16f81137aef63e464f337b5c84fb -- Add how to connect the server to the CLI command output [#11400](https://github.com/rerun-io/rerun/pull/11400) 02dabe991bd3c2cb7343ca90ba71b0165b8c1fea -