spatialrust_sync/
mcap_io.rs

1//! MCAP read/write for SpatialRust stamped XYZ records.
2//!
3//! Payload encoding: `application/x-spatialrust-xyz-v1`
4//!
5//! ```text
6//! u32 le magic = 0x5352_5859 ("SRXY")
7//! u32 le point_count
8//! u32 le schema_id_len
9//! utf8 schema_id
10//! u32 le schema_major
11//! u32 le schema_minor
12//! u64 le stamp_ns
13//! u8  clock_domain (0 HostSteady, 1 HostWall, 2 Sensor, 3 External)
14//! f32 le xyz[point_count * 3]
15//! ```
16
17use std::{
18    collections::BTreeMap,
19    fs::File,
20    io::{BufWriter, Cursor, Read},
21    path::Path,
22};
23
24use mcap::{records::MessageHeader, Message, Writer};
25use spatialrust_core::{
26    PointBuffer, PointBufferSet, PointCloud, SpatialMetadata, StandardSchemas, Timestamp,
27};
28use spatialrust_records::{SchemaVersion, SpatialRecord};
29
30use crate::{
31    ClockDomain, ClockId, MemoryEpisode, StampedRecord, StampedTime, SyncError, SyncResult, TopicId,
32};
33
34const MAGIC: u32 = 0x5352_5859;
35const ENCODING: &str = "application/x-spatialrust-xyz-v1";
36const SCHEMA_NAME: &str = "spatialrust/xyz/v1";
37
38/// Writes a [`MemoryEpisode`] of XYZ records into an MCAP file.
39pub fn write_memory_episode_mcap(
40    path: impl AsRef<Path>,
41    episode: &MemoryEpisode,
42) -> SyncResult<()> {
43    let file = File::create(path.as_ref()).map_err(io_err)?;
44    let mut writer = Writer::new(BufWriter::new(file)).map_err(mcap_err)?;
45    let schema_id = writer.add_schema(SCHEMA_NAME, ENCODING, &[]).map_err(mcap_err)?;
46
47    let mut channels = BTreeMap::<String, u16>::new();
48    for (sequence, stamped) in episode.records().iter().enumerate() {
49        let topic = stamped.topic.as_str().to_owned();
50        let channel_id = if let Some(id) = channels.get(&topic) {
51            *id
52        } else {
53            let id = writer
54                .add_channel(schema_id, &topic, ENCODING, &BTreeMap::new())
55                .map_err(mcap_err)?;
56            channels.insert(topic, id);
57            id
58        };
59        let payload = encode_stamped_xyz(stamped)?;
60        let stamp = stamped.stamp.as_nanos();
61        writer
62            .write_to_known_channel(
63                &MessageHeader {
64                    channel_id,
65                    sequence: sequence as u32,
66                    log_time: stamp,
67                    publish_time: stamp,
68                },
69                &payload,
70            )
71            .map_err(mcap_err)?;
72    }
73    writer.finish().map_err(mcap_err)?;
74    Ok(())
75}
76
77/// Reads stamped XYZ records from an MCAP file into a [`MemoryEpisode`].
78pub fn read_memory_episode_mcap(path: impl AsRef<Path>) -> SyncResult<MemoryEpisode> {
79    let bytes = std::fs::read(path.as_ref()).map_err(io_err)?;
80    let mut records = Vec::new();
81    for message in mcap::MessageStream::new(&bytes).map_err(mcap_err)? {
82        let message = message.map_err(mcap_err)?;
83        if message.channel.message_encoding != ENCODING {
84            return Err(SyncError::InvalidConfiguration(format!(
85                "unsupported MCAP encoding `{}`",
86                message.channel.message_encoding
87            )));
88        }
89        records.push(decode_stamped_xyz(&message)?);
90    }
91    Ok(MemoryEpisode::from_records(records))
92}
93
94fn encode_stamped_xyz(stamped: &StampedRecord) -> SyncResult<Vec<u8>> {
95    let cloud = stamped.record.cloud();
96    let xs = cloud.field("x")?.as_f32()?;
97    let ys = cloud.field("y")?.as_f32()?;
98    let zs = cloud.field("z")?.as_f32()?;
99    if xs.len() != ys.len() || ys.len() != zs.len() {
100        return Err(SyncError::InvalidConfiguration("xyz lengths disagree".into()));
101    }
102    let schema = stamped.record.schema();
103    let mut out = Vec::new();
104    out.extend_from_slice(&MAGIC.to_le_bytes());
105    out.extend_from_slice(&(xs.len() as u32).to_le_bytes());
106    let id = schema.id.as_str().as_bytes();
107    out.extend_from_slice(&(id.len() as u32).to_le_bytes());
108    out.extend_from_slice(id);
109    out.extend_from_slice(&schema.version.major.to_le_bytes());
110    out.extend_from_slice(&schema.version.minor.to_le_bytes());
111    out.extend_from_slice(&stamped.stamp.as_nanos().to_le_bytes());
112    out.push(domain_byte(stamped.stamp.domain));
113    for i in 0..xs.len() {
114        out.extend_from_slice(&xs[i].to_le_bytes());
115        out.extend_from_slice(&ys[i].to_le_bytes());
116        out.extend_from_slice(&zs[i].to_le_bytes());
117    }
118    Ok(out)
119}
120
121fn decode_stamped_xyz(message: &Message<'_>) -> SyncResult<StampedRecord> {
122    let mut cur = Cursor::new(message.data.as_ref());
123    let magic = read_u32(&mut cur)?;
124    if magic != MAGIC {
125        return Err(SyncError::InvalidConfiguration("bad spatialrust MCAP magic".into()));
126    }
127    let count = read_u32(&mut cur)? as usize;
128    let id_len = read_u32(&mut cur)? as usize;
129    let mut id_bytes = vec![0_u8; id_len];
130    cur.read_exact(&mut id_bytes).map_err(io_err)?;
131    let schema_id = String::from_utf8(id_bytes)
132        .map_err(|_| SyncError::InvalidConfiguration("schema id is not UTF-8".into()))?;
133    let major = read_u32(&mut cur)?;
134    let minor = read_u32(&mut cur)?;
135    let stamp_ns = read_u64(&mut cur)?;
136    let mut domain_byte = [0_u8; 1];
137    cur.read_exact(&mut domain_byte).map_err(io_err)?;
138    let domain = byte_domain(domain_byte[0])?;
139
140    let mut xs = Vec::with_capacity(count);
141    let mut ys = Vec::with_capacity(count);
142    let mut zs = Vec::with_capacity(count);
143    for _ in 0..count {
144        xs.push(read_f32(&mut cur)?);
145        ys.push(read_f32(&mut cur)?);
146        zs.push(read_f32(&mut cur)?);
147    }
148
149    let mut buffers = PointBufferSet::new();
150    buffers.insert("x", PointBuffer::from_f32(xs));
151    buffers.insert("y", PointBuffer::from_f32(ys));
152    buffers.insert("z", PointBuffer::from_f32(zs));
153    let cloud = PointCloud::try_from_parts(
154        StandardSchemas::point_xyz(),
155        buffers,
156        SpatialMetadata::default(),
157    )?;
158    let record = SpatialRecord::try_from_cloud(schema_id, SchemaVersion::new(major, minor), cloud)?;
159    let stamp = StampedTime {
160        clock: ClockId::new(message.channel.topic.clone()),
161        domain,
162        timestamp: Timestamp::from_nanos(stamp_ns),
163        quality: crate::SyncQuality::exact(),
164    };
165    Ok(StampedRecord::new(TopicId::new(message.channel.topic.clone()), stamp, record))
166}
167
168fn domain_byte(domain: ClockDomain) -> u8 {
169    match domain {
170        ClockDomain::HostSteady => 0,
171        ClockDomain::HostWall => 1,
172        ClockDomain::Sensor => 2,
173        ClockDomain::External => 3,
174    }
175}
176
177fn byte_domain(value: u8) -> SyncResult<ClockDomain> {
178    match value {
179        0 => Ok(ClockDomain::HostSteady),
180        1 => Ok(ClockDomain::HostWall),
181        2 => Ok(ClockDomain::Sensor),
182        3 => Ok(ClockDomain::External),
183        other => Err(SyncError::InvalidConfiguration(format!("unknown clock domain byte {other}"))),
184    }
185}
186
187fn read_u32(cur: &mut Cursor<&[u8]>) -> SyncResult<u32> {
188    let mut buf = [0_u8; 4];
189    cur.read_exact(&mut buf).map_err(io_err)?;
190    Ok(u32::from_le_bytes(buf))
191}
192
193fn read_u64(cur: &mut Cursor<&[u8]>) -> SyncResult<u64> {
194    let mut buf = [0_u8; 8];
195    cur.read_exact(&mut buf).map_err(io_err)?;
196    Ok(u64::from_le_bytes(buf))
197}
198
199fn read_f32(cur: &mut Cursor<&[u8]>) -> SyncResult<f32> {
200    let mut buf = [0_u8; 4];
201    cur.read_exact(&mut buf).map_err(io_err)?;
202    Ok(f32::from_le_bytes(buf))
203}
204
205fn io_err(error: impl std::fmt::Display) -> SyncError {
206    SyncError::Io(error.to_string())
207}
208
209fn mcap_err(error: impl std::fmt::Display) -> SyncError {
210    SyncError::Mcap(error.to_string())
211}
212
213#[cfg(test)]
214mod tests {
215    use super::{read_memory_episode_mcap, write_memory_episode_mcap};
216    use crate::{ClockDomain, MemoryEpisode, StampedRecord, StampedTime};
217    use spatialrust_core::{
218        PointBuffer, PointBufferSet, PointCloud, SpatialMetadata, StandardSchemas, Timestamp,
219    };
220    use spatialrust_records::{SchemaVersion, SpatialRecord};
221
222    #[test]
223    fn roundtrips_xyz_episode() {
224        let mut buffers = PointBufferSet::new();
225        buffers.insert("x", PointBuffer::from_f32(vec![1.0, 2.0]));
226        buffers.insert("y", PointBuffer::from_f32(vec![0.0, 0.0]));
227        buffers.insert("z", PointBuffer::from_f32(vec![3.0, 4.0]));
228        let cloud = PointCloud::try_from_parts(
229            StandardSchemas::point_xyz(),
230            buffers,
231            SpatialMetadata::default(),
232        )
233        .unwrap();
234        let record =
235            SpatialRecord::try_from_cloud("point", SchemaVersion::new(1, 2), cloud).unwrap();
236        let stamped = StampedRecord::new(
237            "lidar",
238            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(42)),
239            record,
240        );
241        let episode = MemoryEpisode::from_records(vec![stamped]);
242        let dir = tempfile::tempdir().unwrap();
243        let path = dir.path().join("episode.mcap");
244        write_memory_episode_mcap(&path, &episode).unwrap();
245        let loaded = read_memory_episode_mcap(&path).unwrap();
246        assert_eq!(loaded.records().len(), 1);
247        assert_eq!(loaded.records()[0].topic.as_str(), "lidar");
248        assert_eq!(loaded.records()[0].stamp.as_nanos(), 42);
249        assert_eq!(
250            loaded.records()[0].record.cloud().field("x").unwrap().as_f32().unwrap(),
251            &[1.0, 2.0]
252        );
253    }
254}
spatialrust_sync/mcap_io.rs

spatialrust_sync/
mcap_io.rs
