now can convert from mcap for Float32Stamped, restore Odometry next

lucasw · Jun 23, 2024 · d80f231 · d80f231
1 parent 38ce91d
commit d80f231
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 4 deletions.
diff --git a/mcap_to_rerun/Cargo.toml b/mcap_to_rerun/Cargo.toml
@@ -17,4 +17,5 @@ roslibrust_codegen = "0.9.0"
 roslibrust_codegen_macro = "0.9.0"
 serde = { version = "1.0", features = ["derive"] }
 serde-big-array = "0.5.1"
+serde_rosmsg = "0.2.0"
 smart-default = "0.7.1"
diff --git a/mcap_to_rerun/src/main.rs b/mcap_to_rerun/src/main.rs
@@ -38,11 +38,39 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         match message_raw {
             Ok(message) => {
                 if message.channel.topic == *odom_topic {  // && message.channel.schema == "nav_msgs/Odometry" {
-                    println!("{:?}", message.channel.schema);
-                    let ros_msg = nav_msgs::Odometry::default();
-                    println!("{:#?}", ros_msg);
-                    // TODO(lucasw) do something with serde to deserialize/decode message.data
+                    // println!("{:?}", message.channel.schema);
 
+                    // https://github.com/adnanademovic/serde_rosmsg/blob/master/src/lib.rs#L9
+                    let len_header = message.data.len() as u32;
+                    let mut msg_with_header = Vec::from(len_header.to_le_bytes());
+                    let mut message_data = Vec::from(message.data.clone());
+                    // println!("{:?}", print_type_of(&message));
+                    msg_with_header.append(&mut message_data);
+
+                    /*
+                    {
+                        let mut test_msg = marti_common_msgs::Float32Stamped::default();
+                        test_msg.header.seq = 3;
+                        test_msg.header.frame_id = "test".to_string();
+                        test_msg.header.stamp.secs = 1;
+                        test_msg.header.stamp.nsecs = 7;
+                        test_msg.value = 1.0;
+                        let test_data = serde_rosmsg::to_vec(&test_msg).unwrap();
+                        println!("test {} - {:02x?}", test_data.len(), &test_data[..]);
+                        println!("mcap {} - {:02x?}", msg_with_header_vec.len(), &msg_with_header_vec[..]);
+                    }
+                    */
+
+                    // match serde_rosmsg::from_slice::<nav_msgs::Odometry>(&msg_with_header) {
+                    match serde_rosmsg::from_slice::<marti_common_msgs::Float32Stamped>(&msg_with_header) {
+                        Ok(odom_msg) => {
+                            println!("{:#?}", odom_msg.header);
+                        },
+                        Err(e) => {
+                            println!("{:?}", e);
+                        },
+                    }
+                    /*
                     let ts = message.publish_time;
                     println!(
                         "{} {} [{}] [{}]...",
@@ -62,6 +90,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
                             .collect::<Vec<_>>()
                             .join(" ")
                     );
+                    */
                 }
             }
             Err(e) => {