use crate::rosbag::error::{ReaderError, Result};
use std::convert::TryInto;
#[derive(Debug, Clone, Copy)]
pub struct CdrHeader {
pub endianness: Endianness,
pub encapsulation_kind: u8,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum Endianness {
LittleEndian,
BigEndian,
}
pub struct CdrDeserializer<'a> {
data: &'a [u8],
pos: usize,
endianness: Endianness,
}
impl<'a> CdrDeserializer<'a> {
pub fn new(data: &'a [u8]) -> Result<Self> {
if data.len() < 4 {
return Err(ReaderError::generic("CDR data too short for header"));
}
let header = CdrHeader::parse(&data[0..4])?;
Ok(Self {
data,
pos: 4, endianness: header.endianness,
})
}
pub fn position(&self) -> usize {
self.pos
}
pub fn data_len(&self) -> usize {
self.data.len()
}
pub fn has_remaining(&self, bytes: usize) -> bool {
self.pos + bytes <= self.data.len()
}
pub fn data(&self) -> &[u8] {
self.data
}
fn align(&mut self, alignment: usize) {
self.pos = (self.pos + alignment - 1) & !(alignment - 1);
}
fn read_primitive<T>(&mut self, size: usize) -> Result<T>
where
T: FromBytes,
{
self.align(size);
if self.pos + size > self.data.len() {
return Err(ReaderError::generic(format!(
"CDR data truncated: need {} bytes at pos {}, but only {} bytes available",
size,
self.pos,
self.data.len()
)));
}
let bytes = &self.data[self.pos..self.pos + size];
self.pos += size;
T::from_bytes(bytes, self.endianness)
}
pub fn read_i8(&mut self) -> Result<i8> {
self.read_primitive(1)
}
pub fn read_u8(&mut self) -> Result<u8> {
self.read_primitive(1)
}
pub fn read_u16(&mut self) -> Result<u16> {
self.read_primitive(2)
}
pub fn read_i32(&mut self) -> Result<i32> {
self.read_primitive(4)
}
pub fn read_u32(&mut self) -> Result<u32> {
self.read_primitive(4)
}
pub fn read_f64(&mut self) -> Result<f64> {
self.align(8);
if self.pos + 8 > self.data.len() {
return Err(ReaderError::generic(format!(
"CDR data truncated: need 8 bytes at pos {}, but only {} bytes available",
self.pos,
self.data.len()
)));
}
let bytes = &self.data[self.pos..self.pos + 8];
self.pos += 8;
f64::from_bytes(bytes, self.endianness)
}
pub fn read_string(&mut self) -> Result<String> {
let length = self.read_u32()? as usize;
if length == 0 {
return Ok(String::new());
}
if self.pos + length > self.data.len() {
return Err(ReaderError::generic("CDR string data truncated"));
}
let string_bytes = if length > 0 && self.data[self.pos + length - 1] == 0 {
&self.data[self.pos..self.pos + length - 1]
} else {
&self.data[self.pos..self.pos + length]
};
self.pos += length;
String::from_utf8(string_bytes.to_vec())
.map_err(|_| ReaderError::generic("Invalid UTF-8 in CDR string"))
}
pub fn read_f64_array<const N: usize>(&mut self) -> Result<[f64; N]> {
let mut array = [0.0; N];
for item in array.iter_mut().take(N) {
*item = self.read_f64()?;
}
Ok(array)
}
pub fn read_sequence<T, F>(&mut self, read_element: F) -> Result<Vec<T>>
where
F: Fn(&mut Self) -> Result<T>,
{
let length = self.read_u32()? as usize;
let mut vec = Vec::with_capacity(length);
for _ in 0..length {
vec.push(read_element(self)?);
}
Ok(vec)
}
pub fn read_byte_sequence(&mut self) -> Result<Vec<u8>> {
let length = self.read_u32()? as usize;
if self.pos + length > self.data.len() {
return Err(ReaderError::generic(format!(
"CDR data truncated: need {} bytes at pos {}, but only {} bytes available",
length,
self.pos,
self.data.len()
)));
}
let bytes = self.data[self.pos..self.pos + length].to_vec();
self.pos += length;
Ok(bytes)
}
pub fn read_bool(&mut self) -> Result<bool> {
let byte = self.read_u8()?;
Ok(byte != 0)
}
pub fn read_f32(&mut self) -> Result<f32> {
self.align(4);
if self.pos + 4 > self.data.len() {
return Err(ReaderError::generic(format!(
"CDR data truncated: need 4 bytes at pos {}, but only {} bytes available",
self.pos,
self.data.len()
)));
}
let bytes = &self.data[self.pos..self.pos + 4];
self.pos += 4;
f32::from_bytes(bytes, self.endianness)
}
}
impl CdrHeader {
pub fn parse(header_bytes: &[u8]) -> Result<Self> {
if header_bytes.len() != 4 {
return Err(ReaderError::generic("CDR header must be exactly 4 bytes"));
}
let endianness = match header_bytes[1] {
0 => Endianness::BigEndian,
1 => Endianness::LittleEndian,
_ => return Err(ReaderError::generic("Invalid CDR endianness flag")),
};
Ok(Self {
endianness,
encapsulation_kind: header_bytes[2],
})
}
}
trait FromBytes: Sized {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self>;
}
impl FromBytes for i8 {
fn from_bytes(bytes: &[u8], _endianness: Endianness) -> Result<Self> {
if bytes.len() != 1 {
return Err(ReaderError::generic("Invalid i8 bytes"));
}
Ok(bytes[0] as i8)
}
}
impl FromBytes for u8 {
fn from_bytes(bytes: &[u8], _endianness: Endianness) -> Result<Self> {
if bytes.len() != 1 {
return Err(ReaderError::generic("Invalid u8 bytes"));
}
Ok(bytes[0])
}
}
impl FromBytes for u16 {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self> {
let array: [u8; 2] = bytes
.try_into()
.map_err(|_| ReaderError::generic("Invalid u16 bytes"))?;
Ok(match endianness {
Endianness::LittleEndian => u16::from_le_bytes(array),
Endianness::BigEndian => u16::from_be_bytes(array),
})
}
}
impl FromBytes for i32 {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self> {
let array: [u8; 4] = bytes
.try_into()
.map_err(|_| ReaderError::generic("Invalid i32 bytes"))?;
Ok(match endianness {
Endianness::LittleEndian => i32::from_le_bytes(array),
Endianness::BigEndian => i32::from_be_bytes(array),
})
}
}
impl FromBytes for u32 {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self> {
let array: [u8; 4] = bytes
.try_into()
.map_err(|_| ReaderError::generic("Invalid u32 bytes"))?;
Ok(match endianness {
Endianness::LittleEndian => u32::from_le_bytes(array),
Endianness::BigEndian => u32::from_be_bytes(array),
})
}
}
impl FromBytes for f32 {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self> {
let array: [u8; 4] = bytes
.try_into()
.map_err(|_| ReaderError::generic("Invalid f32 bytes"))?;
Ok(match endianness {
Endianness::LittleEndian => f32::from_le_bytes(array),
Endianness::BigEndian => f32::from_be_bytes(array),
})
}
}
impl FromBytes for f64 {
fn from_bytes(bytes: &[u8], endianness: Endianness) -> Result<Self> {
let array: [u8; 8] = bytes
.try_into()
.map_err(|_| ReaderError::generic("Invalid f64 bytes"))?;
Ok(match endianness {
Endianness::LittleEndian => f64::from_le_bytes(array),
Endianness::BigEndian => f64::from_be_bytes(array),
})
}
}
pub struct CdrSerializer {
buf: Vec<u8>,
}
impl CdrSerializer {
pub fn new() -> Self {
let mut s = Self {
buf: Vec::with_capacity(64),
};
s.buf.extend_from_slice(&[0x00, 0x01, 0x00, 0x00]);
s
}
fn align(&mut self, n: usize) {
while self.buf.len() % n != 0 {
self.buf.push(0);
}
}
pub fn write_u8(&mut self, v: u8) {
self.buf.push(v);
}
pub fn write_i32(&mut self, v: i32) {
self.align(4);
self.buf.extend_from_slice(&v.to_le_bytes());
}
pub fn write_u32(&mut self, v: u32) {
self.align(4);
self.buf.extend_from_slice(&v.to_le_bytes());
}
pub fn write_f64(&mut self, v: f64) {
self.align(8);
self.buf.extend_from_slice(&v.to_le_bytes());
}
pub fn write_string(&mut self, s: &str) {
let len = s.len() as u32 + 1; self.write_u32(len);
self.buf.extend_from_slice(s.as_bytes());
self.buf.push(0);
}
pub fn write_byte_sequence(&mut self, bytes: &[u8]) {
self.write_u32(bytes.len() as u32);
self.buf.extend_from_slice(bytes);
}
pub fn into_bytes(self) -> Vec<u8> {
self.buf
}
}
impl Default for CdrSerializer {
fn default() -> Self {
Self::new()
}
}
#[cfg(test)]
mod tests {
type TestResult = std::result::Result<(), Box<dyn std::error::Error>>;
use super::*;
fn le_header() -> Vec<u8> {
vec![0x00, 0x01, 0x00, 0x00]
}
fn be_header() -> Vec<u8> {
vec![0x00, 0x00, 0x00, 0x00]
}
#[test]
fn test_cdr_header_parsing() -> TestResult {
let header_le = CdrHeader::parse(&[0x00, 0x01, 0x00, 0x00])?;
assert_eq!(header_le.endianness, Endianness::LittleEndian);
assert_eq!(header_le.encapsulation_kind, 0);
let header_be = CdrHeader::parse(&[0x00, 0x00, 0x00, 0x00])?;
assert_eq!(header_be.endianness, Endianness::BigEndian);
Ok(())
}
#[test]
fn cdr_header_wrong_length_returns_err() -> TestResult {
assert!(CdrHeader::parse(&[0x00, 0x01, 0x00]).is_err());
assert!(CdrHeader::parse(&[]).is_err());
assert!(CdrHeader::parse(&[0x00, 0x01, 0x00, 0x00, 0x00]).is_err());
Ok(())
}
#[test]
fn cdr_header_invalid_endian_flag_returns_err() -> TestResult {
assert!(CdrHeader::parse(&[0x00, 0x02, 0x00, 0x00]).is_err());
Ok(())
}
#[test]
fn deserializer_new_too_short_returns_err() -> TestResult {
assert!(CdrDeserializer::new(&[0x00, 0x01, 0x00]).is_err());
assert!(CdrDeserializer::new(&[]).is_err());
Ok(())
}
#[test]
fn deserializer_new_le_header_ok() -> TestResult {
let data = le_header();
let d = CdrDeserializer::new(&data)?;
assert_eq!(d.position(), 4);
assert_eq!(d.data_len(), 4);
Ok(())
}
#[test]
fn deserializer_new_be_header_ok() -> TestResult {
let data = be_header();
let d = CdrDeserializer::new(&data)?;
assert_eq!(d.position(), 4);
Ok(())
}
#[test]
fn has_remaining_true_and_false() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0x01, 0x02]);
let d = CdrDeserializer::new(&data)?;
assert!(d.has_remaining(2));
assert!(d.has_remaining(1));
assert!(!d.has_remaining(3));
Ok(())
}
#[test]
fn data_len_matches_slice() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0xAA; 8]);
let d = CdrDeserializer::new(&data)?;
assert_eq!(d.data_len(), 12);
Ok(())
}
#[test]
fn data_accessor_returns_full_slice() -> TestResult {
let data = le_header();
let d = CdrDeserializer::new(&data)?;
assert_eq!(d.data(), &data[..]);
Ok(())
}
#[test]
fn test_primitive_deserialization() -> TestResult {
let data = [
0x00, 0x01, 0x00, 0x00, 0x2A, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04,
];
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_i32()?, 42);
assert_eq!(d.read_u32()?, 0x04030201);
Ok(())
}
#[test]
fn read_u8_le() -> TestResult {
let mut data = le_header();
data.push(0xAB);
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_u8()?, 0xAB);
Ok(())
}
#[test]
fn read_i8_le() -> TestResult {
let mut data = le_header();
data.push(0xFF); let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_i8()?, -1i8);
Ok(())
}
#[test]
fn read_u16_le() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&300u16.to_le_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_u16()?, 300);
Ok(())
}
#[test]
fn read_u16_be() -> TestResult {
let mut data = be_header();
data.extend_from_slice(&300u16.to_be_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_u16()?, 300);
Ok(())
}
#[test]
fn read_i32_be() -> TestResult {
let mut data = be_header();
data.extend_from_slice(&(-99i32).to_be_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_i32()?, -99);
Ok(())
}
#[test]
fn read_u32_be() -> TestResult {
let mut data = be_header();
data.extend_from_slice(&0xDEADBEEFu32.to_be_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_u32()?, 0xDEADBEEF);
Ok(())
}
#[test]
fn read_f64_le() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0u8; 4]); data.extend_from_slice(&1.5f64.to_le_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert!((d.read_f64()? - 1.5).abs() < 1e-10);
Ok(())
}
#[test]
fn read_f32_le() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&2.5f32.to_le_bytes());
let mut d = CdrDeserializer::new(&data)?;
assert!((d.read_f32()? - 2.5f32).abs() < 1e-7);
Ok(())
}
#[test]
fn read_bool_true_and_false() -> TestResult {
let mut data = le_header();
data.push(0x01);
data.push(0x00);
let mut d = CdrDeserializer::new(&data)?;
assert!(d.read_bool()?);
assert!(!d.read_bool()?);
Ok(())
}
#[test]
fn read_string_empty() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&0u32.to_le_bytes()); let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_string()?, "");
Ok(())
}
#[test]
fn read_string_with_null_terminator() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&5u32.to_le_bytes()); data.extend_from_slice(b"hello\0"); let mut data2 = le_header();
data2.extend_from_slice(&5u32.to_le_bytes());
data2.extend_from_slice(b"hell\0");
let mut d = CdrDeserializer::new(&data2)?;
assert_eq!(d.read_string()?, "hell");
Ok(())
}
#[test]
fn read_string_without_null_terminator() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&3u32.to_le_bytes()); data.extend_from_slice(b"abc"); let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_string()?, "abc");
Ok(())
}
#[test]
fn read_f64_array_3() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0u8; 4]); for &v in &[1.0f64, 2.0, 3.0] {
data.extend_from_slice(&v.to_le_bytes());
}
let mut d = CdrDeserializer::new(&data)?;
let arr = d.read_f64_array::<3>()?;
assert!((arr[0] - 1.0).abs() < 1e-12);
assert!((arr[1] - 2.0).abs() < 1e-12);
assert!((arr[2] - 3.0).abs() < 1e-12);
Ok(())
}
#[test]
fn read_sequence_of_u32() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&3u32.to_le_bytes()); data.extend_from_slice(&10u32.to_le_bytes());
data.extend_from_slice(&20u32.to_le_bytes());
data.extend_from_slice(&30u32.to_le_bytes());
let mut d = CdrDeserializer::new(&data)?;
let seq = d.read_sequence(|d| d.read_u32())?;
assert_eq!(seq, vec![10, 20, 30]);
Ok(())
}
#[test]
fn read_sequence_empty() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&0u32.to_le_bytes()); let mut d = CdrDeserializer::new(&data)?;
let seq = d.read_sequence(|d| d.read_u32())?;
assert!(seq.is_empty());
Ok(())
}
#[test]
fn read_byte_sequence_basic() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&4u32.to_le_bytes()); data.extend_from_slice(&[0xAA, 0xBB, 0xCC, 0xDD]);
let mut d = CdrDeserializer::new(&data)?;
assert_eq!(d.read_byte_sequence()?, &[0xAA, 0xBB, 0xCC, 0xDD]);
Ok(())
}
#[test]
fn read_i32_truncated_returns_err() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0x01, 0x02]); let mut d = CdrDeserializer::new(&data)?;
assert!(d.read_i32().is_err());
Ok(())
}
#[test]
fn read_f64_truncated_returns_err() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0u8; 4]); data.extend_from_slice(&[0x01, 0x02, 0x03]); let mut d = CdrDeserializer::new(&data)?;
assert!(d.read_f64().is_err());
Ok(())
}
#[test]
fn serializer_new_has_cdr_header() {
let s = CdrSerializer::new();
let bytes = s.into_bytes();
assert_eq!(&bytes[0..4], &[0x00, 0x01, 0x00, 0x00]);
}
#[test]
fn serializer_default_matches_new() {
let a = CdrSerializer::new().into_bytes();
let b = CdrSerializer::default().into_bytes();
assert_eq!(a, b);
}
#[test]
fn serializer_write_u8() {
let mut s = CdrSerializer::new();
s.write_u8(0xAB);
let bytes = s.into_bytes();
assert_eq!(bytes[4], 0xAB);
}
#[test]
fn serializer_write_i32_with_alignment() -> TestResult {
let mut s = CdrSerializer::new();
s.write_u8(1);
s.write_i32(-42);
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_u8()?, 1);
assert_eq!(d.read_i32()?, -42);
Ok(())
}
#[test]
fn serializer_write_u32_with_alignment() -> TestResult {
let mut s = CdrSerializer::new();
s.write_u8(0xFF);
s.write_u32(0xDEADBEEF);
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_u8()?, 0xFF);
assert_eq!(d.read_u32()?, 0xDEADBEEF);
Ok(())
}
#[test]
fn serializer_write_f64_with_alignment() -> TestResult {
let mut s = CdrSerializer::new();
s.write_u8(1);
s.write_f64(123.456);
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_u8()?, 1);
assert!((d.read_f64()? - 123.456).abs() < 1e-10);
Ok(())
}
#[test]
fn serializer_write_string_with_null() -> TestResult {
let mut s = CdrSerializer::new();
s.write_string("hello");
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_string()?, "hello");
Ok(())
}
#[test]
fn serializer_write_string_empty() -> TestResult {
let mut s = CdrSerializer::new();
s.write_string("");
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_string()?, "");
Ok(())
}
#[test]
fn serializer_write_byte_sequence() -> TestResult {
let mut s = CdrSerializer::new();
s.write_byte_sequence(&[0xAA, 0xBB, 0xCC]);
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_byte_sequence()?, &[0xAA, 0xBB, 0xCC]);
Ok(())
}
#[test]
fn serializer_write_byte_sequence_empty() -> TestResult {
let mut s = CdrSerializer::new();
s.write_byte_sequence(&[]);
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert!(d.read_byte_sequence()?.is_empty());
Ok(())
}
#[test]
fn serializer_multiple_primitives_round_trip() -> TestResult {
let mut s = CdrSerializer::new();
s.write_u8(42);
s.write_i32(-100);
s.write_u32(999);
s.write_f64(123.456);
s.write_string("test");
let bytes = s.into_bytes();
let mut d = CdrDeserializer::new(&bytes)?;
assert_eq!(d.read_u8()?, 42);
assert_eq!(d.read_i32()?, -100);
assert_eq!(d.read_u32()?, 999);
assert!((d.read_f64()? - 123.456).abs() < 1e-10);
assert_eq!(d.read_string()?, "test");
Ok(())
}
#[test]
fn read_f32_truncated_returns_err() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&[0x01, 0x02]);
let mut d = CdrDeserializer::new(&data)?;
assert!(d.read_f32().is_err());
Ok(())
}
#[test]
fn read_byte_sequence_truncated_returns_err() -> TestResult {
let mut data = le_header();
data.extend_from_slice(&10u32.to_le_bytes());
data.extend_from_slice(&[0x01, 0x02]);
let mut d = CdrDeserializer::new(&data)?;
assert!(d.read_byte_sequence().is_err());
Ok(())
}
}