Merge pull request RosLibRust#192 from lucasw/subscriber_any2

subscribe to any topic of any type and get received messages as a raw vector of bytes

CHANGELOG.md

@@ -26,6 +26,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - ROS1 Native Publishers now support latching behavior
 - The XML RPC client for interacting directly with the rosmaster server has been exposed as a public API
 - Experimental: Initial support for writing generic clients that can be compile time specialized for rosbridge or ros1
+- Can subscribe to any topic and get raw bytes instead of a deserialized message of known type
 
 ### Fixed
 

roslibrust/src/ros1/node/handle.rs

@@ -2,7 +2,7 @@ use super::actor::{Node, NodeServerHandle};
 use crate::{
     ros1::{
         names::Name, publisher::Publisher, service_client::ServiceClient, subscriber::Subscriber,
-        NodeError, ServiceServer,
+        subscriber::SubscriberAny, NodeError, ServiceServer,
     },
     ServiceFn,
 };
@@ -75,6 +75,18 @@ impl NodeHandle {
         Ok(Publisher::new(topic_name, sender))
     }
 
+    pub async fn subscribe_any(
+        &self,
+        topic_name: &str,
+        queue_size: usize,
+    ) -> Result<SubscriberAny, NodeError> {
+        let receiver = self
+            .inner
+            .register_subscriber::<roslibrust_codegen::ShapeShifter>(topic_name, queue_size)
+            .await?;
+        Ok(SubscriberAny::new(receiver))
+    }
+
     pub async fn subscribe<T: roslibrust_codegen::RosMessageType>(
         &self,
         topic_name: &str,

roslibrust/src/ros1/subscriber.rs

@@ -1,6 +1,6 @@
 use crate::ros1::{names::Name, tcpros::ConnectionHeader};
 use abort_on_drop::ChildTask;
-use roslibrust_codegen::RosMessageType;
+use roslibrust_codegen::{RosMessageType, ShapeShifter};
 use std::{marker::PhantomData, sync::Arc};
 use tokio::{
     io::AsyncWriteExt,
@@ -39,6 +39,30 @@ impl<T: RosMessageType> Subscriber<T> {
     }
 }
 
+pub struct SubscriberAny {
+    receiver: broadcast::Receiver<Vec<u8>>,
+    _phantom: PhantomData<ShapeShifter>,
+}
+
+impl SubscriberAny {
+    pub(crate) fn new(receiver: broadcast::Receiver<Vec<u8>>) -> Self {
+        Self {
+            receiver,
+            _phantom: PhantomData,
+        }
+    }
+
+    // pub async fn next(&mut self) -> Option<Result<ShapeShifter, SubscriberError>> {
+    pub async fn next(&mut self) -> Option<Result<Vec<u8>, SubscriberError>> {
+        let data = match self.receiver.recv().await {
+            Ok(v) => v,
+            Err(RecvError::Closed) => return None,
+            Err(RecvError::Lagged(n)) => return Some(Err(SubscriberError::Lagged(n))),
+        };
+        Some(Ok(data))
+    }
+}
+
 pub struct Subscription {
     subscription_tasks: Vec<ChildTask<()>>,
     _msg_receiver: broadcast::Receiver<Vec<u8>>,
@@ -154,7 +178,9 @@ async fn establish_publisher_connection(
     stream.write_all(&conn_header_bytes[..]).await?;
 
     if let Ok(responded_header) = tcpros::receive_header(&mut stream).await {
-        if conn_header.md5sum == responded_header.md5sum {
+        if conn_header.md5sum == Some("*".to_string())
+            || conn_header.md5sum == responded_header.md5sum
+        {
             log::debug!(
                 "Established connection with publisher for {:?}",
                 conn_header.topic

roslibrust/tests/ros1_native_integration_tests.rs

@@ -12,6 +12,33 @@ mod tests {
         "assets/ros1_common_interfaces"
     );
 
+    #[test_log::test(tokio::test)]
+    async fn test_subscribe_any() {
+        // get a single message in raw bytes and test the bytes are as expected
+        let nh = NodeHandle::new("http://localhost:11311", "test_subscribe_any")
+            .await
+            .unwrap();
+
+        let publisher = nh
+            .advertise::<std_msgs::String>("/test_subscribe_any", 1, true)
+            .await
+            .unwrap();
+
+        let mut subscriber = nh.subscribe_any("/test_subscribe_any", 1).await.unwrap();
+
+        publisher
+            .publish(&std_msgs::String {
+                data: "test".to_owned(),
+            })
+            .await
+            .unwrap();
+
+        let res =
+            tokio::time::timeout(tokio::time::Duration::from_millis(250), subscriber.next()).await;
+        let res = res.unwrap().unwrap().unwrap();
+        assert!(res == vec![8, 0, 0, 0, 4, 0, 0, 0, 116, 101, 115, 116]);
+    }
+
     #[test_log::test(tokio::test)]
     async fn test_latching() {
         let nh = NodeHandle::new("http://localhost:11311", "test_latching")

roslibrust_codegen/src/integral_types.rs

@@ -36,6 +36,16 @@ impl RosMessageType for Time {
     const DEFINITION: &'static str = "";
 }
 
+#[derive(:: serde :: Deserialize, :: serde :: Serialize, Debug, Default, Clone, PartialEq)]
+pub struct ShapeShifter(Vec<u8>);
+
+// The equivalent of rospy AnyMsg or C++ ShapeShifter, subscribe_any() uses this type
+impl RosMessageType for ShapeShifter {
+    const ROS_TYPE_NAME: &'static str = "*";
+    const MD5SUM: &'static str = "*";
+    const DEFINITION: &'static str = "";
+}
+
 // TODO provide chrono conversions here behind a cfg flag
 
 /// Matches the integral ros1 duration type, with extensions for ease of use