diff --git a/.github/workflows/ubuntu_22_04.yml b/.github/workflows/ubuntu_22_04.yml index 117227e..97ff7ce 100644 --- 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 index d72a666..927f518 100644 --- 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 index 4a8df8e..25f40ca 100644 --- 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 { let d = SystemTime::UNIX_EPOCH + std::time::Duration::from_secs_f64(secs); let utc_datetime = DateTime::::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::(&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!(