edgefirst_schemas/
std_msgs.rs

1// SPDX-License-Identifier: Apache-2.0
2// Copyright © 2025 Au-Zone Technologies. All Rights Reserved.
3
4//! ROS 2 `std_msgs` message types.
5//!
6//! - `ColorRGBA` — CdrFixed, 16 bytes (4 × f32)
7//! - `Header` — buffer-backed, contains stamp + frame_id
8
9use crate::builtin_interfaces::Time;
10use crate::cdr::*;
11
12// ── CdrFixed types ──────────────────────────────────────────────────
13
14#[derive(PartialEq, Clone, Copy, Debug)]
15pub struct ColorRGBA {
16    pub r: f32,
17    pub g: f32,
18    pub b: f32,
19    pub a: f32,
20}
21
22impl CdrFixed for ColorRGBA {
23    const CDR_SIZE: usize = 16; // 4 x f32
24    fn read_cdr(cursor: &mut CdrCursor<'_>) -> Result<Self, CdrError> {
25        Ok(ColorRGBA {
26            r: cursor.read_f32()?,
27            g: cursor.read_f32()?,
28            b: cursor.read_f32()?,
29            a: cursor.read_f32()?,
30        })
31    }
32    fn write_cdr(&self, writer: &mut CdrWriter<'_>) {
33        writer.write_f32(self.r);
34        writer.write_f32(self.g);
35        writer.write_f32(self.b);
36        writer.write_f32(self.a);
37    }
38    fn size_cdr(sizer: &mut CdrSizer) {
39        sizer.size_f32();
40        sizer.size_f32();
41        sizer.size_f32();
42        sizer.size_f32();
43    }
44}
45
46// ── Buffer-backed types ─────────────────────────────────────────────
47
48// CDR layout (after 4-byte CDR LE header):
49//   4: i32 stamp.sec
50//   8: u32 stamp.nanosec
51//  12: u32 frame_id_len (incl NUL)
52//  16: [u8] frame_id bytes + NUL
53//  offsets[0] = end of frame_id
54
55pub struct Header<B> {
56    buf: B,
57    offsets: [usize; 1],
58}
59
60impl<B> Header<B> {
61    /// Convert the buffer type without re-parsing the offset table.
62    #[inline]
63    pub fn map_buffer<C>(self, f: impl FnOnce(B) -> C) -> Header<C> {
64        Header {
65            buf: f(self.buf),
66            offsets: self.offsets,
67        }
68    }
69}
70
71impl<B: AsRef<[u8]>> Header<B> {
72    pub fn from_cdr(buf: B) -> Result<Self, CdrError> {
73        let mut c = CdrCursor::new(buf.as_ref())?;
74        // skip stamp (8 bytes)
75        c.skip(8)?;
76        let _ = c.read_string()?;
77        Ok(Header {
78            offsets: [c.offset()],
79            buf,
80        })
81    }
82
83    pub fn stamp(&self) -> Time {
84        rd_time(self.buf.as_ref(), CDR_HEADER_SIZE)
85    }
86
87    pub fn frame_id(&self) -> &str {
88        rd_string(self.buf.as_ref(), CDR_HEADER_SIZE + 8).0
89    }
90
91    /// Byte position after the last field (useful for composite types).
92    pub fn end_offset(&self) -> usize {
93        self.offsets[0]
94    }
95
96    pub fn as_cdr(&self) -> &[u8] {
97        self.buf.as_ref()
98    }
99
100    pub fn cdr_size(&self) -> usize {
101        self.buf.as_ref().len()
102    }
103
104    pub fn to_cdr(&self) -> Vec<u8> {
105        self.buf.as_ref().to_vec()
106    }
107}
108
109impl Header<Vec<u8>> {
110    #[deprecated(
111        since = "3.2.0",
112        note = "use Header::builder() for allocation-free buffer reuse; Header::new will be removed in 4.0"
113    )]
114    pub fn new(stamp: Time, frame_id: &str) -> Result<Self, CdrError> {
115        let mut sizer = CdrSizer::new();
116        Time::size_cdr(&mut sizer);
117        sizer.size_string(frame_id);
118
119        let mut buf = vec![0u8; sizer.size()];
120        let mut w = CdrWriter::new(&mut buf)?;
121        stamp.write_cdr(&mut w);
122        w.write_string(frame_id);
123        let offsets = [w.offset()];
124        w.finish()?;
125        Ok(Header { buf, offsets })
126    }
127
128    pub fn into_cdr(self) -> Vec<u8> {
129        self.buf
130    }
131
132    /// Start a new `HeaderBuilder` with zero-valued defaults.
133    ///
134    /// Returned with a generic lifetime parameter `'a` so the compiler
135    /// infers it from subsequent setter calls — `.frame_id(&local_str)`
136    /// binds `'a` to the caller's scope without forcing `'static`.
137    pub fn builder<'a>() -> HeaderBuilder<'a> {
138        HeaderBuilder::new()
139    }
140}
141
142// ── HeaderBuilder<'a> ───────────────────────────────────────────────
143
144/// Builder for `Header<Vec<u8>>` with buffer-reuse finalizers.
145///
146/// `frame_id` uses `Cow<'a, str>` so that `frame_id("lit")` borrows a
147/// `&'static str` and `frame_id(owned)` takes ownership. The borrow must
148/// remain valid until `build()`, `encode_into_vec()`, or
149/// `encode_into_slice()` is called.
150pub struct HeaderBuilder<'a> {
151    stamp: Time,
152    frame_id: std::borrow::Cow<'a, str>,
153}
154
155impl<'a> Default for HeaderBuilder<'a> {
156    fn default() -> Self {
157        Self {
158            stamp: Time { sec: 0, nanosec: 0 },
159            frame_id: std::borrow::Cow::Borrowed(""),
160        }
161    }
162}
163
164impl<'a> HeaderBuilder<'a> {
165    pub fn new() -> Self {
166        Self::default()
167    }
168
169    pub fn stamp(&mut self, t: Time) -> &mut Self {
170        self.stamp = t;
171        self
172    }
173
174    pub fn frame_id(&mut self, s: impl Into<std::borrow::Cow<'a, str>>) -> &mut Self {
175        self.frame_id = s.into();
176        self
177    }
178
179    fn size(&self) -> usize {
180        let mut s = CdrSizer::new();
181        Time::size_cdr(&mut s);
182        s.size_string(&self.frame_id);
183        s.size()
184    }
185
186    fn write_into(&self, buf: &mut [u8]) -> Result<(), CdrError> {
187        let mut w = CdrWriter::new(buf)?;
188        self.stamp.write_cdr(&mut w);
189        w.write_string(&self.frame_id);
190        w.finish()
191    }
192
193    /// Allocate a fresh `Vec<u8>` and return a fully-parsed `Header<Vec<u8>>`.
194    pub fn build(&self) -> Result<Header<Vec<u8>>, CdrError> {
195        let mut buf = vec![0u8; self.size()];
196        self.write_into(&mut buf)?;
197        Header::from_cdr(buf)
198    }
199
200    /// Serialize into the caller's `Vec<u8>`, resizing to exactly the encoded
201    /// size. After return, `buf.len()` is the CDR size and `&buf[..]` is a
202    /// complete CDR message. Reuses existing allocation when capacity suffices.
203    pub fn encode_into_vec(&self, buf: &mut Vec<u8>) -> Result<(), CdrError> {
204        buf.resize(self.size(), 0);
205        self.write_into(buf)
206    }
207
208    /// Serialize into `buf` and return bytes written. Errors with
209    /// `BufferTooShort` when `buf` is smaller than the required size; nothing
210    /// is mutated in that case.
211    pub fn encode_into_slice(&self, buf: &mut [u8]) -> Result<usize, CdrError> {
212        let need = self.size();
213        if buf.len() < need {
214            return Err(CdrError::BufferTooShort {
215                need,
216                have: buf.len(),
217            });
218        }
219        self.write_into(&mut buf[..need])?;
220        Ok(need)
221    }
222}
223
224impl<B: AsRef<[u8]> + AsMut<[u8]>> Header<B> {
225    pub fn set_stamp(&mut self, t: Time) -> Result<(), CdrError> {
226        let b = self.buf.as_mut();
227        wr_i32(b, CDR_HEADER_SIZE, t.sec)?;
228        wr_u32(b, CDR_HEADER_SIZE + 4, t.nanosec)
229    }
230}
231
232// ── Registry ────────────────────────────────────────────────────────
233
234/// Check if a type name is supported by this module.
235pub fn is_type_supported(type_name: &str) -> bool {
236    matches!(type_name, "Header" | "ColorRGBA")
237}
238
239/// List all type schema names in this module.
240pub fn list_types() -> &'static [&'static str] {
241    &["std_msgs/msg/Header", "std_msgs/msg/ColorRGBA"]
242}
243
244// SchemaType implementations
245use crate::schema_registry::SchemaType;
246
247impl SchemaType for ColorRGBA {
248    const SCHEMA_NAME: &'static str = "std_msgs/msg/ColorRGBA";
249}
250
251#[cfg(test)]
252#[allow(deprecated)]
253mod tests {
254    use super::*;
255    use crate::builtin_interfaces::Time;
256
257    #[test]
258    fn header_roundtrip() {
259        let cases = [
260            (0, 0, "", "empty"),
261            (100, 500_000_000, "camera", "typical"),
262            (42, 0, "a]long_frame_id", "with special chars"),
263            (1, 0, "camera/optical_frame", "path separator"),
264            (i32::MAX, 999_999_999, "max_frame", "max time"),
265        ];
266        for (sec, nanosec, frame_id, name) in cases {
267            let header = Header::new(Time::new(sec, nanosec), frame_id).unwrap();
268            assert_eq!(
269                header.stamp(),
270                Time::new(sec, nanosec),
271                "stamp failed: {}",
272                name
273            );
274            assert_eq!(header.frame_id(), frame_id, "frame_id failed: {}", name);
275
276            // Round-trip through CDR bytes
277            let bytes = header.to_cdr();
278            let decoded = Header::from_cdr(bytes).unwrap();
279            assert_eq!(
280                decoded.stamp(),
281                Time::new(sec, nanosec),
282                "rt stamp failed: {}",
283                name
284            );
285            assert_eq!(decoded.frame_id(), frame_id, "rt frame_id failed: {}", name);
286        }
287    }
288
289    #[test]
290    fn header_set_stamp() {
291        let mut header = Header::new(Time::new(0, 0), "test").unwrap();
292        header.set_stamp(Time::new(42, 123)).unwrap();
293        assert_eq!(header.stamp(), Time::new(42, 123));
294    }
295
296    #[test]
297    fn color_rgba_roundtrip() {
298        use crate::cdr::{decode_fixed, encode_fixed};
299
300        let cases = [
301            (0.0, 0.0, 0.0, 0.0, "zero/transparent"),
302            (1.0, 0.0, 0.0, 1.0, "red"),
303            (0.0, 1.0, 0.0, 1.0, "green"),
304            (0.0, 0.0, 1.0, 1.0, "blue"),
305            (1.0, 1.0, 1.0, 1.0, "white"),
306            (0.5, 0.5, 0.5, 0.5, "gray semi-transparent"),
307        ];
308        for (r, g, b, a, name) in cases {
309            let color = ColorRGBA { r, g, b, a };
310            let bytes = encode_fixed(&color).unwrap();
311            let decoded: ColorRGBA = decode_fixed(&bytes).unwrap();
312            assert_eq!(color, decoded, "failed for case: {}", name);
313        }
314    }
315}
edgefirst_schemas/std_msgs.rs

edgefirst_schemas/
std_msgs.rs
