From 38ce91d5976d3f362f5ec9f7e92b3237290bfff0 Mon Sep 17 00:00:00 2001
From: Lucas Walter <wsacul@gmail.com>
Date: Sun, 23 Jun 2024 09:26:15 -0700
Subject: [PATCH] decode an mcap into a ros message using roslibrust, but not
 yet sure how to do the deserialization

---
 mcap_to_rerun/Cargo.toml  | 20 +++++++++++
 mcap_to_rerun/src/main.rs | 73 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 93 insertions(+)
 create mode 100644 mcap_to_rerun/Cargo.toml
 create mode 100644 mcap_to_rerun/src/main.rs

diff --git a/mcap_to_rerun/Cargo.toml b/mcap_to_rerun/Cargo.toml
new file mode 100644
index 0000000..62bb12c
--- /dev/null
+++ b/mcap_to_rerun/Cargo.toml
@@ -0,0 +1,20 @@
+[package]
+name = "bag_odom_to_rerun"
+version = "0.1.0"
+edition = "2021"
+rust-version = "1.75"
+license = "BSD 3-Clause"
+publish = false
+
+[dependencies]
+anyhow = "1.0.86"
+camino = "1.1.7"
+mcap = "0.9.0"
+memmap = "0.7.0"
+rerun = "0.15.1"
+# roslibrust = "0.9.0"
+roslibrust_codegen = "0.9.0"
+roslibrust_codegen_macro = "0.9.0"
+serde = { version = "1.0", features = ["derive"] }
+serde-big-array = "0.5.1"
+smart-default = "0.7.1"
diff --git a/mcap_to_rerun/src/main.rs b/mcap_to_rerun/src/main.rs
new file mode 100644
index 0000000..9bfcdf2
--- /dev/null
+++ b/mcap_to_rerun/src/main.rs
@@ -0,0 +1,73 @@
+//! load mcaps with an Odometry topic and visualize in rerun.io
+
+use std::{env, fs};
+
+use anyhow::{Context, Result};
+use camino::Utf8Path;
+use memmap::Mmap;
+
+use roslibrust_codegen_macro::find_and_generate_ros_messages;
+
+find_and_generate_ros_messages!();
+
+fn print_type_of<T>(_: &T) -> String {
+    format!("{}", std::any::type_name::<T>())
+}
+
+fn map_mcap<P: AsRef<Utf8Path>>(p: P) -> Result<Mmap> {
+    let fd = fs::File::open(p.as_ref()).context("Couldn't open MCAP file")?;
+    unsafe { Mmap::map(&fd) }.context("Couldn't map MCAP file")
+}
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let rec = rerun::RecordingStreamBuilder::new("bag_odom_to_rerun").spawn()?;
+
+    let args: Vec<String> = env::args().collect();
+
+    let path = &args[1];
+    let odom_topic = &args[2];
+
+    // dbg!(args);
+    dbg!(path);
+    dbg!(odom_topic);
+
+    let mapped = map_mcap(path)?;
+
+    for message_raw in mcap::MessageStream::new(&mapped)? {
+        // println!("{:?}", print_type_of(&message));
+        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
+
+                    let ts = message.publish_time;
+                    println!(
+                        "{} {} [{}] [{}]...",
+                        ts,
+                        message.channel.topic,
+                        message
+                            .channel
+                            .schema
+                            .as_ref()
+                            .map(|s| s.name.as_str())
+                            .unwrap_or_default(),
+                        message
+                            .data
+                            .iter()
+                            .take(10)
+                            .map(|b| b.to_string())
+                            .collect::<Vec<_>>()
+                            .join(" ")
+                    );
+                }
+            }
+            Err(e) => {
+                println!("{:?}", e);
+            },
+        }
+    }
+    Ok(())
+}