Merge pull request #157 from Carter12s/debugging-rostopic-hz

Debugging rostopic hz
RosLibRust · Jun 11, 2024 · 883057f · 883057f
2 parents 7b66397 + 5d33bfd
commit 883057f
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 20 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -26,6 +26,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Fixed
 
 - The reconnection logic for rosbridge clients was fundamentally broken and failing to reconnect. This has been fixed.
+- Generic subscriptions coming from rospy that specified "*" as the md5sum were not properly handled. This has been fixed.
 
 ### Changed
 

diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs
@@ -88,27 +88,45 @@ impl Publication {
                         if let Ok(connection_header) =
                             ConnectionHeader::from_bytes(&connection_header[..bytes])
                         {
-                            if connection_header.md5sum == responding_conn_header.md5sum {
-                                log::debug!(
-                                    "Received subscribe request for {}",
-                                    connection_header.topic
-                                );
-                                // Write our own connection header in response
-                                let response_header_bytes = responding_conn_header
-                                    .to_bytes(false)
-                                    .expect("Couldn't serialize connection header");
-                                stream
-                                    .write(&response_header_bytes[..])
-                                    .await
-                                    .expect("Unable to respond on tcpstream");
-                                let mut wlock = subscriber_streams.write().await;
-                                wlock.push(stream);
-                                log::debug!(
-                                    "Added stream for topic {} to subscriber {}",
-                                    connection_header.topic,
-                                    peer_addr
-                                );
+                            log::debug!(
+                                "Received subscribe request for {} with md5sum {}",
+                                connection_header.topic,
+                                connection_header.md5sum
+                            );
+                            // I can't find documentation for this anywhere, but when using
+                            // `rostopic hz` with one of our publishers I discovered that the rospy code sent "*" as the md5sum
+                            // To indicate a "generic subscription"...
+                            if connection_header.md5sum != "*" {
+                                if connection_header.md5sum != responding_conn_header.md5sum {
+                                    log::warn!(
+                                    "Got subscribe request for {}, but md5sums do not match. Expected {}, received {}",
+                                    topic_name,
+                                    responding_conn_header.md5sum,
+                                    connection_header.md5sum,
+                                    );
+                                    // Close the TCP connection
+                                    stream
+                                        .shutdown()
+                                        .await
+                                        .expect("Unable to shutdown tcpstream");
+                                    continue;
+                                }
                             }
+                            // Write our own connection header in response
+                            let response_header_bytes = responding_conn_header
+                                .to_bytes(false)
+                                .expect("Couldn't serialize connection header");
+                            stream
+                                .write(&response_header_bytes[..])
+                                .await
+                                .expect("Unable to respond on tcpstream");
+                            let mut wlock = subscriber_streams.write().await;
+                            wlock.push(stream);
+                            log::debug!(
+                                "Added stream for topic {} to subscriber {}",
+                                connection_header.topic,
+                                peer_addr
+                            );
                         } else {
                             let header_str = connection_header[..bytes]
                                 .into_iter()