special case handling for tf_static, need to clean up mcap_play later…

… instead of letting it get longer and longer
lucasw · Sep 9, 2024 · bc44c9b · bc44c9b
1 parent 6400abc
commit bc44c9b
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 3 deletions.
diff --git a/.github/workflows/ubuntu_22_04.yml b/.github/workflows/ubuntu_22_04.yml
@@ -52,6 +52,6 @@ jobs:
         run: |
           # TODO(lucasw) build docs, and run tests
           cd mcap_tools/mcap_tools
-          ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo build --release
+          ROS_PACKAGE_PATH=`rospack find actionlib_msgs`:`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs`:`rospack find tf2_msgs` cargo build --release
           target/release/mcap_record --version
           target/release/mcap_record --help
diff --git a/README.md b/README.md
@@ -8,7 +8,7 @@ Setup on Ubuntu 22.04 and Debian Trixie using Debian Science Team ROS1 apt packa
 
 ```
 apt install python3-rosmsg rospack-tools libsensor-msgs-dev libstd-msgs-dev libgeometry-msgs-dev ros-sensor-msgs ros-std-msgs ros-geometry-msgs cargo pkg-config librust-openssl-dev
-ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo install --git https://github.com/lucasw/mcap_tools
+ROS_PACKAGE_PATH=`rospack find actionlib_msgs`:`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs`:`rospack find tf2_msgs` cargo install --git https://github.com/lucasw/mcap_tools
 export PATH=$PATH:$HOME/.cargo/bin
 ```
 

diff --git a/mcap_tools/src/mcap_play.rs b/mcap_tools/src/mcap_play.rs
@@ -20,6 +20,8 @@ use crossterm::{
     terminal::{disable_raw_mode, enable_raw_mode},
 };
 
+use crate::misc::tf2_msgs;
+
 fn f64_secs_to_local_datetime(secs: f64) -> DateTime<chrono::prelude::Local> {
     let d = SystemTime::UNIX_EPOCH + std::time::Duration::from_secs_f64(secs);
     let utc_datetime = DateTime::<chrono::Utc>::from(d);
@@ -397,6 +399,12 @@ async fn play(
             get_wall_time() - wall_t0
         );
 
+        // TODO(lucasw) make this generic somehow?  In the case of tf_static and TFMessage we can
+        // aggregate all the static messages into one TFMessage, but what about other types of
+        // messages that are on static publishers?  (Any use cases that make that more worthy
+        // of attention?)
+        let mut tf_static_aggregated = tf2_msgs::TFMessage::default();
+
         let mut count = 0;
         for message_raw in mcap::MessageStream::new(mapped)? {
             match message_raw {
@@ -478,7 +486,32 @@ async fn play(
                             break;
                         }
 
-                        let _ = publisher.publish(&msg_with_header).await;
+                        // ugly special case to handle tf static aggregation
+                        // TODO(lucasW) this maybe be a little funny if the mcap contains
+                        // multiple static transforms with the same parent and child
+                        if channel.topic == "/tf_static" {
+                            // && channel.schema.unwrap().name == "tf2_msgs/TFMessage" {
+                            match serde_rosmsg::from_slice::<tf2_msgs::TFMessage>(&msg_with_header)
+                            {
+                                Ok(tf_msg) => {
+                                    log::info!(
+                                        "add {} transforms to tf_static",
+                                        tf_msg.transforms.len()
+                                    );
+                                    for transform in tf_msg.transforms {
+                                        tf_static_aggregated.transforms.push(transform);
+                                    }
+
+                                    let data = serde_rosmsg::to_vec(&tf_static_aggregated).unwrap();
+                                    let _ = publisher.publish(&data).await;
+                                }
+                                Err(err) => {
+                                    log::error!("{loop_count} {mcap_name} {err:?}");
+                                }
+                            }
+                        } else {
+                            let _ = publisher.publish(&msg_with_header).await;
+                        }
 
                         count += 1;
                         log::debug!(