diff --git a/mcap_to_rerun/Cargo.toml b/mcap_to_rerun/Cargo.toml index 62bb12c..fce456a 100644 --- 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 index 9bfcdf2..09e5b0d 100644 --- a/mcap_to_rerun/src/main.rs +++ b/mcap_to_rerun/src/main.rs @@ -38,11 +38,39 @@ fn main() -> Result<(), Box> { 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::(&msg_with_header) { + match serde_rosmsg::from_slice::(&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> { .collect::>() .join(" ") ); + */ } } Err(e) => {