From f719e1f0536c932cebe4179e3e7d3deaa79a5d4a Mon Sep 17 00:00:00 2001 From: Carter Date: Sat, 29 Jun 2024 11:24:20 -0600 Subject: [PATCH] Fix rostopic echo not working because it doesn't provide md5sum --- roslibrust/examples/ros1_talker.rs | 2 +- roslibrust/src/ros1/publisher.rs | 33 +++++++++++++++------------ roslibrust/src/ros1/service_client.rs | 2 +- roslibrust/src/ros1/service_server.rs | 2 +- roslibrust/src/ros1/subscriber.rs | 4 ++-- roslibrust/src/ros1/tcpros.rs | 16 ++++++++----- 6 files changed, 34 insertions(+), 25 deletions(-) diff --git a/roslibrust/examples/ros1_talker.rs b/roslibrust/examples/ros1_talker.rs index 3bcfd0ed..315e03ab 100644 --- a/roslibrust/examples/ros1_talker.rs +++ b/roslibrust/examples/ros1_talker.rs @@ -16,7 +16,7 @@ async fn main() -> Result<(), anyhow::Error> { .map_err(|err| err)?; let publisher = nh.advertise::("/chatter", 1).await?; - for count in 0..10 { + for count in 0..50 { publisher .publish(&std_msgs::String { data: format!("hello world from rust {count}"), diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index b3b46ebe..213350ec 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -76,7 +76,7 @@ impl Publication { caller_id: node_name.to_string(), latching, msg_definition: msg_definition.to_owned(), - md5sum: md5sum.to_owned(), + md5sum: Some(md5sum.to_owned()), topic: Some(topic_name.to_owned()), topic_type: topic_type.to_owned(), tcp_nodelay: false, @@ -100,27 +100,32 @@ impl Publication { ConnectionHeader::from_bytes(&connection_header[..bytes]) { log::debug!( - "Received subscribe request for {:?} with md5sum {}", + "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 {}", + // I also discovered that `rostopic echo` does not send a md5sum (even thou ros documentation says its required) + if let Some(connection_md5sum) = connection_header.md5sum { + if connection_md5sum != "*" { + if let Some(local_md5sum) = &responding_conn_header.md5sum { + if connection_md5sum != *local_md5sum { + log::warn!( + "Got subscribe request for {}, but md5sums do not match. Expected {:?}, received {:?}", topic_name, - responding_conn_header.md5sum, - connection_header.md5sum, + local_md5sum, + connection_md5sum, ); - // Close the TCP connection - stream - .shutdown() - .await - .expect("Unable to shutdown tcpstream"); - continue; + // Close the TCP connection + stream + .shutdown() + .await + .expect("Unable to shutdown tcpstream"); + continue; + } + } } } // Write our own connection header in response diff --git a/roslibrust/src/ros1/service_client.rs b/roslibrust/src/ros1/service_client.rs index 5fd01ccf..f0e1a136 100644 --- a/roslibrust/src/ros1/service_client.rs +++ b/roslibrust/src/ros1/service_client.rs @@ -97,7 +97,7 @@ impl ServiceClientLink { caller_id: node_name.to_string(), latching: false, msg_definition: srv_definition.to_owned(), - md5sum: md5sum.to_owned(), + md5sum: Some(md5sum.to_owned()), // Note: using "topic" indicates a subscription // using "service" indicates a service client topic: None, diff --git a/roslibrust/src/ros1/service_server.rs b/roslibrust/src/ros1/service_server.rs index cb4b58c5..6f45098e 100644 --- a/roslibrust/src/ros1/service_server.rs +++ b/roslibrust/src/ros1/service_server.rs @@ -169,7 +169,7 @@ impl ServiceServerLink { caller_id: node_name.to_string(), latching: false, msg_definition: "".to_string(), - md5sum: "".to_string(), + md5sum: None, service: None, topic: None, topic_type: "".to_string(), diff --git a/roslibrust/src/ros1/subscriber.rs b/roslibrust/src/ros1/subscriber.rs index 07c662d2..ebc6885b 100644 --- a/roslibrust/src/ros1/subscriber.rs +++ b/roslibrust/src/ros1/subscriber.rs @@ -59,7 +59,7 @@ impl Subscription { caller_id: node_name.to_string(), latching: false, msg_definition, - md5sum, + md5sum: Some(md5sum), topic: Some(topic_name.to_owned()), topic_type: topic_type.to_owned(), tcp_nodelay: false, @@ -168,7 +168,7 @@ async fn establish_publisher_connection( Ok(stream) } else { log::error!( - "Tried to subscribe to {}, but md5sums do not match. Expected {}, received {}", + "Tried to subscribe to {}, but md5sums do not match. Expected {:?}, received {:?}", topic_name, conn_header.md5sum, responded_header.md5sum diff --git a/roslibrust/src/ros1/tcpros.rs b/roslibrust/src/ros1/tcpros.rs index 9bb23426..16096193 100644 --- a/roslibrust/src/ros1/tcpros.rs +++ b/roslibrust/src/ros1/tcpros.rs @@ -15,7 +15,7 @@ pub struct ConnectionHeader { pub caller_id: String, pub latching: bool, pub msg_definition: String, - pub md5sum: String, + pub md5sum: Option, // TODO we may want to distinguish between service and topic headers with different types? pub service: Option, pub topic: Option, @@ -44,7 +44,7 @@ impl ConnectionHeader { let mut msg_definition = String::new(); let mut caller_id = String::new(); let mut latching = false; - let mut md5sum = String::new(); + let mut md5sum = None; let mut topic = None; let mut service = None; let mut topic_type = String::new(); @@ -72,7 +72,9 @@ impl ConnectionHeader { field[equals_pos + 1..].clone_into(&mut latching_str); latching = &latching_str != "0"; } else if field.starts_with("md5sum=") { - field[equals_pos + 1..].clone_into(&mut md5sum); + let mut md5sum_str = String::new(); + field[equals_pos + 1..].clone_into(&mut md5sum_str); + md5sum = Some(md5sum_str); } else if field.starts_with("topic=") { let mut topic_str = String::new(); field[equals_pos + 1..].clone_into(&mut topic_str); @@ -122,9 +124,11 @@ impl ConnectionHeader { header_data.write_u32::(latching_str.len() as u32)?; header_data.write(latching_str.as_bytes())?; - let md5sum = format!("md5sum={}", self.md5sum); - header_data.write_u32::(md5sum.len() as u32)?; - header_data.write(md5sum.as_bytes())?; + if let Some(md5sum) = self.md5sum.as_ref() { + let md5sum = format!("md5sum={}", md5sum); + header_data.write_u32::(md5sum.len() as u32)?; + header_data.write(md5sum.as_bytes())?; + } let msg_definition = format!("message_definition={}", self.msg_definition); header_data.write_u32::(msg_definition.len() as u32)?;