pamoja_ros2/msg.rs
1//! CDR serialization and the geometry messages a robot is driven by.
2//!
3//! ROS 2 and `rmw_zenoh` put messages on the wire as CDR, the OMG Common Data Representation. A
4//! CDR stream opens with a four-byte encapsulation header naming the byte order, after which each
5//! primitive is written in that byte order and aligned to its own size relative to the start of the
6//! body. Getting the alignment padding wrong is the classic CDR bug, so the [`CdrWriter`] and
7//! [`CdrReader`] handle it once, and the message types build on them. This slice covers the
8//! little-endian encapsulation and the geometry messages used to command motion; more messages and
9//! big-endian decoding arrive with the live bridge.
10
11use alloc::vec::Vec;
12
13// The CDR encapsulation header is four bytes: a two-byte representation identifier and two option
14// bytes. `00 01` selects classic CDR, little-endian; the options are unused here.
15const ENCAPSULATION: [u8; 4] = [0x00, 0x01, 0x00, 0x00];
16const ENCAPSULATION_LEN: usize = 4;
17
18/// Writes primitives as little-endian CDR, handling alignment padding.
19///
20/// The writer starts with the little-endian encapsulation header; each write aligns the cursor to
21/// the value's size (measured from the start of the body) before appending the bytes.
22///
23/// # Examples
24///
25/// ```
26/// use pamoja_ros2::msg::CdrWriter;
27///
28/// let mut w = CdrWriter::new();
29/// w.write_f64(1.0);
30/// // Four-byte header plus eight bytes for the double.
31/// assert_eq!(w.into_bytes().len(), 12);
32/// ```
33#[derive(Clone, Debug, Default)]
34pub struct CdrWriter {
35 buf: Vec<u8>,
36}
37
38impl CdrWriter {
39 /// Creates a writer primed with the little-endian CDR encapsulation header.
40 ///
41 /// # Returns
42 ///
43 /// The writer.
44 pub fn new() -> Self {
45 let mut buf = Vec::new();
46 buf.extend_from_slice(&ENCAPSULATION);
47 Self { buf }
48 }
49
50 fn align(&mut self, alignment: usize) {
51 let offset = self.buf.len() - ENCAPSULATION_LEN;
52 let padding = (alignment - (offset % alignment)) % alignment;
53 self.buf.resize(self.buf.len() + padding, 0);
54 }
55
56 /// Writes a 32-bit signed integer.
57 ///
58 /// # Arguments
59 ///
60 /// * `value` - the integer to write.
61 pub fn write_i32(&mut self, value: i32) {
62 self.align(4);
63 self.buf.extend_from_slice(&value.to_le_bytes());
64 }
65
66 /// Writes a 32-bit unsigned integer.
67 ///
68 /// # Arguments
69 ///
70 /// * `value` - the integer to write.
71 pub fn write_u32(&mut self, value: u32) {
72 self.align(4);
73 self.buf.extend_from_slice(&value.to_le_bytes());
74 }
75
76 /// Writes a 32-bit float.
77 ///
78 /// # Arguments
79 ///
80 /// * `value` - the float to write.
81 pub fn write_f32(&mut self, value: f32) {
82 self.align(4);
83 self.buf.extend_from_slice(&value.to_le_bytes());
84 }
85
86 /// Writes a 64-bit float.
87 ///
88 /// # Arguments
89 ///
90 /// * `value` - the float to write.
91 pub fn write_f64(&mut self, value: f64) {
92 self.align(8);
93 self.buf.extend_from_slice(&value.to_le_bytes());
94 }
95
96 /// Consumes the writer and returns the encoded bytes, header included.
97 ///
98 /// # Returns
99 ///
100 /// The CDR-encoded buffer.
101 pub fn into_bytes(self) -> Vec<u8> {
102 self.buf
103 }
104}
105
106/// Reads primitives from a little-endian CDR buffer, handling alignment padding.
107///
108/// The reader checks the encapsulation header on construction and then mirrors [`CdrWriter`]'s
109/// alignment, so a value written by the writer is read back identically.
110pub struct CdrReader<'a> {
111 body: &'a [u8],
112 pos: usize,
113}
114
115impl<'a> CdrReader<'a> {
116 /// Creates a reader over a CDR buffer.
117 ///
118 /// # Arguments
119 ///
120 /// * `data` - the CDR-encoded buffer, including the four-byte encapsulation header.
121 ///
122 /// # Returns
123 ///
124 /// `Some(reader)` if `data` carries a classic little-endian CDR header; `None` otherwise,
125 /// including a buffer too short to hold a header or one declaring a byte order this reader does
126 /// not decode.
127 pub fn new(data: &'a [u8]) -> Option<Self> {
128 if data.len() < ENCAPSULATION_LEN || data[0] != 0x00 || data[1] != 0x01 {
129 return None;
130 }
131 Some(Self {
132 body: &data[ENCAPSULATION_LEN..],
133 pos: 0,
134 })
135 }
136
137 fn align(&mut self, alignment: usize) {
138 let padding = (alignment - (self.pos % alignment)) % alignment;
139 self.pos += padding;
140 }
141
142 fn take<const N: usize>(&mut self, alignment: usize) -> Option<[u8; N]> {
143 self.align(alignment);
144 let end = self.pos.checked_add(N)?;
145 if end > self.body.len() {
146 return None;
147 }
148 let bytes: [u8; N] = self.body[self.pos..end].try_into().ok()?;
149 self.pos = end;
150 Some(bytes)
151 }
152
153 /// Reads a 32-bit signed integer.
154 ///
155 /// # Returns
156 ///
157 /// `Some(value)`, or `None` if the buffer is exhausted.
158 pub fn read_i32(&mut self) -> Option<i32> {
159 self.take::<4>(4).map(i32::from_le_bytes)
160 }
161
162 /// Reads a 32-bit unsigned integer.
163 ///
164 /// # Returns
165 ///
166 /// `Some(value)`, or `None` if the buffer is exhausted.
167 pub fn read_u32(&mut self) -> Option<u32> {
168 self.take::<4>(4).map(u32::from_le_bytes)
169 }
170
171 /// Reads a 32-bit float.
172 ///
173 /// # Returns
174 ///
175 /// `Some(value)`, or `None` if the buffer is exhausted.
176 pub fn read_f32(&mut self) -> Option<f32> {
177 self.take::<4>(4).map(f32::from_le_bytes)
178 }
179
180 /// Reads a 64-bit float.
181 ///
182 /// # Returns
183 ///
184 /// `Some(value)`, or `None` if the buffer is exhausted.
185 pub fn read_f64(&mut self) -> Option<f64> {
186 self.take::<8>(8).map(f64::from_le_bytes)
187 }
188}
189
190/// A three-dimensional vector (`geometry_msgs/msg/Vector3`): three 64-bit floats.
191#[derive(Clone, Copy, Debug, PartialEq)]
192pub struct Vector3 {
193 /// The x component.
194 pub x: f64,
195 /// The y component.
196 pub y: f64,
197 /// The z component.
198 pub z: f64,
199}
200
201impl Vector3 {
202 /// Creates a vector from its components.
203 ///
204 /// # Arguments
205 ///
206 /// * `x` - the x component.
207 /// * `y` - the y component.
208 /// * `z` - the z component.
209 ///
210 /// # Returns
211 ///
212 /// The vector.
213 pub fn new(x: f64, y: f64, z: f64) -> Self {
214 Self { x, y, z }
215 }
216
217 /// Encodes the vector into a CDR writer.
218 ///
219 /// # Arguments
220 ///
221 /// * `writer` - the writer to append to.
222 pub fn encode(&self, writer: &mut CdrWriter) {
223 writer.write_f64(self.x);
224 writer.write_f64(self.y);
225 writer.write_f64(self.z);
226 }
227
228 /// Decodes a vector from a CDR reader.
229 ///
230 /// # Arguments
231 ///
232 /// * `reader` - the reader to consume from.
233 ///
234 /// # Returns
235 ///
236 /// `Some(vector)`, or `None` if the buffer is exhausted.
237 pub fn decode(reader: &mut CdrReader) -> Option<Self> {
238 Some(Self {
239 x: reader.read_f64()?,
240 y: reader.read_f64()?,
241 z: reader.read_f64()?,
242 })
243 }
244}
245
246/// A body velocity command (`geometry_msgs/msg/Twist`): a linear and an angular [`Vector3`].
247///
248/// This is the message a ROS 2 robot is driven by on `cmd_vel`, the natural target for the body
249/// twists the `pamoja-kit` chassis and navigation helpers produce.
250///
251/// # Examples
252///
253/// ```
254/// use pamoja_ros2::msg::{Twist, Vector3};
255///
256/// let cmd = Twist {
257/// linear: Vector3::new(0.5, 0.0, 0.0),
258/// angular: Vector3::new(0.0, 0.0, 0.2),
259/// };
260/// assert_eq!(Twist::from_cdr(&cmd.to_cdr()), Some(cmd));
261/// ```
262#[derive(Clone, Copy, Debug, PartialEq)]
263pub struct Twist {
264 /// The linear velocity, in metres per second.
265 pub linear: Vector3,
266 /// The angular velocity, in radians per second.
267 pub angular: Vector3,
268}
269
270impl Twist {
271 /// Encodes the twist as a CDR message.
272 ///
273 /// # Returns
274 ///
275 /// The CDR-encoded bytes, header included.
276 pub fn to_cdr(&self) -> Vec<u8> {
277 let mut writer = CdrWriter::new();
278 self.linear.encode(&mut writer);
279 self.angular.encode(&mut writer);
280 writer.into_bytes()
281 }
282
283 /// Decodes a twist from a CDR message.
284 ///
285 /// # Arguments
286 ///
287 /// * `data` - the CDR-encoded bytes, header included.
288 ///
289 /// # Returns
290 ///
291 /// `Some(twist)`, or `None` if the buffer is not a valid little-endian CDR twist.
292 pub fn from_cdr(data: &[u8]) -> Option<Self> {
293 let mut reader = CdrReader::new(data)?;
294 let linear = Vector3::decode(&mut reader)?;
295 let angular = Vector3::decode(&mut reader)?;
296 Some(Self { linear, angular })
297 }
298}
299
300#[cfg(test)]
301mod tests {
302 use super::*;
303
304 #[test]
305 fn alignment_pads_a_double_after_an_int() {
306 // A u32 then an f64: the double must align to an 8-byte boundary in the body, so four
307 // padding bytes sit between them.
308 let mut w = CdrWriter::new();
309 w.write_u32(0x0102_0304);
310 w.write_f64(1.0);
311 let bytes = w.into_bytes();
312 assert_eq!(
313 bytes,
314 [
315 0x00, 0x01, 0x00, 0x00, // encapsulation header
316 0x04, 0x03, 0x02, 0x01, // u32, little-endian
317 0x00, 0x00, 0x00, 0x00, // alignment padding to offset 8
318 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x3F, // 1.0_f64, little-endian
319 ]
320 );
321 }
322
323 #[test]
324 fn twist_matches_a_hand_computed_cdr_vector() {
325 let cmd = Twist {
326 linear: Vector3::new(1.0, 0.0, 0.0),
327 angular: Vector3::new(0.0, 0.0, 0.5),
328 };
329 let bytes = cmd.to_cdr();
330 let mut expected = Vec::new();
331 expected.extend_from_slice(&[0x00, 0x01, 0x00, 0x00]); // header
332 expected.extend_from_slice(&1.0_f64.to_le_bytes()); // linear.x
333 expected.extend_from_slice(&0.0_f64.to_le_bytes()); // linear.y
334 expected.extend_from_slice(&0.0_f64.to_le_bytes()); // linear.z
335 expected.extend_from_slice(&0.0_f64.to_le_bytes()); // angular.x
336 expected.extend_from_slice(&0.0_f64.to_le_bytes()); // angular.y
337 expected.extend_from_slice(&0.5_f64.to_le_bytes()); // angular.z
338 assert_eq!(bytes, expected);
339 assert_eq!(bytes.len(), 4 + 48);
340 }
341
342 #[test]
343 fn twist_round_trips_through_cdr() {
344 let cmd = Twist {
345 linear: Vector3::new(0.5, -1.5, 0.0),
346 angular: Vector3::new(0.0, 0.0, 0.25),
347 };
348 assert_eq!(Twist::from_cdr(&cmd.to_cdr()), Some(cmd));
349 }
350
351 #[test]
352 fn primitives_round_trip_with_alignment() {
353 let mut w = CdrWriter::new();
354 w.write_i32(-7);
355 w.write_f64(2.5);
356 w.write_f32(1.25);
357 w.write_u32(42);
358 let bytes = w.into_bytes();
359
360 let mut r = CdrReader::new(&bytes).unwrap();
361 assert_eq!(r.read_i32(), Some(-7));
362 assert_eq!(r.read_f64(), Some(2.5));
363 assert_eq!(r.read_f32(), Some(1.25));
364 assert_eq!(r.read_u32(), Some(42));
365 }
366
367 #[test]
368 fn a_short_or_wrong_endian_buffer_is_rejected() {
369 assert!(CdrReader::new(&[0x00]).is_none()); // too short for a header
370 assert!(CdrReader::new(&[0x00, 0x00, 0x00, 0x00]).is_none()); // big-endian, not decoded here
371 assert!(Twist::from_cdr(&[0x00, 0x01, 0x00, 0x00]).is_none()); // header but no body
372 }
373}