From f0597576dc49cada08114a8c493bc7a8e471f367 Mon Sep 17 00:00:00 2001
From: Martin Dahl <martin.dahl@gmail.com>
Date: Tue, 27 Aug 2019 14:21:53 +0200
Subject: [PATCH] Examples for node querying and "untyped" publish/subscribe

---
 examples/rostopic_echo.rs | 49 +++++++++++++++++++++++++++++++++++++++
 examples/rostopic_list.rs | 19 +++++++++++++++
 src/lib.rs                |  7 +-----
 tests/threads.rs          |  1 -
 4 files changed, 69 insertions(+), 7 deletions(-)
 create mode 100644 examples/rostopic_echo.rs
 create mode 100644 examples/rostopic_list.rs

diff --git a/examples/rostopic_echo.rs b/examples/rostopic_echo.rs
new file mode 100644
index 0000000..76b76b2
--- /dev/null
+++ b/examples/rostopic_echo.rs
@@ -0,0 +1,49 @@
+use r2r::*;
+use std::thread;
+use std::env;
+use std::collections::HashMap;
+
+fn main() -> Result<(), ()> {
+    let ctx = Context::create()?;
+    let mut node = Node::create(ctx, "echo", "")?;
+
+    let args: Vec<String> = env::args().collect();
+    let topic = args.get(1).expect("provide a topic!");
+
+    // run for a while to populate the topic list
+    let mut count = 0;
+    let mut nt = HashMap::new();
+    while count < 50 {
+        thread::sleep(std::time::Duration::from_millis(10));
+        nt = node.get_topic_names_and_types()?;
+        if nt.contains_key(topic) { break; }
+        count += 1;
+    }
+
+    let type_name = nt.get(topic).and_then(|types|types.get(0));
+    let type_name = match type_name {
+        Some(tn) => tn,
+        None => {
+            eprintln!("Could not determine the type for the passed topic");
+            return Err(());
+        },
+    };
+
+    println!("topic: {}, type: {}", topic, type_name);
+
+    // create echo topic
+    let echo = &format!("{}_echo", topic);
+    let echo_pub = node.create_publisher_untyped(echo, type_name)?;
+
+    let cb = move |msg: serde_json::Value | {
+        let s = serde_json::to_string_pretty(&msg).unwrap();
+        println!("{}\n---\n", &s);
+        echo_pub.publish(msg).unwrap();
+    };
+
+    let _subref = node.subscribe_untyped(topic, type_name, Box::new(cb))?;
+
+    loop {
+        node.spin_once(std::time::Duration::from_millis(100));
+    }
+}
diff --git a/examples/rostopic_list.rs b/examples/rostopic_list.rs
new file mode 100644
index 0000000..d016ee2
--- /dev/null
+++ b/examples/rostopic_list.rs
@@ -0,0 +1,19 @@
+use r2r;
+use std::thread;
+use std::time::Duration;
+
+fn main() -> Result<(), ()> {
+    let ctx = r2r::Context::create()?;
+    let node = r2r::Node::create(ctx, "testnode", "")?;
+
+    loop {
+        let nt = node.get_topic_names_and_types()?;
+        let mut keys: Vec<&String> = nt.keys().collect();
+        keys.sort();
+        println!("---------------------");
+        for k in keys {
+            println!("{}: {:?}", k, nt[k]);
+        }
+        thread::sleep(Duration::from_millis(500));
+    };
+}
diff --git a/src/lib.rs b/src/lib.rs
index 0b3d21a..a6e4ac4 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -430,12 +430,7 @@ impl Node {
     }
 
     pub fn create_publisher_untyped(&mut self, topic: &str, topic_type: &str) -> Result<PublisherUntyped, ()> {
-        let ts = if topic_type == "std_msgs/msg/String" {
-            std_msgs::msg::String::get_ts()
-        } else {
-            return Err(());
-        };
-
+        let ts = untyped_ts_helper(topic_type)?;
         let mut publisher_handle = unsafe { rcl_get_zero_initialized_publisher() };
         let topic_c_string = CString::new(topic).unwrap();
 
diff --git a/tests/threads.rs b/tests/threads.rs
index b547535..b8d8f2d 100644
--- a/tests/threads.rs
+++ b/tests/threads.rs
@@ -1,4 +1,3 @@
-use std::sync::mpsc;
 use std::thread;
 use std::time::Duration;