spatialrust_runtime/
ros2.rs

1//! ROS 2 adaptation contracts and CDR PointCloud2 codecs (no rclrs link).
2//!
3//! Native `rclrs` executors still require an installed ROS 2 toolchain and stay
4//! outside this crate. Enabling `ros2` provides message negotiation, CDR LE
5//! `sensor_msgs/msg/PointCloud2` XYZ codecs, and an in-process loopback node.
6
7use crate::{RuntimeError, RuntimeResult};
8
9/// Hint describing a ROS 2 message mapping.
10#[derive(Clone, Debug, PartialEq, Eq)]
11pub struct Ros2MessageHint {
12    /// Fully-qualified ROS type name, e.g. `sensor_msgs/msg/PointCloud2`.
13    pub type_name: String,
14    /// SpatialRust topic / schema id.
15    pub spatial_topic: String,
16}
17
18/// Adapter interface for ROS 2 type negotiation.
19pub trait Ros2Adapter {
20    /// Returns supported type mappings.
21    fn supported_types(&self) -> &[Ros2MessageHint];
22
23    /// Negotiates a preferred mapping for one ROS type.
24    fn negotiate(&self, ros_type: &str) -> Option<&Ros2MessageHint>;
25}
26
27/// In-memory catalog adapter used by default builds with `ros2` enabled.
28#[derive(Clone, Debug, Default)]
29pub struct CatalogRos2Adapter {
30    hints: Vec<Ros2MessageHint>,
31}
32
33impl CatalogRos2Adapter {
34    /// Creates an adapter from a catalog.
35    #[must_use]
36    pub fn new(hints: Vec<Ros2MessageHint>) -> Self {
37        Self { hints }
38    }
39
40    /// Returns a catalog covering common XYZ point-cloud mappings.
41    #[must_use]
42    pub fn point_cloud2_xyz() -> Self {
43        Self::new(vec![Ros2MessageHint {
44            type_name: POINT_CLOUD2_TYPE.into(),
45            spatial_topic: "point/xyz".into(),
46        }])
47    }
48}
49
50impl Ros2Adapter for CatalogRos2Adapter {
51    fn supported_types(&self) -> &[Ros2MessageHint] {
52        &self.hints
53    }
54
55    fn negotiate(&self, ros_type: &str) -> Option<&Ros2MessageHint> {
56        self.hints.iter().find(|hint| hint.type_name == ros_type)
57    }
58}
59
60/// Canonical ROS 2 type name for PointCloud2.
61pub const POINT_CLOUD2_TYPE: &str = "sensor_msgs/msg/PointCloud2";
62
63/// CDR encapsulation header for little-endian ROS 2 messages.
64const CDR_LE_ENCAP: [u8; 4] = [0x00, 0x01, 0x00, 0x00];
65
66/// Interleaved XYZ PointCloud2 payload.
67#[derive(Clone, Debug, PartialEq)]
68pub struct PointCloud2Xyz {
69    /// ROS frame id.
70    pub frame_id: String,
71    /// Header stamp seconds.
72    pub stamp_sec: i32,
73    /// Header stamp nanoseconds.
74    pub stamp_nanosec: u32,
75    /// Interleaved XYZ floats.
76    pub xyz: Vec<f32>,
77}
78
79impl PointCloud2Xyz {
80    /// Creates a validated XYZ cloud (`xyz.len()` divisible by 3).
81    pub fn try_new(
82        frame_id: impl Into<String>,
83        stamp_sec: i32,
84        stamp_nanosec: u32,
85        xyz: Vec<f32>,
86    ) -> RuntimeResult<Self> {
87        if xyz.len() % 3 != 0 {
88            return Err(RuntimeError::InvalidConfiguration(
89                "xyz length must be a multiple of 3".into(),
90            ));
91        }
92        Ok(Self { frame_id: frame_id.into(), stamp_sec, stamp_nanosec, xyz })
93    }
94
95    /// Returns point count.
96    #[must_use]
97    pub fn point_count(&self) -> usize {
98        self.xyz.len() / 3
99    }
100}
101
102/// Encodes an XYZ PointCloud2 as ROS 2 CDR little-endian bytes.
103pub fn encode_point_cloud2_xyz(msg: &PointCloud2Xyz) -> RuntimeResult<Vec<u8>> {
104    let mut w = CdrWriter::new();
105    w.write_encap();
106    w.write_i32(msg.stamp_sec);
107    w.write_u32(msg.stamp_nanosec);
108    w.write_string(&msg.frame_id)?;
109    let width = msg.point_count() as u32;
110    w.write_u32(1); // height
111    w.write_u32(width);
112    w.write_u32(3); // fields length
113    write_point_field(&mut w, "x", 0)?;
114    write_point_field(&mut w, "y", 4)?;
115    write_point_field(&mut w, "z", 8)?;
116    w.write_bool(false); // is_bigendian
117    w.write_u32(12); // point_step
118    w.write_u32(width.saturating_mul(12)); // row_step
119    let data: Vec<u8> = msg.xyz.iter().flat_map(|v| v.to_le_bytes()).collect();
120    w.write_u32(data.len() as u32);
121    w.write_bytes(&data);
122    w.write_bool(true); // is_dense
123    Ok(w.into_bytes())
124}
125
126/// Decodes an XYZ PointCloud2 from ROS 2 CDR little-endian bytes.
127pub fn decode_point_cloud2_xyz(bytes: &[u8]) -> RuntimeResult<PointCloud2Xyz> {
128    let mut r = CdrReader::new(bytes)?;
129    r.expect_encap()?;
130    let stamp_sec = r.read_i32()?;
131    let stamp_nanosec = r.read_u32()?;
132    let frame_id = r.read_string()?;
133    let height = r.read_u32()?;
134    let width = r.read_u32()?;
135    let field_count = r.read_u32()?;
136    for _ in 0..field_count {
137        let _name = r.read_string()?;
138        let _offset = r.read_u32()?;
139        let _datatype = r.read_u8()?;
140        r.align(4);
141        let _count = r.read_u32()?;
142    }
143    let _is_bigendian = r.read_bool()?;
144    let point_step = r.read_u32()?;
145    let _row_step = r.read_u32()?;
146    let data_len = r.read_u32()? as usize;
147    let data = r.read_bytes(data_len)?;
148    let _is_dense = r.read_bool()?;
149    if height == 0 || width == 0 {
150        return Ok(PointCloud2Xyz { frame_id, stamp_sec, stamp_nanosec, xyz: Vec::new() });
151    }
152    if point_step < 12 {
153        return Err(RuntimeError::InvalidConfiguration(
154            "point_step must be at least 12 for XYZ".into(),
155        ));
156    }
157    let points = (height as usize).saturating_mul(width as usize);
158    let mut xyz = Vec::with_capacity(points * 3);
159    for i in 0..points {
160        let base = i * point_step as usize;
161        if base + 12 > data.len() {
162            return Err(RuntimeError::InvalidConfiguration(
163                "PointCloud2 data shorter than point_step layout".into(),
164            ));
165        }
166        xyz.push(f32::from_le_bytes(data[base..base + 4].try_into().unwrap()));
167        xyz.push(f32::from_le_bytes(data[base + 4..base + 8].try_into().unwrap()));
168        xyz.push(f32::from_le_bytes(data[base + 8..base + 12].try_into().unwrap()));
169    }
170    PointCloud2Xyz::try_new(frame_id, stamp_sec, stamp_nanosec, xyz)
171}
172
173fn write_point_field(w: &mut CdrWriter, name: &str, offset: u32) -> RuntimeResult<()> {
174    w.write_string(name)?;
175    w.write_u32(offset);
176    w.write_u8(7); // FLOAT32
177    w.align(4);
178    w.write_u32(1);
179    Ok(())
180}
181
182/// In-process loopback node for testing ROS-shaped publish/subscribe without rclrs.
183#[derive(Clone, Debug, Default)]
184pub struct LoopbackRos2Node {
185    topics: std::collections::BTreeMap<String, Vec<u8>>,
186}
187
188impl LoopbackRos2Node {
189    /// Creates an empty node.
190    #[must_use]
191    pub fn new() -> Self {
192        Self::default()
193    }
194
195    /// Publishes one CDR payload on a topic (replacing the previous sample).
196    pub fn publish(&mut self, topic: impl Into<String>, payload: Vec<u8>) {
197        self.topics.insert(topic.into(), payload);
198    }
199
200    /// Takes the latest payload for a topic, if any.
201    pub fn take(&mut self, topic: &str) -> Option<Vec<u8>> {
202        self.topics.remove(topic)
203    }
204
205    /// Returns whether a topic currently has a sample.
206    #[must_use]
207    pub fn has_topic(&self, topic: &str) -> bool {
208        self.topics.contains_key(topic)
209    }
210}
211
212struct CdrWriter {
213    buf: Vec<u8>,
214}
215
216impl CdrWriter {
217    fn new() -> Self {
218        Self { buf: Vec::new() }
219    }
220
221    fn into_bytes(self) -> Vec<u8> {
222        self.buf
223    }
224
225    fn write_encap(&mut self) {
226        self.buf.extend_from_slice(&CDR_LE_ENCAP);
227    }
228
229    fn align(&mut self, n: usize) {
230        while self.buf.len() % n != 0 {
231            self.buf.push(0);
232        }
233    }
234
235    fn write_u8(&mut self, v: u8) {
236        self.buf.push(v);
237    }
238
239    fn write_bool(&mut self, v: bool) {
240        self.align(1);
241        self.buf.push(u8::from(v));
242    }
243
244    fn write_i32(&mut self, v: i32) {
245        self.align(4);
246        self.buf.extend_from_slice(&v.to_le_bytes());
247    }
248
249    fn write_u32(&mut self, v: u32) {
250        self.align(4);
251        self.buf.extend_from_slice(&v.to_le_bytes());
252    }
253
254    fn write_bytes(&mut self, bytes: &[u8]) {
255        self.buf.extend_from_slice(bytes);
256    }
257
258    fn write_string(&mut self, value: &str) -> RuntimeResult<()> {
259        if value.len() >= u32::MAX as usize {
260            return Err(RuntimeError::InvalidConfiguration("string too long".into()));
261        }
262        self.align(4);
263        // ROS CDR strings include the trailing NUL in the length.
264        let len = (value.len() + 1) as u32;
265        self.write_u32(len);
266        self.buf.extend_from_slice(value.as_bytes());
267        self.buf.push(0);
268        Ok(())
269    }
270}
271
272struct CdrReader<'a> {
273    buf: &'a [u8],
274    pos: usize,
275}
276
277impl<'a> CdrReader<'a> {
278    fn new(buf: &'a [u8]) -> RuntimeResult<Self> {
279        if buf.len() < 4 {
280            return Err(RuntimeError::InvalidConfiguration("CDR buffer too short".into()));
281        }
282        Ok(Self { buf, pos: 0 })
283    }
284
285    fn expect_encap(&mut self) -> RuntimeResult<()> {
286        if self.buf.len() < 4 || self.buf[..4] != CDR_LE_ENCAP {
287            return Err(RuntimeError::InvalidConfiguration(
288                "expected ROS 2 CDR little-endian encapsulation".into(),
289            ));
290        }
291        self.pos = 4;
292        Ok(())
293    }
294
295    fn align(&mut self, n: usize) {
296        let rem = self.pos % n;
297        if rem != 0 {
298            self.pos += n - rem;
299        }
300    }
301
302    fn read_u8(&mut self) -> RuntimeResult<u8> {
303        let v = *self
304            .buf
305            .get(self.pos)
306            .ok_or_else(|| RuntimeError::InvalidConfiguration("CDR truncated".into()))?;
307        self.pos += 1;
308        Ok(v)
309    }
310
311    fn read_bool(&mut self) -> RuntimeResult<bool> {
312        Ok(self.read_u8()? != 0)
313    }
314
315    fn read_i32(&mut self) -> RuntimeResult<i32> {
316        self.align(4);
317        let bytes = self.read_exact(4)?;
318        Ok(i32::from_le_bytes(bytes.try_into().unwrap()))
319    }
320
321    fn read_u32(&mut self) -> RuntimeResult<u32> {
322        self.align(4);
323        let bytes = self.read_exact(4)?;
324        Ok(u32::from_le_bytes(bytes.try_into().unwrap()))
325    }
326
327    fn read_bytes(&mut self, len: usize) -> RuntimeResult<&'a [u8]> {
328        let end = self
329            .pos
330            .checked_add(len)
331            .ok_or_else(|| RuntimeError::InvalidConfiguration("CDR overflow".into()))?;
332        if end > self.buf.len() {
333            return Err(RuntimeError::InvalidConfiguration("CDR truncated".into()));
334        }
335        let slice = &self.buf[self.pos..end];
336        self.pos = end;
337        Ok(slice)
338    }
339
340    fn read_exact(&mut self, len: usize) -> RuntimeResult<&'a [u8]> {
341        self.read_bytes(len)
342    }
343
344    fn read_string(&mut self) -> RuntimeResult<String> {
345        let len = self.read_u32()? as usize;
346        if len == 0 {
347            return Err(RuntimeError::InvalidConfiguration(
348                "CDR string length must include NUL".into(),
349            ));
350        }
351        let bytes = self.read_bytes(len)?;
352        if bytes.last().copied() != Some(0) {
353            return Err(RuntimeError::InvalidConfiguration(
354                "CDR string missing NUL terminator".into(),
355            ));
356        }
357        String::from_utf8(bytes[..len - 1].to_vec())
358            .map_err(|_| RuntimeError::InvalidConfiguration("CDR string is not UTF-8".into()))
359    }
360}
361
362#[cfg(test)]
363mod tests {
364    use super::{
365        decode_point_cloud2_xyz, encode_point_cloud2_xyz, CatalogRos2Adapter, LoopbackRos2Node,
366        PointCloud2Xyz, Ros2Adapter, POINT_CLOUD2_TYPE,
367    };
368
369    #[test]
370    fn negotiates_point_cloud2() {
371        let adapter = CatalogRos2Adapter::point_cloud2_xyz();
372        assert_eq!(adapter.negotiate(POINT_CLOUD2_TYPE).unwrap().spatial_topic, "point/xyz");
373    }
374
375    #[test]
376    fn roundtrips_xyz_cdr_and_loopback() {
377        let msg =
378            PointCloud2Xyz::try_new("lidar", 1, 2, vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0]).unwrap();
379        let bytes = encode_point_cloud2_xyz(&msg).unwrap();
380        let mut node = LoopbackRos2Node::new();
381        node.publish("/points", bytes.clone());
382        let taken = node.take("/points").unwrap();
383        let decoded = decode_point_cloud2_xyz(&taken).unwrap();
384        assert_eq!(decoded, msg);
385    }
386}
spatialrust_runtime/ros2.rs

spatialrust_runtime/
ros2.rs
