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/// Canonical ROS 2 type name for TFMessage.
63pub const TF_MESSAGE_TYPE: &str = "tf2_msgs/msg/TFMessage";
64
65/// CDR encapsulation header for little-endian ROS 2 messages.
66const CDR_LE_ENCAP: [u8; 4] = [0x00, 0x01, 0x00, 0x00];
67
68/// Interleaved XYZ or XYZ-I PointCloud2 payload.
69#[derive(Clone, Debug, PartialEq)]
70pub struct PointCloud2Xyz {
71    /// ROS frame id.
72    pub frame_id: String,
73    /// Header stamp seconds.
74    pub stamp_sec: i32,
75    /// Header stamp nanoseconds.
76    pub stamp_nanosec: u32,
77    /// Interleaved XYZ floats.
78    pub xyz: Vec<f32>,
79    /// Optional per-point float32 LiDAR intensity values.
80    pub intensity: Option<Vec<f32>>,
81}
82
83/// One `geometry_msgs/msg/TransformStamped` decoded from a ROS 2 TFMessage.
84#[derive(Clone, Debug, PartialEq)]
85pub struct TfTransform {
86    /// Header timestamp seconds.
87    pub stamp_sec: i32,
88    /// Header timestamp nanoseconds.
89    pub stamp_nanosec: u32,
90    /// Parent coordinate frame.
91    pub frame_id: String,
92    /// Child coordinate frame.
93    pub child_frame_id: String,
94    /// Translation in meters as x/y/z.
95    pub translation: [f64; 3],
96    /// Quaternion rotation as x/y/z/w.
97    pub rotation_xyzw: [f64; 4],
98}
99
100impl PointCloud2Xyz {
101    /// Creates a validated XYZ cloud (`xyz.len()` divisible by 3).
102    pub fn try_new(
103        frame_id: impl Into<String>,
104        stamp_sec: i32,
105        stamp_nanosec: u32,
106        xyz: Vec<f32>,
107    ) -> RuntimeResult<Self> {
108        if xyz.len() % 3 != 0 {
109            return Err(RuntimeError::InvalidConfiguration(
110                "xyz length must be a multiple of 3".into(),
111            ));
112        }
113        Ok(Self { frame_id: frame_id.into(), stamp_sec, stamp_nanosec, xyz, intensity: None })
114    }
115
116    /// Creates a validated XYZ-I cloud.
117    pub fn try_new_with_intensity(
118        frame_id: impl Into<String>,
119        stamp_sec: i32,
120        stamp_nanosec: u32,
121        xyz: Vec<f32>,
122        intensity: Vec<f32>,
123    ) -> RuntimeResult<Self> {
124        Self::try_new_with_optional_intensity(
125            frame_id,
126            stamp_sec,
127            stamp_nanosec,
128            xyz,
129            Some(intensity),
130        )
131    }
132
133    fn try_new_with_optional_intensity(
134        frame_id: impl Into<String>,
135        stamp_sec: i32,
136        stamp_nanosec: u32,
137        xyz: Vec<f32>,
138        intensity: Option<Vec<f32>>,
139    ) -> RuntimeResult<Self> {
140        if xyz.len() % 3 != 0 {
141            return Err(RuntimeError::InvalidConfiguration(
142                "xyz length must be a multiple of 3".into(),
143            ));
144        }
145        if intensity.as_ref().is_some_and(|values| values.len() != xyz.len() / 3) {
146            return Err(RuntimeError::InvalidConfiguration(
147                "intensity length must match the XYZ point count".into(),
148            ));
149        }
150        Ok(Self { frame_id: frame_id.into(), stamp_sec, stamp_nanosec, xyz, intensity })
151    }
152
153    /// Returns point count.
154    #[must_use]
155    pub fn point_count(&self) -> usize {
156        self.xyz.len() / 3
157    }
158}
159
160/// Encodes an XYZ or XYZ-I PointCloud2 as ROS 2 CDR little-endian bytes.
161pub fn encode_point_cloud2_xyz(msg: &PointCloud2Xyz) -> RuntimeResult<Vec<u8>> {
162    let mut w = CdrWriter::new();
163    w.write_encap();
164    w.write_i32(msg.stamp_sec);
165    w.write_u32(msg.stamp_nanosec);
166    w.write_string(&msg.frame_id)?;
167    let width = msg.point_count() as u32;
168    w.write_u32(1); // height
169    w.write_u32(width);
170    let intensity = msg.intensity.as_deref();
171    if intensity.is_some_and(|values| values.len() != msg.point_count()) {
172        return Err(RuntimeError::InvalidConfiguration(
173            "intensity length must match the XYZ point count".into(),
174        ));
175    }
176    w.write_u32(if intensity.is_some() { 4 } else { 3 }); // fields length
177    write_point_field(&mut w, "x", 0)?;
178    write_point_field(&mut w, "y", 4)?;
179    write_point_field(&mut w, "z", 8)?;
180    if intensity.is_some() {
181        write_point_field(&mut w, "intensity", 12)?;
182    }
183    w.write_bool(false); // is_bigendian
184    let point_step = if intensity.is_some() { 16 } else { 12 };
185    w.write_u32(point_step); // point_step
186    w.write_u32(width.saturating_mul(point_step)); // row_step
187    let mut data = Vec::with_capacity(msg.point_count() * point_step as usize);
188    for (index, point) in msg.xyz.chunks_exact(3).enumerate() {
189        for value in point {
190            data.extend_from_slice(&value.to_le_bytes());
191        }
192        if let Some(intensity) = intensity {
193            data.extend_from_slice(&intensity[index].to_le_bytes());
194        }
195    }
196    let data_len = u32::try_from(data.len())
197        .map_err(|_| RuntimeError::InvalidConfiguration("PointCloud2 data is too large".into()))?;
198    w.write_u32(data_len);
199    w.write_bytes(&data);
200    w.write_bool(true); // is_dense
201    Ok(w.into_bytes())
202}
203
204/// Inspects a PointCloud2 CDR header without materializing its point data.
205pub fn point_cloud2_has_intensity(bytes: &[u8]) -> RuntimeResult<bool> {
206    let mut r = CdrReader::new(bytes)?;
207    r.expect_encap()?;
208    let _stamp_sec = r.read_i32()?;
209    let _stamp_nanosec = r.read_u32()?;
210    let _frame_id = r.read_string()?;
211    let _height = r.read_u32()?;
212    let _width = r.read_u32()?;
213    let field_count = r.read_u32()?;
214    let mut has_intensity = false;
215    for _ in 0..field_count {
216        let name = r.read_string()?;
217        let _offset = r.read_u32()?;
218        let datatype = r.read_u8()?;
219        r.align(4);
220        let count = r.read_u32()?;
221        if name.eq_ignore_ascii_case("intensity") {
222            if count != 1 || datatype != 7 {
223                return Err(RuntimeError::InvalidConfiguration(
224                    "PointCloud2 intensity must be a scalar float32 field".into(),
225                ));
226            }
227            has_intensity = true;
228        }
229    }
230    Ok(has_intensity)
231}
232
233/// Decodes a ROS 2 CDR little-endian `tf2_msgs/msg/TFMessage` payload.
234///
235/// The transform message uses the ROS 2 CDR alignment origin immediately
236/// after the four-byte encapsulation header for its nested float64 fields.
237/// This function preserves message order and does not compose or otherwise
238/// interpret the frame edges.
239pub fn decode_tf_message(bytes: &[u8]) -> RuntimeResult<Vec<TfTransform>> {
240    let mut reader = CdrReader::new(bytes)?;
241    reader.expect_encap()?;
242    let count = reader.read_u32()? as usize;
243    let minimum_bytes_per_transform = 8_usize;
244    if count > bytes.len().saturating_sub(8) / minimum_bytes_per_transform {
245        return Err(RuntimeError::InvalidConfiguration(
246            "TFMessage transform count exceeds the payload bound".into(),
247        ));
248    }
249    let mut transforms = Vec::with_capacity(count);
250    for _ in 0..count {
251        let stamp_sec = reader.read_i32()?;
252        let stamp_nanosec = reader.read_u32()?;
253        if stamp_nanosec >= 1_000_000_000 {
254            return Err(RuntimeError::InvalidConfiguration(
255                "TF transform nanosecond field is outside [0, 1_000_000_000)".into(),
256            ));
257        }
258        let frame_id = reader.read_string()?;
259        let child_frame_id = reader.read_string()?;
260        if frame_id.is_empty() || child_frame_id.is_empty() {
261            return Err(RuntimeError::InvalidConfiguration(
262                "TF transform frame identifiers must not be empty".into(),
263            ));
264        }
265        let translation =
266            [reader.read_f64_from(4)?, reader.read_f64_from(4)?, reader.read_f64_from(4)?];
267        let rotation_xyzw = [
268            reader.read_f64_from(4)?,
269            reader.read_f64_from(4)?,
270            reader.read_f64_from(4)?,
271            reader.read_f64_from(4)?,
272        ];
273        transforms.push(TfTransform {
274            stamp_sec,
275            stamp_nanosec,
276            frame_id,
277            child_frame_id,
278            translation,
279            rotation_xyzw,
280        });
281    }
282    reader.require_end()?;
283    Ok(transforms)
284}
285
286/// Decodes an XYZ or XYZ-I PointCloud2 from ROS 2 CDR little-endian bytes.
287pub fn decode_point_cloud2_xyz(bytes: &[u8]) -> RuntimeResult<PointCloud2Xyz> {
288    let mut r = CdrReader::new(bytes)?;
289    r.expect_encap()?;
290    let stamp_sec = r.read_i32()?;
291    let stamp_nanosec = r.read_u32()?;
292    let frame_id = r.read_string()?;
293    let height = r.read_u32()?;
294    let width = r.read_u32()?;
295    let field_count = r.read_u32()?;
296    let mut x_offset = None;
297    let mut y_offset = None;
298    let mut z_offset = None;
299    let mut intensity_offset = None;
300    for _ in 0..field_count {
301        let name = r.read_string()?;
302        let offset = r.read_u32()?;
303        let datatype = r.read_u8()?;
304        r.align(4);
305        let count = r.read_u32()?;
306        match name.to_ascii_lowercase().as_str() {
307            "x" if count == 1 && datatype == 7 => x_offset = Some(offset),
308            "y" if count == 1 && datatype == 7 => y_offset = Some(offset),
309            "z" if count == 1 && datatype == 7 => z_offset = Some(offset),
310            "intensity" => {
311                if count != 1 || datatype != 7 {
312                    return Err(RuntimeError::InvalidConfiguration(
313                        "PointCloud2 intensity must be a scalar float32 field".into(),
314                    ));
315                }
316                intensity_offset = Some(offset);
317            }
318            _ => {}
319        }
320    }
321    let is_bigendian = r.read_bool()?;
322    let point_step = r.read_u32()?;
323    let row_step = r.read_u32()?;
324    let data_len = r.read_u32()? as usize;
325    let data = r.read_bytes(data_len)?;
326    let _is_dense = r.read_bool()?;
327    if height == 0 || width == 0 {
328        return PointCloud2Xyz::try_new_with_optional_intensity(
329            frame_id,
330            stamp_sec,
331            stamp_nanosec,
332            Vec::new(),
333            intensity_offset.map(|_| Vec::new()),
334        );
335    }
336    let x_offset = x_offset.ok_or_else(|| {
337        RuntimeError::InvalidConfiguration("PointCloud2 is missing float32 x field".into())
338    })?;
339    let y_offset = y_offset.ok_or_else(|| {
340        RuntimeError::InvalidConfiguration("PointCloud2 is missing float32 y field".into())
341    })?;
342    let z_offset = z_offset.ok_or_else(|| {
343        RuntimeError::InvalidConfiguration("PointCloud2 is missing float32 z field".into())
344    })?;
345    let last_offset = [x_offset, y_offset, z_offset]
346        .into_iter()
347        .chain(intensity_offset)
348        .max()
349        .expect("XYZ offsets are present");
350    if u64::from(point_step) < u64::from(last_offset) + 4 {
351        return Err(RuntimeError::InvalidConfiguration(
352            "PointCloud2 point_step does not contain the declared fields".into(),
353        ));
354    }
355    let height = height as usize;
356    let width = width as usize;
357    let point_step = point_step as usize;
358    let row_step = row_step as usize;
359    let row_bytes = width.checked_mul(point_step).ok_or_else(|| {
360        RuntimeError::InvalidConfiguration("PointCloud2 row size overflow".into())
361    })?;
362    if row_step < row_bytes {
363        return Err(RuntimeError::InvalidConfiguration(
364            "PointCloud2 row_step is shorter than one row".into(),
365        ));
366    }
367    let required_bytes = row_step.checked_mul(height).ok_or_else(|| {
368        RuntimeError::InvalidConfiguration("PointCloud2 data size overflow".into())
369    })?;
370    if required_bytes > data.len() {
371        return Err(RuntimeError::InvalidConfiguration(
372            "PointCloud2 data is shorter than row_step × height".into(),
373        ));
374    }
375    let points = height.checked_mul(width).ok_or_else(|| {
376        RuntimeError::InvalidConfiguration("PointCloud2 point count overflow".into())
377    })?;
378    let xyz_capacity = points.checked_mul(3).ok_or_else(|| {
379        RuntimeError::InvalidConfiguration("PointCloud2 XYZ capacity overflow".into())
380    })?;
381    let mut xyz = Vec::with_capacity(xyz_capacity);
382    let mut intensity = intensity_offset.map(|_| Vec::with_capacity(points));
383    for row in 0..height {
384        for column in 0..width {
385            let base = row
386                .checked_mul(row_step)
387                .and_then(|value| value.checked_add(column.checked_mul(point_step)?))
388                .ok_or_else(|| {
389                    RuntimeError::InvalidConfiguration("PointCloud2 point offset overflow".into())
390                })?;
391            for offset in [x_offset, y_offset, z_offset] {
392                xyz.push(read_point_f32(data, base, offset, is_bigendian)?);
393            }
394            if let (Some(intensity), Some(offset)) = (&mut intensity, intensity_offset) {
395                intensity.push(read_point_f32(data, base, offset, is_bigendian)?);
396            }
397        }
398    }
399    PointCloud2Xyz::try_new_with_optional_intensity(
400        frame_id,
401        stamp_sec,
402        stamp_nanosec,
403        xyz,
404        intensity,
405    )
406}
407
408fn read_point_f32(data: &[u8], base: usize, offset: u32, is_bigendian: bool) -> RuntimeResult<f32> {
409    let start = base.checked_add(offset as usize).ok_or_else(|| {
410        RuntimeError::InvalidConfiguration("PointCloud2 field offset overflow".into())
411    })?;
412    let end = start.checked_add(4).ok_or_else(|| {
413        RuntimeError::InvalidConfiguration("PointCloud2 field end overflow".into())
414    })?;
415    if end > data.len() {
416        return Err(RuntimeError::InvalidConfiguration(
417            "PointCloud2 data shorter than field layout".into(),
418        ));
419    }
420    let bytes = data[start..end].try_into().unwrap();
421    Ok(if is_bigendian { f32::from_be_bytes(bytes) } else { f32::from_le_bytes(bytes) })
422}
423
424fn write_point_field(w: &mut CdrWriter, name: &str, offset: u32) -> RuntimeResult<()> {
425    w.write_string(name)?;
426    w.write_u32(offset);
427    w.write_u8(7); // FLOAT32
428    w.align(4);
429    w.write_u32(1);
430    Ok(())
431}
432
433/// In-process loopback node for testing ROS-shaped publish/subscribe without rclrs.
434#[derive(Clone, Debug, Default)]
435pub struct LoopbackRos2Node {
436    topics: std::collections::BTreeMap<String, Vec<u8>>,
437}
438
439impl LoopbackRos2Node {
440    /// Creates an empty node.
441    #[must_use]
442    pub fn new() -> Self {
443        Self::default()
444    }
445
446    /// Publishes one CDR payload on a topic (replacing the previous sample).
447    pub fn publish(&mut self, topic: impl Into<String>, payload: Vec<u8>) {
448        self.topics.insert(topic.into(), payload);
449    }
450
451    /// Takes the latest payload for a topic, if any.
452    pub fn take(&mut self, topic: &str) -> Option<Vec<u8>> {
453        self.topics.remove(topic)
454    }
455
456    /// Returns whether a topic currently has a sample.
457    #[must_use]
458    pub fn has_topic(&self, topic: &str) -> bool {
459        self.topics.contains_key(topic)
460    }
461}
462
463struct CdrWriter {
464    buf: Vec<u8>,
465}
466
467impl CdrWriter {
468    fn new() -> Self {
469        Self { buf: Vec::new() }
470    }
471
472    fn into_bytes(self) -> Vec<u8> {
473        self.buf
474    }
475
476    fn write_encap(&mut self) {
477        self.buf.extend_from_slice(&CDR_LE_ENCAP);
478    }
479
480    fn align(&mut self, n: usize) {
481        while self.buf.len() % n != 0 {
482            self.buf.push(0);
483        }
484    }
485
486    #[cfg(test)]
487    fn align_from(&mut self, n: usize, origin: usize) {
488        let relative = self.buf.len().saturating_sub(origin);
489        let rem = relative % n;
490        if rem != 0 {
491            self.buf.resize(self.buf.len() + n - rem, 0);
492        }
493    }
494
495    fn write_u8(&mut self, v: u8) {
496        self.buf.push(v);
497    }
498
499    fn write_bool(&mut self, v: bool) {
500        self.align(1);
501        self.buf.push(u8::from(v));
502    }
503
504    fn write_i32(&mut self, v: i32) {
505        self.align(4);
506        self.buf.extend_from_slice(&v.to_le_bytes());
507    }
508
509    fn write_u32(&mut self, v: u32) {
510        self.align(4);
511        self.buf.extend_from_slice(&v.to_le_bytes());
512    }
513
514    #[cfg(test)]
515    fn write_f64(&mut self, v: f64) {
516        self.buf.extend_from_slice(&v.to_le_bytes());
517    }
518
519    fn write_bytes(&mut self, bytes: &[u8]) {
520        self.buf.extend_from_slice(bytes);
521    }
522
523    fn write_string(&mut self, value: &str) -> RuntimeResult<()> {
524        if value.len() >= u32::MAX as usize {
525            return Err(RuntimeError::InvalidConfiguration("string too long".into()));
526        }
527        self.align(4);
528        // ROS CDR strings include the trailing NUL in the length.
529        let len = (value.len() + 1) as u32;
530        self.write_u32(len);
531        self.buf.extend_from_slice(value.as_bytes());
532        self.buf.push(0);
533        Ok(())
534    }
535}
536
537struct CdrReader<'a> {
538    buf: &'a [u8],
539    pos: usize,
540}
541
542impl<'a> CdrReader<'a> {
543    fn new(buf: &'a [u8]) -> RuntimeResult<Self> {
544        if buf.len() < 4 {
545            return Err(RuntimeError::InvalidConfiguration("CDR buffer too short".into()));
546        }
547        Ok(Self { buf, pos: 0 })
548    }
549
550    fn expect_encap(&mut self) -> RuntimeResult<()> {
551        if self.buf.len() < 4 || self.buf[..4] != CDR_LE_ENCAP {
552            return Err(RuntimeError::InvalidConfiguration(
553                "expected ROS 2 CDR little-endian encapsulation".into(),
554            ));
555        }
556        self.pos = 4;
557        Ok(())
558    }
559
560    fn align(&mut self, n: usize) {
561        let rem = self.pos % n;
562        if rem != 0 {
563            self.pos += n - rem;
564        }
565    }
566
567    fn align_from(&mut self, n: usize, origin: usize) {
568        let relative = self.pos.saturating_sub(origin);
569        let rem = relative % n;
570        if rem != 0 {
571            self.pos += n - rem;
572        }
573    }
574
575    fn read_u8(&mut self) -> RuntimeResult<u8> {
576        let v = *self
577            .buf
578            .get(self.pos)
579            .ok_or_else(|| RuntimeError::InvalidConfiguration("CDR truncated".into()))?;
580        self.pos += 1;
581        Ok(v)
582    }
583
584    fn read_bool(&mut self) -> RuntimeResult<bool> {
585        Ok(self.read_u8()? != 0)
586    }
587
588    fn read_i32(&mut self) -> RuntimeResult<i32> {
589        self.align(4);
590        let bytes = self.read_exact(4)?;
591        Ok(i32::from_le_bytes(bytes.try_into().unwrap()))
592    }
593
594    fn read_u32(&mut self) -> RuntimeResult<u32> {
595        self.align(4);
596        let bytes = self.read_exact(4)?;
597        Ok(u32::from_le_bytes(bytes.try_into().unwrap()))
598    }
599
600    fn read_f64_from(&mut self, origin: usize) -> RuntimeResult<f64> {
601        self.align_from(8, origin);
602        let bytes = self.read_exact(8)?;
603        Ok(f64::from_le_bytes(bytes.try_into().unwrap()))
604    }
605
606    fn read_bytes(&mut self, len: usize) -> RuntimeResult<&'a [u8]> {
607        let end = self
608            .pos
609            .checked_add(len)
610            .ok_or_else(|| RuntimeError::InvalidConfiguration("CDR overflow".into()))?;
611        if end > self.buf.len() {
612            return Err(RuntimeError::InvalidConfiguration("CDR truncated".into()));
613        }
614        let slice = &self.buf[self.pos..end];
615        self.pos = end;
616        Ok(slice)
617    }
618
619    fn read_exact(&mut self, len: usize) -> RuntimeResult<&'a [u8]> {
620        self.read_bytes(len)
621    }
622
623    fn read_string(&mut self) -> RuntimeResult<String> {
624        let len = self.read_u32()? as usize;
625        if len == 0 {
626            return Err(RuntimeError::InvalidConfiguration(
627                "CDR string length must include NUL".into(),
628            ));
629        }
630        let bytes = self.read_bytes(len)?;
631        if bytes.last().copied() != Some(0) {
632            return Err(RuntimeError::InvalidConfiguration(
633                "CDR string missing NUL terminator".into(),
634            ));
635        }
636        String::from_utf8(bytes[..len - 1].to_vec())
637            .map_err(|_| RuntimeError::InvalidConfiguration("CDR string is not UTF-8".into()))
638    }
639
640    fn require_end(&self) -> RuntimeResult<()> {
641        if self.pos != self.buf.len() {
642            return Err(RuntimeError::InvalidConfiguration(format!(
643                "CDR payload has {} trailing bytes",
644                self.buf.len().saturating_sub(self.pos)
645            )));
646        }
647        Ok(())
648    }
649}
650
651#[cfg(test)]
652mod tests {
653    use super::{
654        decode_point_cloud2_xyz, decode_tf_message, encode_point_cloud2_xyz,
655        point_cloud2_has_intensity, write_point_field, CatalogRos2Adapter, CdrWriter,
656        LoopbackRos2Node, PointCloud2Xyz, Ros2Adapter, TfTransform, POINT_CLOUD2_TYPE,
657    };
658
659    #[test]
660    fn negotiates_point_cloud2() {
661        let adapter = CatalogRos2Adapter::point_cloud2_xyz();
662        assert_eq!(adapter.negotiate(POINT_CLOUD2_TYPE).unwrap().spatial_topic, "point/xyz");
663    }
664
665    #[test]
666    fn roundtrips_xyz_cdr_and_loopback() {
667        let msg =
668            PointCloud2Xyz::try_new("lidar", 1, 2, vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0]).unwrap();
669        let bytes = encode_point_cloud2_xyz(&msg).unwrap();
670        assert!(!point_cloud2_has_intensity(&bytes).unwrap());
671        let mut node = LoopbackRos2Node::new();
672        node.publish("/points", bytes.clone());
673        let taken = node.take("/points").unwrap();
674        let decoded = decode_point_cloud2_xyz(&taken).unwrap();
675        assert_eq!(decoded, msg);
676    }
677
678    #[test]
679    fn roundtrips_xyzi_cdr_and_loopback() {
680        let msg = PointCloud2Xyz::try_new_with_intensity(
681            "lidar",
682            1,
683            2,
684            vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
685            vec![10.0, 20.0],
686        )
687        .unwrap();
688        let bytes = encode_point_cloud2_xyz(&msg).unwrap();
689        assert!(point_cloud2_has_intensity(&bytes).unwrap());
690        let decoded = decode_point_cloud2_xyz(&bytes).unwrap();
691        assert_eq!(decoded, msg);
692    }
693
694    #[test]
695    fn decodes_xyz_offsets_and_row_padding() {
696        let mut writer = CdrWriter::new();
697        writer.write_encap();
698        writer.write_i32(3);
699        writer.write_u32(4);
700        writer.write_string("padded_lidar").unwrap();
701        writer.write_u32(2); // height
702        writer.write_u32(2); // width
703        writer.write_u32(4); // fields
704        write_point_field(&mut writer, "z", 8).unwrap();
705        write_point_field(&mut writer, "intensity", 12).unwrap();
706        write_point_field(&mut writer, "x", 0).unwrap();
707        write_point_field(&mut writer, "y", 4).unwrap();
708        writer.write_bool(false);
709        writer.write_u32(16); // point_step
710        writer.write_u32(40); // row_step includes eight bytes of row padding
711
712        let mut data = Vec::new();
713        for row in 0..2 {
714            for column in 0..2 {
715                let base = (row * 2 + column) as f32;
716                for value in [base + 1.0, base + 2.0, base + 3.0, base + 4.0] {
717                    data.extend_from_slice(&value.to_le_bytes());
718                }
719            }
720            data.extend_from_slice(&[0; 8]);
721        }
722        writer.write_u32(data.len() as u32);
723        writer.write_bytes(&data);
724        writer.write_bool(true);
725
726        let decoded = decode_point_cloud2_xyz(&writer.into_bytes()).unwrap();
727        assert_eq!(decoded.frame_id, "padded_lidar");
728        assert_eq!(decoded.stamp_sec, 3);
729        assert_eq!(decoded.stamp_nanosec, 4);
730        assert_eq!(decoded.xyz, vec![1.0, 2.0, 3.0, 2.0, 3.0, 4.0, 3.0, 4.0, 5.0, 4.0, 5.0, 6.0,]);
731        assert_eq!(decoded.intensity, Some(vec![4.0, 5.0, 6.0, 7.0]));
732    }
733
734    fn encode_tf_message(transforms: &[TfTransform]) -> Vec<u8> {
735        let mut writer = CdrWriter::new();
736        writer.write_encap();
737        writer.write_u32(transforms.len() as u32);
738        for transform in transforms {
739            writer.write_i32(transform.stamp_sec);
740            writer.write_u32(transform.stamp_nanosec);
741            writer.write_string(&transform.frame_id).unwrap();
742            writer.write_string(&transform.child_frame_id).unwrap();
743            writer.align_from(8, 4);
744            for value in transform.translation.into_iter().chain(transform.rotation_xyzw) {
745                writer.write_f64(value);
746            }
747        }
748        writer.into_bytes()
749    }
750
751    #[test]
752    fn decodes_tf_message_with_nested_float64_alignment() {
753        let expected = vec![TfTransform {
754            stamp_sec: 12,
755            stamp_nanosec: 34,
756            frame_id: "base_link".into(),
757            child_frame_id: "lidar_front".into(),
758            translation: [1.0, -2.0, 3.5],
759            rotation_xyzw: [0.0, 0.0, 0.707, 0.707],
760        }];
761        assert_eq!(decode_tf_message(&encode_tf_message(&expected)).unwrap(), expected);
762    }
763
764    #[test]
765    fn rejects_invalid_tf_message_timestamp_and_trailing_bytes() {
766        let transform = TfTransform {
767            stamp_sec: 0,
768            stamp_nanosec: 1_000_000_000,
769            frame_id: "base".into(),
770            child_frame_id: "sensor".into(),
771            translation: [0.0; 3],
772            rotation_xyzw: [0.0, 0.0, 0.0, 1.0],
773        };
774        let error = decode_tf_message(&encode_tf_message(&[transform])).unwrap_err();
775        assert!(error.to_string().contains("nanosecond"));
776
777        let valid = TfTransform {
778            stamp_sec: 0,
779            stamp_nanosec: 0,
780            frame_id: "base".into(),
781            child_frame_id: "sensor".into(),
782            translation: [0.0; 3],
783            rotation_xyzw: [0.0, 0.0, 0.0, 1.0],
784        };
785        let mut bytes = encode_tf_message(&[valid]);
786        bytes.push(1);
787        let error = decode_tf_message(&bytes).unwrap_err();
788        assert!(error.to_string().contains("trailing"));
789    }
790}
spatialrust_runtime/ros2.rs

spatialrust_runtime/
ros2.rs
