add optional /clock publishing to mcap_play, now need rosgraph_msgs p…

…ackage path

.github/workflows/ubuntu_22_04.yml

@@ -46,13 +46,14 @@ jobs:
           sudo apt install -yqq 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
           sudo apt install -yqq libstd-srvs-dev python3-std-srvs ros-std-srvs
           sudo apt install -yqq libtf2-msgs-dev python3-tf2-msgs ros-tf2-msgs
+          sudo apt install -yqq libros-rosgraph-msgs-dev python3-rosgraph-msgs ros-rosgraph-msgs
           sudo apt install -yqq librust-openssl-dev
           rospack find geometry_msgs
 
       - name:
         run: |
           # TODO(lucasw) build docs, and run tests
           cd mcap_tools/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 build --release
+          ROS_PACKAGE_PATH=`rospack find actionlib_msgs`:`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find rosgraph_msgs`:`rospack find std_srvs`:`rospack find tf2_msgs` cargo build --release
           target/release/mcap_record --version
           target/release/mcap_record --help

README.md

@@ -8,7 +8,8 @@ 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 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
+ROS_PACKAGE_PATH=`rospack find actionlib_msgs`:`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find rosgraph_msgs`:`rospack find std_srvs`:`rospack find tf2_msgs`
+cargo install --git https://github.com/lucasw/mcap_tools
 export PATH=$PATH:$HOME/.cargo/bin
 ```
 
@@ -43,25 +44,29 @@ data_2024_08_26_09_15_08_-07_00_00001.mcap
 ## build
 
 ```
-ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo build --release
+# use ROS_PACKAGE_PATH from above
+cargo build --release
 ```
 
 ### build and run in one line (for development)
 
 Record all topics with sensors or odom in the full topic name to an mcap:
 ```
-ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo run --release --bin mcap_record -- --regex "(.*)sensors(.*)|(.*)odom(.*)"
+# use ROS_PACKAGE_PATH from above
+cargo run --release --bin mcap_record -- --regex "(.*)sensors(.*)|(.*)odom(.*)"
 ```
 
 ```
-ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo run --release --bin mcap_extract /path/to/some.mcap
+# use ROS_PACKAGE_PATH from above
+cargo run --release --bin mcap_extract /path/to/some.mcap
 ```
 
 ```
 # do a git commit before this to see changes
 cargo fmt --all
 # this only provides suggestions, doesn't change anything
-ROS_PACKAGE_PATH=`rospack find std_msgs`:`rospack find sensor_msgs`:`rospack find geometry_msgs`:`rospack find std_srvs` cargo clippy
+# use ROS_PACKAGE_PATH from above
+cargo clippy
 ```
 
 # existing mcap tools and information

mcap_tools/src/mcap_play.rs

@@ -20,7 +20,7 @@ use crossterm::{
     terminal::{disable_raw_mode, enable_raw_mode},
 };
 
-use crate::misc::tf2_msgs;
+use crate::misc::{rosgraph_msgs, 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);
@@ -148,6 +148,13 @@ async fn main() -> Result<(), anyhow::Error> {
             .value_parser(clap::value_parser!(u64))
             .required(false)
         )
+        .arg(
+            arg!(
+                -c --clock <CLOCK> "publish the clock time, most useful in combination with rosparam set /use_sim_time true"
+            )
+            .action(clap::ArgAction::SetTrue)
+            .required(false)
+        )
         .arg(
             arg!(
                 -e --regex <REGEX> "match topics using regular expressions"
@@ -172,6 +179,10 @@ async fn main() -> Result<(), anyhow::Error> {
     let max_loops = matches.get_one::<u64>("loop").copied();
     log::info!("max loop {max_loops:?}");
 
+    // TODO(lucasw) error if there is a /clock message in an mcap
+    let publish_clock = matches.get_one::<bool>("clock").copied().unwrap();
+    log::info!("publishing clock");
+
     let include_re;
     if let Some(regex_str) = matches.get_one::<String>("regex") {
         include_re = Some(Regex::new(regex_str)?);
@@ -253,6 +264,19 @@ async fn main() -> Result<(), anyhow::Error> {
 
         tokio::time::sleep(tokio::time::Duration::from_secs(1)).await;
 
+        let clock_publisher = {
+            let latching = false;
+            let queue_size = 10;
+            match publish_clock {
+                true => Some(
+                    nh.advertise::<rosgraph_msgs::Clock>("/clock", queue_size, latching)
+                        .await
+                        .unwrap(),
+                ),
+                false => None,
+            }
+        };
+
         // a flag to exit playback early
         let mut finish_playback = false;
         let mut loop_count = 0;
@@ -276,9 +300,20 @@ async fn main() -> Result<(), anyhow::Error> {
                     if playback_elapsed > duration + 1.0 {
                         break;
                     }
+
+                    if let Some(ref clock_publisher) = clock_publisher {
+                        let clock_seconds = msg_t_start + playback_elapsed;
+                        let clock_msg = rosgraph_msgs::Clock {
+                            clock: roslibrust_codegen::Time {
+                                secs: clock_seconds as u32,
+                                nsecs: ((clock_seconds % 1.0) * (1e9 as f64)) as u32,
+                            },
+                        };
+                        clock_publisher.publish(&clock_msg).await?;
+                    }
                 }
 
-                let mut delay = Delay::new(Duration::from_millis(1_000)).fuse();
+                let mut delay = Delay::new(Duration::from_millis(10)).fuse();
                 let mut event = term_reader.next().fuse();
 
                 select! {