support topic remapping from mcap_play, and get_params is now get_par…

…ams_remaps
lucasw · Sep 15, 2024 · 5f94bb8 · 5f94bb8
1 parent 6522f17
commit 5f94bb8
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 12 deletions.
diff --git a/mcap_tools/Cargo.toml b/mcap_tools/Cargo.toml
@@ -1,6 +1,6 @@
 [package]
 name = "mcap_tools"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html

diff --git a/mcap_tools/src/mcap_play.rs b/mcap_tools/src/mcap_play.rs
@@ -61,6 +61,7 @@ async fn mcap_playback_init(
     mapped: &Mmap,
     include_re: &Option<Regex>,
     exclude_re: &Option<Regex>,
+    remaps: &HashMap<String, String>,
 ) -> Result<(HashMap<String, PublisherAny>, f64, f64), anyhow::Error> {
     // TODO(lucasw) could create every publisher from the summary
     let summary = mcap::read::Summary::read(mapped)?;
@@ -108,9 +109,17 @@ async fn mcap_playback_init(
                         None => false,
                     }
                 };
+
+                // remap a topic if it's in the remaps hashmap
+                let topic = {
+                    match remaps.get(&channel.topic) {
+                        Some(topic) => topic.to_string(),
+                        None => channel.topic.clone(),
+                    }
+                };
                 let publisher = nh
                     .advertise_any(
-                        &channel.topic,
+                        &topic,
                         &schema.name,
                         std::str::from_utf8(&schema.data.clone()).unwrap(),
                         10,
@@ -239,9 +248,14 @@ async fn main() -> Result<(), anyhow::Error> {
         .init()
         .unwrap();
 
-    let mut params = HashMap::<String, String>::new();
-    params.insert("_name".to_string(), "mcap_play".to_string());
-    let (_ns, full_node_name, unused_args) = misc::get_params(&mut params);
+    let (full_node_name, unused_args, remaps) = {
+        let mut params = HashMap::<String, String>::new();
+        params.insert("_name".to_string(), "mcap_play".to_string());
+        let mut remaps = HashMap::<String, String>::new();
+        let (_ns, full_node_name, unused_args) = misc::get_params_remaps(&mut params, &mut remaps);
+        (full_node_name, unused_args, remaps)
+    };
+
     let master_uri =
         std::env::var("ROS_MASTER_URI").unwrap_or("http://localhost:11311".to_string());
 
@@ -272,7 +286,8 @@ async fn main() -> Result<(), anyhow::Error> {
             let mapped = misc::map_mcap(mcap_name)?;
 
             // initialize the start times and publishers
-            let rv = mcap_playback_init(&nh, mcap_name, &mapped, &include_re, &exclude_re).await;
+            let rv = mcap_playback_init(&nh, mcap_name, &mapped, &include_re, &exclude_re, &remaps)
+                .await;
 
             match rv {
                 Ok((pubs, msg_t0, msg_t1)) => {

diff --git a/mcap_tools/src/mcap_record.rs b/mcap_tools/src/mcap_record.rs
@@ -274,7 +274,8 @@ async fn main() -> Result<(), anyhow::Error> {
     let mut params = HashMap::<String, String>::new();
     params.insert("_name".to_string(), "mcap_record".to_string());
     // TODO(lucasw) parse := ros arguments with clap?
-    let (ns, full_node_name, unused_args) = misc::get_params(&mut params);
+    let mut _remaps = HashMap::<String, String>::new();
+    let (ns, full_node_name, unused_args) = misc::get_params_remaps(&mut params, &mut _remaps);
 
     let matches = command!()
         .arg(

diff --git a/mcap_tools/src/misc.rs b/mcap_tools/src/misc.rs
@@ -44,7 +44,10 @@ pub async fn get_master_client(node_name: &str) -> Result<MasterClient, anyhow::
 /// params.insert("update_rate".to_string(), "5.0".to_string());
 ///
 /// returns full path node name, the namespace, and a vector of unused args
-pub fn get_params(params: &mut HashMap<String, String>) -> (String, String, Vec<String>) {
+pub fn get_params_remaps(
+    params: &mut HashMap<String, String>,
+    remaps: &mut HashMap<String, String>,
+) -> (String, String, Vec<String>) {
     // TODO(lucasw) generate a unique node name
     // let _ = params.try_insert("_name".to_string(), "node_tbd".to_string());
     if !params.contains_key("_name") {
@@ -64,14 +67,14 @@ pub fn get_params(params: &mut HashMap<String, String>) -> (String, String, Vec<
 
         let (mut key, val) = (key_val[0].to_string(), key_val[1].to_string());
         if !key.starts_with('_') {
-            println!("unused arg pair {key}:={val}- need to prefix name with underscore");
+            remaps.insert(key, val);
             continue;
         }
         key.replace_range(0..1, "");
 
         if !params.contains_key(&key) {
-            println!("unused '{key}' '{val}'");
-            continue;
+            println!("unexpected param: '{key}' '{val}'");
+            // continue;
         }
         params.insert(key, val);
     }
@@ -80,6 +83,25 @@ pub fn get_params(params: &mut HashMap<String, String>) -> (String, String, Vec<
     let ns = params.remove("_ns").unwrap();
     let full_node_name = &format!("/{}/{}", &ns, &params["_name"],).replace("//", "/");
 
+    let mut updated_remaps = HashMap::new();
+    // if the topic is relative, add the namespace here and update the remaps
+    for (topic_orig, topic_remap) in &mut *remaps {
+        let topic_orig = topic_orig.clone();
+        if !topic_remap.starts_with('/') {
+            let updated_topic_remap = format!("{ns}/{}", topic_remap).to_string();
+            updated_remaps.insert(topic_orig, updated_topic_remap);
+        } else {
+            updated_remaps.insert(topic_orig, topic_remap.to_string());
+        }
+        // TODO(lucasw) handle private topics with '~' leading?
+    }
+    *remaps = updated_remaps;
+    /*
+    for (topic_orig, topic_remap) in updated_remaps {
+        remaps.insert(topic_orig, topic_remap);
+    }
+    */
+
     (ns.to_string(), full_node_name.to_string(), args2)
 }
 

diff --git a/mcap_tools/src/rostopic_list.rs b/mcap_tools/src/rostopic_list.rs
@@ -13,7 +13,8 @@ async fn main() -> Result<(), anyhow::Error> {
     let master_client = misc::get_master_client(node_name).await?;
 
     let mut params = HashMap::<String, String>::new();
-    let (ns, _full_node_name, _unused_args) = misc::get_params(&mut params);
+    let mut _remaps = HashMap::<String, String>::new();
+    let (ns, _full_node_name, _unused_args) = misc::get_params_remaps(&mut params, &mut _remaps);
 
     let topics = master_client.get_published_topics(ns).await?;
     for (topic_name, topic_type) in topics {