fix(bag): use ROS1 alignment rules for headerless BAG message decoding

examples/test_bag_decode_small.rs

@@ -0,0 +1,61 @@
+use robocodec::io::formats::bag::BagFormat;
+use robocodec::FormatReader;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let path = "/Users/zhexuany/Downloads/leju_bag/Rubbish_sorting_P4-278_20250830101814.bag";
+    let reader = BagFormat::open(path)?;
+
+    println!("Opened bag file");
+    println!("Channels: {}", reader.channels().len());
+    println!("Total messages: {}", reader.message_count());
+
+    // Try to decode messages
+    let decoded_iter = reader.decode_messages()?;
+    let mut stream = decoded_iter.stream()?;
+
+    let mut count = 0;
+    let mut errors = 0;
+    let mut metadata_count = 0;
+
+    for result in &mut stream {
+        match result {
+            Ok((msg, channel)) => {
+                count += 1;
+                if channel.topic.contains("metadata") {
+                    metadata_count += 1;
+                    if metadata_count <= 3 {
+                        println!(
+                            "Metadata message {}: topic={}, fields={}",
+                            metadata_count,
+                            channel.topic,
+                            msg.len()
+                        );
+                    }
+                }
+                if count <= 5 {
+                    println!(
+                        "Message {}: topic={}, fields={}",
+                        count,
+                        channel.topic,
+                        msg.len()
+                    );
+                }
+            }
+            Err(e) => {
+                errors += 1;
+                if errors <= 5 {
+                    eprintln!("Error {}: {}", errors, e);
+                }
+            }
+        }
+        if count >= 100 || errors >= 100 {
+            break;
+        }
+    }
+
+    println!("\nSuccessfully decoded {} messages", count);
+    println!("Metadata messages decoded: {}", metadata_count);
+    println!("Total errors: {}", errors);
+
+    Ok(())
+}

examples/test_bag_dump.rs

@@ -0,0 +1,26 @@
+use robocodec::io::formats::bag::BagFormat;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let path = "/Users/zhexuany/Downloads/leju_bag/Rubbish_sorting_P4-278_20250830101814.bag";
+    let reader = BagFormat::open(path)?;
+
+    let mut iter = reader.iter_raw()?;
+
+    // Look at first few messages
+    for i in 0..5 {
+        if let Some(Ok((msg, channel))) = iter.next() {
+            println!(
+                "Message {}: topic={}, data_len={}",
+                i,
+                channel.topic,
+                msg.data.len()
+            );
+            println!(
+                "  First 32 bytes: {:02x?}",
+                &msg.data[..msg.data.len().min(32)]
+            );
+        }
+    }
+
+    Ok(())
+}

examples/test_decode_debug.rs

@@ -0,0 +1,64 @@
+use robocodec::encoding::CdrDecoder;
+use robocodec::io::formats::bag::BagFormat;
+use robocodec::schema::parse_schema;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let path = "/Users/zhexuany/Downloads/leju_bag/Rubbish_sorting_P4-278_20250830101814.bag";
+    let reader = BagFormat::open(path)?;
+
+    let mut iter = reader.iter_raw()?;
+
+    // Find a simple message to debug
+    while let Some(Ok((msg, channel))) = iter.next() {
+        // Try the metadata message which has a simple structure
+        if channel.topic.contains("metadata") {
+            println!("Topic: {}", channel.topic);
+            println!("Type: {}", channel.message_type);
+            println!("Data length: {}", msg.data.len());
+            println!(
+                "First 64 bytes: {:02x?}",
+                &msg.data[..msg.data.len().min(64)]
+            );
+
+            // Parse the schema
+            if let Some(schema_str) = &channel.schema {
+                println!("\nSchema:\n{}", schema_str);
+
+                // Try to parse and decode
+                match parse_schema(&channel.message_type, schema_str) {
+                    Ok(schema) => {
+                        println!("\nParsed schema successfully");
+                        println!(
+                            "Schema types: {:?}",
+                            schema.types.keys().collect::<Vec<_>>()
+                        );
+
+                        // Try decoding
+                        let decoder = CdrDecoder::new();
+                        match decoder.decode_headerless_ros1(
+                            &schema,
+                            &msg.data,
+                            Some(&channel.message_type),
+                        ) {
+                            Ok(decoded) => {
+                                println!("\nDecoded successfully!");
+                                for (k, v) in decoded.iter() {
+                                    println!("  {}: {:?}", k, v);
+                                }
+                            }
+                            Err(e) => {
+                                println!("\nDecode error: {}", e);
+                            }
+                        }
+                    }
+                    Err(e) => {
+                        println!("\nSchema parse error: {}", e);
+                    }
+                }
+            }
+            break;
+        }
+    }
+
+    Ok(())
+}

examples/test_decode_trace.rs

@@ -0,0 +1,80 @@
+use robocodec::encoding::cdr::cursor::CdrCursor;
+use robocodec::io::formats::bag::BagFormat;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let path = "/Users/zhexuany/Downloads/leju_bag/Rubbish_sorting_P4-278_20250830101814.bag";
+    let reader = BagFormat::open(path)?;
+
+    let mut iter = reader.iter_raw()?;
+
+    // Find a metadata message
+    for result in &mut iter {
+        let Ok((msg, channel)) = result else { continue };
+        if !channel.topic.contains("metadata") {
+            continue;
+        }
+
+        println!("Topic: {}", channel.topic);
+        println!("Data length: {}", msg.data.len());
+
+        // Create a ROS1 cursor and manually decode
+        let mut cursor = CdrCursor::new_headerless_ros1(&msg.data, true);
+
+        println!("\nManual decoding:");
+        println!("is_ros1: {}", cursor.is_ros1());
+
+        // Read Header.seq (uint32)
+        let seq = cursor.read_u32()?;
+        println!("Header.seq = {} (offset now: {})", seq, cursor.position());
+
+        // Read Header.stamp.sec (int32)
+        let stamp_sec = cursor.read_i32()?;
+        println!(
+            "Header.stamp.sec = {} (offset now: {})",
+            stamp_sec,
+            cursor.position()
+        );
+
+        // Read Header.stamp.nsec (uint32)
+        let stamp_nsec = cursor.read_u32()?;
+        println!(
+            "Header.stamp.nsec = {} (offset now: {})",
+            stamp_nsec,
+            cursor.position()
+        );
+
+        // Read Header.frame_id length (uint32)
+        let frame_id_len = cursor.read_u32()?;
+        println!(
+            "Header.frame_id length = {} (offset now: {})",
+            frame_id_len,
+            cursor.position()
+        );
+
+        // Read Header.frame_id string
+        let frame_id_bytes = cursor.read_bytes(frame_id_len as usize)?;
+        let frame_id = String::from_utf8_lossy(frame_id_bytes);
+        println!(
+            "Header.frame_id = '{}' (offset now: {})",
+            frame_id,
+            cursor.position()
+        );
+
+        // Read json_data length (uint32)
+        let json_data_len = cursor.read_u32()?;
+        println!(
+            "json_data length = {} (offset now: {})",
+            json_data_len,
+            cursor.position()
+        );
+
+        // Read json_data string (partial)
+        let json_data_bytes = cursor.read_bytes(json_data_len.min(50) as usize)?;
+        let json_data = String::from_utf8_lossy(json_data_bytes);
+        println!("json_data (partial) = '{}'", json_data);
+
+        break;
+    }
+
+    Ok(())
+}

examples/test_fixture_decode.rs

@@ -0,0 +1,71 @@
+use robocodec::io::formats::bag::BagFormat;
+use std::path::Path;
+
+fn main() {
+    let bag_path = "tests/fixtures/robocodec_test_15.bag";
+
+    if !Path::new(bag_path).exists() {
+        println!("Fixture file not found");
+        return;
+    }
+
+    let reader = BagFormat::open(bag_path).expect("Failed to open BAG file");
+
+    // Get raw messages using iter_raw
+    let raw_iter = reader.iter_raw().expect("Failed to get raw iterator");
+
+    // Print first few messages with raw data
+    for (idx, result) in raw_iter.enumerate() {
+        if idx >= 5 {
+            break;
+        }
+        match result {
+            Ok((msg, channel)) => {
+                println!("\n=== Message {} ===", idx + 1);
+                println!("Topic: {}", channel.topic);
+                println!("Type: {}", channel.message_type);
+                println!("Data length: {} bytes", msg.data.len());
+
+                // Print first 64 bytes as hex
+                let hex: Vec<String> = msg
+                    .data
+                    .iter()
+                    .take(64)
+                    .map(|b| format!("{:02x}", b))
+                    .collect();
+                println!("First 64 bytes: {}", hex.join(" "));
+
+                // Try to interpret as ROS1 message with header
+                if msg.data.len() >= 16 {
+                    let seq =
+                        u32::from_le_bytes([msg.data[0], msg.data[1], msg.data[2], msg.data[3]]);
+                    let sec =
+                        u32::from_le_bytes([msg.data[4], msg.data[5], msg.data[6], msg.data[7]]);
+                    let nsec =
+                        u32::from_le_bytes([msg.data[8], msg.data[9], msg.data[10], msg.data[11]]);
+                    let str_len = u32::from_le_bytes([
+                        msg.data[12],
+                        msg.data[13],
+                        msg.data[14],
+                        msg.data[15],
+                    ]);
+                    println!("Interpreted as ROS1 header:");
+                    println!("  seq: {}", seq);
+                    println!("  stamp.sec: {}", sec);
+                    println!("  stamp.nsec: {}", nsec);
+                    println!("  frame_id length: {}", str_len);
+
+                    if str_len < 1000 && (16 + str_len as usize) <= msg.data.len() {
+                        let frame_id =
+                            String::from_utf8_lossy(&msg.data[16..16 + str_len as usize]);
+                        println!("  frame_id: \"{}\"", frame_id);
+                    }
+                }
+            }
+            Err(e) => {
+                println!("\n=== Message {} (ERROR) ===", idx + 1);
+                println!("Error: {:?}", e);
+            }
+        }
+    }
+}

examples/test_read_mcap.rs

@@ -0,0 +1,41 @@
+use robocodec::io::formats::mcap::McapFormat;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let mcap_path = "/tmp/leju_bag.mcap";
+
+    let reader = McapFormat::open(mcap_path)?;
+    println!("Opened MCAP file");
+    println!("Channels: {}", reader.channels().len());
+
+    // Try to decode messages
+    let decoded_iter = reader.decode_messages()?;
+    let mut stream = decoded_iter.stream()?;
+
+    let mut count = 0;
+    for result in &mut stream {
+        match result {
+            Ok((msg, channel)) => {
+                count += 1;
+                if count <= 5 {
+                    println!(
+                        "Message {}: topic={}, fields={}",
+                        count,
+                        channel.topic,
+                        msg.len()
+                    );
+                }
+            }
+            Err(e) => {
+                if count < 5 {
+                    eprintln!("Error {}: {}", count + 1, e);
+                }
+            }
+        }
+        if count >= 100 {
+            break;
+        }
+    }
+
+    println!("\nSuccessfully decoded {} messages from MCAP", count);
+    Ok(())
+}

examples/test_ros_version.rs

@@ -0,0 +1,15 @@
+use robocodec::schema::RosVersion;
+
+fn main() {
+    let types = [
+        "realsense2_camera/Metadata",
+        "std_msgs/Header",
+        "kuavo_msgs/sensorsData",
+        "sensor_msgs/CompressedImage",
+    ];
+
+    for t in types {
+        let version = RosVersion::from_type_name(t);
+        println!("{}: {:?}", t, version);
+    }
+}