rosrust/
rosmsg.rs

Help
1use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt};
2use ros_message::{Duration, Time};
3use std::collections::HashMap;
4use std::io;
5
6pub trait RosMsg: std::marker::Sized {
7    fn encode<W: io::Write>(&self, w: W) -> io::Result<()>;
8    fn decode<R: io::Read>(r: R) -> io::Result<Self>;
9
10    #[inline]
11    fn encode_vec(&self) -> io::Result<Vec<u8>> {
12        let mut writer = io::Cursor::new(Vec::with_capacity(128));
13        // skip the first 4 bytes that will contain the message length
14        writer.set_position(4);
15
16        self.encode(&mut writer)?;
17
18        // write the message length to the start of the header
19        let message_length = (writer.position() - 4) as u32;
20        writer.set_position(0);
21        message_length.encode(&mut writer)?;
22        Ok(writer.into_inner())
23    }
24
25    #[inline]
26    fn decode_slice(bytes: &[u8]) -> io::Result<Self> {
27        let mut reader = io::Cursor::new(bytes);
28        // skip the first 4 bytes that contain the message length
29        reader.set_position(4);
30        Self::decode(&mut reader)
31    }
32}
33
34impl RosMsg for bool {
35    #[inline]
36    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
37        w.write_u8(*self as u8)
38    }
39
40    #[inline]
41    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
42        r.read_u8().map(|u| u > 0)
43    }
44}
45
46impl RosMsg for u8 {
47    #[inline]
48    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
49        w.write_u8(*self)
50    }
51
52    #[inline]
53    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
54        r.read_u8()
55    }
56}
57
58impl RosMsg for i8 {
59    #[inline]
60    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
61        w.write_i8(*self)
62    }
63
64    #[inline]
65    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
66        r.read_i8()
67    }
68}
69
70impl RosMsg for u16 {
71    #[inline]
72    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
73        w.write_u16::<LittleEndian>(*self)
74    }
75
76    #[inline]
77    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
78        r.read_u16::<LittleEndian>()
79    }
80}
81
82impl RosMsg for i16 {
83    #[inline]
84    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
85        w.write_i16::<LittleEndian>(*self)
86    }
87
88    #[inline]
89    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
90        r.read_i16::<LittleEndian>()
91    }
92}
93
94impl RosMsg for u32 {
95    #[inline]
96    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
97        w.write_u32::<LittleEndian>(*self)
98    }
99
100    #[inline]
101    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
102        r.read_u32::<LittleEndian>()
103    }
104}
105
106impl RosMsg for i32 {
107    #[inline]
108    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
109        w.write_i32::<LittleEndian>(*self)
110    }
111
112    #[inline]
113    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
114        r.read_i32::<LittleEndian>()
115    }
116}
117
118impl RosMsg for u64 {
119    #[inline]
120    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
121        w.write_u64::<LittleEndian>(*self)
122    }
123
124    #[inline]
125    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
126        r.read_u64::<LittleEndian>()
127    }
128}
129
130impl RosMsg for i64 {
131    #[inline]
132    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
133        w.write_i64::<LittleEndian>(*self)
134    }
135
136    #[inline]
137    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
138        r.read_i64::<LittleEndian>()
139    }
140}
141
142impl RosMsg for f32 {
143    #[inline]
144    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
145        w.write_f32::<LittleEndian>(*self)
146    }
147
148    #[inline]
149    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
150        r.read_f32::<LittleEndian>()
151    }
152}
153
154impl RosMsg for f64 {
155    #[inline]
156    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
157        w.write_f64::<LittleEndian>(*self)
158    }
159
160    #[inline]
161    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
162        r.read_f64::<LittleEndian>()
163    }
164}
165
166#[inline]
167pub fn encode_fixed_slice<W: io::Write, T: RosMsg>(data: &[T], mut w: W) -> io::Result<()> {
168    data.iter().try_for_each(|v| v.encode(w.by_ref()))
169}
170
171#[inline]
172pub fn decode_fixed_vec<R: io::Read, T: RosMsg>(len: u32, mut r: R) -> io::Result<Vec<T>> {
173    (0..len).map(move |_| T::decode(r.by_ref())).collect()
174}
175
176#[inline]
177pub fn encode_variable_slice<W: io::Write, T: RosMsg>(data: &[T], mut w: W) -> io::Result<()> {
178    (data.len() as u32).encode(w.by_ref())?;
179    encode_fixed_slice(data, w)
180}
181
182#[inline]
183pub fn decode_variable_vec<R: io::Read, T: RosMsg>(mut r: R) -> io::Result<Vec<T>> {
184    decode_fixed_vec(u32::decode(r.by_ref())?, r)
185}
186
187/// Fast vector encoding when platform endiannes matches wire
188/// endiannes (little).
189#[inline]
190#[cfg(target_endian = "little")]
191pub fn encode_variable_primitive_slice<W: io::Write, T: RosMsg>(
192    data: &[T],
193    mut w: W,
194) -> io::Result<()> {
195    (data.len() as u32).encode(w.by_ref())?;
196    let ptr = data.as_ptr() as *const u8;
197
198    // Because both wire and system are little endian, we simply copy
199    // the in-memory slice to the buffer directly.
200    w.write(unsafe { std::slice::from_raw_parts(ptr, data.len() * std::mem::size_of::<T>()) })
201        .map(|_| ())
202}
203
204#[inline]
205#[cfg(target_endian = "big")]
206pub fn encode_variable_primitive_slice<W: io::Write, T: RosMsg>(
207    data: &[T],
208    mut w: W,
209) -> io::Result<()> {
210    encode_variable_slice(data, w)
211}
212
213/// Fast vector decoding when platform endiannes matches wire
214/// endiannes (little).
215#[inline]
216#[cfg(target_endian = "little")]
217pub fn decode_variable_primitive_vec<R: io::Read, T: RosMsg>(mut r: R) -> io::Result<Vec<T>> {
218    let num_elements = u32::decode(r.by_ref())? as usize;
219    let num_bytes = num_elements * std::mem::size_of::<T>();
220
221    // Allocate the memory w/o initializing because we will later fill
222    // all the memory.
223    let mut buf = Vec::<T>::with_capacity(num_elements);
224
225    let buf_ptr = buf.as_mut_ptr();
226
227    // Fill the Vec to full capacity with the stream data.
228    let read_buf = unsafe { std::slice::from_raw_parts_mut(buf_ptr as *mut u8, num_bytes) };
229    r.read_exact(read_buf)?;
230
231    // Do not drop the memory
232    std::mem::forget(buf);
233
234    // Return a new, completely full Vec using the now initialized memory.
235    Ok(unsafe { Vec::from_raw_parts(buf_ptr, num_elements, num_elements) })
236}
237
238#[inline]
239#[cfg(target_endian = "big")]
240pub fn decode_variable_primitive_vec<R: io::Read, T: RosMsg>(mut r: R) -> io::Result<Vec<T>> {
241    decode_variable_vec(r)
242}
243
244#[inline]
245pub fn encode_str<W: io::Write>(value: &str, w: W) -> io::Result<()> {
246    encode_variable_slice(value.as_bytes(), w)
247}
248
249impl RosMsg for String {
250    #[inline]
251    fn encode<W: io::Write>(&self, w: W) -> io::Result<()> {
252        encode_str(self, w)
253    }
254
255    #[inline]
256    fn decode<R: io::Read>(r: R) -> io::Result<Self> {
257        decode_variable_vec::<R, u8>(r).and_then(|v| {
258            String::from_utf8(v).map_err(|err| io::Error::new(io::ErrorKind::InvalidData, err))
259        })
260    }
261}
262
263impl<Hasher> RosMsg for HashMap<String, String, Hasher>
264where
265    Hasher: std::hash::BuildHasher,
266    HashMap<String, String, Hasher>: Default,
267{
268    #[inline]
269    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
270        let rows = self
271            .iter()
272            .map(|(key, value)| format!("{}={}", key, value))
273            .collect::<Vec<String>>();
274        let data_size: usize = rows.iter().map(|item| item.len() + 4).sum();
275        write_data_size(data_size as u32, w.by_ref())?;
276        rows.into_iter()
277            .try_for_each(|item| item.encode(w.by_ref()))
278    }
279
280    #[inline]
281    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
282        let data_size = u64::from(read_data_size(r.by_ref())?);
283        let mut limited_r = r.take(data_size);
284        let mut output = HashMap::<String, String, Hasher>::default();
285        // TODO: ensure we break only on EOF
286        while let Ok(item) = String::decode(&mut limited_r) {
287            let parts = item.splitn(2, '=').collect::<Vec<&str>>();
288            match *parts.as_slice() {
289                [key, value] => output.insert(key.into(), value.into()),
290                _ => {
291                    return Err(io::Error::new(
292                        io::ErrorKind::InvalidData,
293                        "Map rows need to have a format of key=value",
294                    ));
295                }
296            };
297        }
298        Ok(output)
299    }
300}
301
302impl RosMsg for Time {
303    #[inline]
304    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
305        self.sec.encode(w.by_ref())?;
306        self.nsec.encode(w)?;
307        Ok(())
308    }
309
310    #[inline]
311    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
312        Ok(Self {
313            sec: RosMsg::decode(r.by_ref())?,
314            nsec: RosMsg::decode(r)?,
315        })
316    }
317}
318
319impl RosMsg for Duration {
320    #[inline]
321    fn encode<W: io::Write>(&self, mut w: W) -> io::Result<()> {
322        self.sec.encode(w.by_ref())?;
323        self.nsec.encode(w)?;
324        Ok(())
325    }
326
327    #[inline]
328    fn decode<R: io::Read>(mut r: R) -> io::Result<Self> {
329        Ok(Self {
330            sec: RosMsg::decode(r.by_ref())?,
331            nsec: RosMsg::decode(r)?,
332        })
333    }
334}
335
336#[inline]
337fn read_data_size<R: io::Read>(r: R) -> io::Result<u32> {
338    u32::decode(r)
339}
340
341#[inline]
342fn write_data_size<W: io::Write>(value: u32, w: W) -> io::Result<()> {
343    value.encode(w)
344}
rosrust/rosmsg.rs

rosrust/
rosmsg.rs
