From dbc153516bfc3798e0f41633b0794ee3c9bb0d97 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sun, 11 Aug 2024 07:52:17 -0700 Subject: [PATCH 01/11] publish a vector of bytes given a topic type and message definition --- CHANGELOG.md | 1 + roslibrust/src/ros1/node/actor.rs | 26 ++++++++++++++ roslibrust/src/ros1/node/handle.rs | 19 ++++++++-- roslibrust/src/ros1/publisher.rs | 35 ++++++++++++++++++- roslibrust/src/ros1/subscriber.rs | 1 + .../tests/ros1_native_integration_tests.rs | 32 +++++++++++++++++ 6 files changed, 111 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9215f28..fbddd6d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - 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 +- Can publish to any topic and send raw bytes instead of a deserialized message ### Fixed diff --git a/roslibrust/src/ros1/node/actor.rs b/roslibrust/src/ros1/node/actor.rs index 5d83fc9..f2d7794 100644 --- a/roslibrust/src/ros1/node/actor.rs +++ b/roslibrust/src/ros1/node/actor.rs @@ -176,6 +176,32 @@ impl NodeServerHandle { })?) } + /// Registers a publisher with the underlying node server + /// Returns a channel that the raw bytes of a publish can be shoved into to queue the publish + pub(crate) async fn register_publisher_any( + &self, + topic: &str, + topic_type: &str, + msg_definition: &str, + queue_size: usize, + latching: bool, + ) -> Result>, NodeError> { + let (sender, receiver) = oneshot::channel(); + self.node_server_sender.send(NodeMsg::RegisterPublisher { + reply: sender, + topic: topic.to_owned(), + topic_type: topic_type.to_owned(), + queue_size, + msg_definition: msg_definition.to_owned(), + md5sum: "*".to_string(), + latching, + })?; + let received = receiver.await?; + Ok(received.map_err(|_err| { + NodeError::IoError(io::Error::from(io::ErrorKind::ConnectionAborted)) + })?) + } + pub(crate) async fn unregister_publisher(&self, topic: &str) -> Result<(), NodeError> { let (sender, receiver) = oneshot::channel(); self.node_server_sender.send(NodeMsg::UnregisterPublisher { diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust/src/ros1/node/handle.rs index 3ef3e6e..32e80c4 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust/src/ros1/node/handle.rs @@ -1,8 +1,8 @@ use super::actor::{Node, NodeServerHandle}; use crate::{ ros1::{ - names::Name, publisher::Publisher, service_client::ServiceClient, subscriber::Subscriber, - subscriber::SubscriberAny, NodeError, ServiceServer, + names::Name, publisher::Publisher, publisher::PublisherAny, service_client::ServiceClient, + subscriber::Subscriber, subscriber::SubscriberAny, NodeError, ServiceServer, }, ServiceFn, }; @@ -62,6 +62,21 @@ impl NodeHandle { /// Subsequent calls will simply be given additional handles to the underlying publication. /// This behavior was chosen to mirror ROS1's API, however it is reccomended to .clone() the returend publisher /// instead of calling this function multiple times. + pub async fn advertise_any( + &self, + topic_name: &str, + topic_type: &str, + msg_definition: &str, + queue_size: usize, + latching: bool, + ) -> Result { + let sender = self + .inner + .register_publisher_any(topic_name, topic_type, msg_definition, queue_size, latching) + .await?; + Ok(PublisherAny::new(topic_name, sender)) + } + pub async fn advertise( &self, topic_name: &str, diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index aef331d..50f94f5 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -49,6 +49,38 @@ impl Publisher { } } +pub struct PublisherAny { + topic_name: String, + sender: mpsc::Sender>, + phantom: PhantomData>, +} + +impl PublisherAny { + pub(crate) fn new(topic_name: &str, sender: mpsc::Sender>) -> Self { + Self { + topic_name: topic_name.to_owned(), + sender, + phantom: PhantomData, + } + } + + /// Queues a message to be send on the related topic. + /// Returns when the data has been queued not when data is actually sent. + pub async fn publish(&self, data: &Vec) -> Result<(), PublisherError> { + // TODO this is a pretty dumb... + // because of the internal channel used for re-direction this future doesn't + // actually complete when the data is sent, but merely when it is queued to be sent + // This function could probably be non-async + // Or we should do some significant re-work to have it only yield when the data is sent. + self.sender + .send(data.to_vec()) + .await + .map_err(|_| PublisherError::StreamClosed)?; + debug!("Publishing data on topic {}", self.topic_name); + Ok(()) + } +} + pub(crate) struct Publication { topic_type: String, listener_port: u16, @@ -250,7 +282,8 @@ impl Publication { 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 { + // TODO(lucasw) is it ok to match any with "*"? + if local_md5sum != "*" && connection_md5sum != *local_md5sum { warn!( "Got subscribe request for {}, but md5sums do not match. Expected {:?}, received {:?}", topic_name, diff --git a/roslibrust/src/ros1/subscriber.rs b/roslibrust/src/ros1/subscriber.rs index 900453b..1200dd9 100644 --- a/roslibrust/src/ros1/subscriber.rs +++ b/roslibrust/src/ros1/subscriber.rs @@ -179,6 +179,7 @@ async fn establish_publisher_connection( if let Ok(responded_header) = tcpros::receive_header(&mut stream).await { if conn_header.md5sum == Some("*".to_string()) + || responded_header.md5sum == Some("*".to_string()) || conn_header.md5sum == responded_header.md5sum { log::debug!( diff --git a/roslibrust/tests/ros1_native_integration_tests.rs b/roslibrust/tests/ros1_native_integration_tests.rs index 710be2f..7ba0df2 100644 --- a/roslibrust/tests/ros1_native_integration_tests.rs +++ b/roslibrust/tests/ros1_native_integration_tests.rs @@ -12,6 +12,38 @@ mod tests { "assets/ros1_common_interfaces" ); + #[test_log::test(tokio::test)] + async fn test_publish_any() { + // publish a single message in raw bytes and test the received message is as expected + let nh = NodeHandle::new("http://localhost:11311", "test_publish_any") + .await + .unwrap(); + + let publisher = nh + .advertise_any( + "/test_publish_any", + "std_msgs/String", + "string data\n", + 1, + true, + ) + .await + .unwrap(); + + let mut subscriber = nh + .subscribe::("/test_publish_any", 1) + .await + .unwrap(); + + let msg_raw: Vec = vec![8, 0, 0, 0, 4, 0, 0, 0, 116, 101, 115, 116].to_vec(); + publisher.publish(&msg_raw).await.unwrap(); + + let res = + tokio::time::timeout(tokio::time::Duration::from_millis(250), subscriber.next()).await; + let msg = res.unwrap().unwrap().unwrap(); + assert_eq!(msg.data, "test"); + } + #[test_log::test(tokio::test)] async fn test_subscribe_any() { // get a single message in raw bytes and test the bytes are as expected From 77846da210e8ef04227b9dd149c275552b52f5db Mon Sep 17 00:00:00 2001 From: Carter Date: Thu, 22 Aug 2024 12:00:24 -0600 Subject: [PATCH 02/11] Add example, expand documentation --- roslibrust/examples/ros1_publish_any.rs | 45 +++++++++++++++++++++++++ roslibrust/src/ros1/node/handle.rs | 2 +- roslibrust/src/ros1/publisher.rs | 4 +++ 3 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 roslibrust/examples/ros1_publish_any.rs diff --git a/roslibrust/examples/ros1_publish_any.rs b/roslibrust/examples/ros1_publish_any.rs new file mode 100644 index 0000000..cc5db23 --- /dev/null +++ b/roslibrust/examples/ros1_publish_any.rs @@ -0,0 +1,45 @@ +roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces"); + +/// This example demonstrates ths usage of the .advertise_any() function +/// +/// The intent of the API is to support use cases like play back data from a bag file. +/// Most users are encourage to not use this API and instead rely on generated message types. +/// See ros1_talker.rs for a "normal" example. + +#[cfg(feature = "ros1")] +#[tokio::main] +async fn main() -> Result<(), anyhow::Error> { + // Note: this example needs a ros master running to work + let node = + roslibrust::ros1::NodeHandle::new("http://localhost:11311", "/ros1_publish_any").await?; + + // Definition from: https://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html + let msg_definition = r#"string data"#; + + let publisher = node + .advertise_any("/chatter", "std_msgs/String", &msg_definition, 100, false) + .await?; + + // Data taken from example in: + // https://wiki.ros.org/ROS/Connection%20Header + // Note: publish expects the body length field to be present, as well as length of each field + // - First four bytes are Body length = 9 bytes + // - Next four bytes are length of data field = 5 bytes + // - Lass five bytes are the data field as ascii "hello" + // Note: Byte order! + let data: Vec = vec![ + 0x09, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x68, 0x65, 0x6c, 0x6c, 0x6f, + ]; + + // This will publish "hello" in a loop at 1Hz + // `rostopic echo /chatter` will show the message being published + loop { + publisher.publish(&data).await?; + tokio::time::sleep(std::time::Duration::from_secs(1)).await; + } +} + +#[cfg(not(feature = "ros1"))] +fn main() { + eprintln!("This example does nothing without compiling with the feature 'ros1'"); +} diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust/src/ros1/node/handle.rs index 32e80c4..073baf7 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust/src/ros1/node/handle.rs @@ -60,7 +60,7 @@ impl NodeHandle { /// This function can be called multiple times to create multiple publishers for the same topic, /// however the FIRST call will establish the queue size and latching behavior for the topic. /// Subsequent calls will simply be given additional handles to the underlying publication. - /// This behavior was chosen to mirror ROS1's API, however it is reccomended to .clone() the returend publisher + /// This behavior was chosen to mirror ROS1's API, however it is recommended to .clone() the returned publisher /// instead of calling this function multiple times. pub async fn advertise_any( &self, diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index 50f94f5..7c3fcee 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -66,6 +66,10 @@ impl PublisherAny { /// Queues a message to be send on the related topic. /// Returns when the data has been queued not when data is actually sent. + /// + /// This expects the data to be the raw bytes of the message body as they would appear going over the wire. + /// See ros1_publish_any.rs example for more details. + /// Body length should be included as first four bytes. pub async fn publish(&self, data: &Vec) -> Result<(), PublisherError> { // TODO this is a pretty dumb... // because of the internal channel used for re-direction this future doesn't From 826ed466a8af48b468391fe482394ec2da4b3a0d Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Fri, 30 Aug 2024 08:16:35 -0700 Subject: [PATCH 03/11] conversion of https://github.com/lucasw/mcap_tools/blob/main/mcap_tools/scripts/msg_def_md5.py to rust (and maybe the python should live in this repo), move the tests into a separate file later --- roslibrust_codegen/src/lib.rs | 664 +++++++++++++++++++++++++++++++++- 1 file changed, 663 insertions(+), 1 deletion(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index c9b3b2e..349c9cd 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -2,7 +2,7 @@ use log::*; use proc_macro2::TokenStream; use quote::quote; use simple_error::{bail, SimpleError as Error}; -use std::collections::{BTreeMap, BTreeSet, VecDeque}; +use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, VecDeque}; use std::fmt::{Debug, Display}; use std::path::PathBuf; use utils::Package; @@ -64,6 +64,182 @@ pub trait RosServiceType { type Response: RosMessageType; } +// TODO(lucasw) this deserves a lot of str vs String cleanup +// TODO(lucasw) the msg_type isn't actually needed +pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Result { + if full_def == "".to_string() { + return Err("empty input definition".into()); + } + + // the types that don't need to be defined within the definition + let base_types: HashSet = HashSet::from_iter([ + "bool", + "byte", + "int8", + "int16", + "int32", + "int64", + "uint8", + "uint16", + "uint32", + "uint64", + "float32", + "float64", + "time", + "duration", + "string", + ].map(|name| name.to_string())); + + let mut clean_def = String::new(); + + for line_raw in full_def.lines() { + if line_raw.starts_with("#") { + continue; + } + // println!("{line_raw}"); + + // if there is a comment at the end of the line cut it off + let line = line_raw.split("#").collect::>()[0].trim(); + + // replace multiple whitespaces with one space + // https://stackoverflow.com/questions/73962868/how-to-remove-useless-space-in-rust-string-without-using-regex + let line: String = line + // trim leading and trailing space + .trim() + // split on whitespace + // including the space where the string was split + .split_inclusive(char::is_whitespace) + // filter out substrings containing only whitespace + .filter(|part| !part.trim().is_empty()) + .collect(); + + // oddly 'uint8 FOO=1' becomes 'uint8 FOO = 1', so restore it + let line = line.replace(" = ", "="); + if line == "" { + continue; + } + // println!(" -> {line}"); + clean_def += &format!("{line}\n"); // &(line.to_owned() + "\n"); + } + // strip final newline + clean_def = clean_def.trim().to_string(); + // if verbose { println!(clean_def); } + // + if clean_def == "".to_string() { + return Err("empty cleaned definition".into()); + } + + // println!("\n**** {msg_type} ****\n{}", clean_def); + + let sep: &str = + "================================================================================\n"; + let sections = clean_def.split(sep).collect::>(); + if sections.len() == 0 { + return Err("empty sections".into()); + } + + let mut sub_messages: HashMap = HashMap::new(); + sub_messages.insert(msg_type.clone(), sections[0].trim().to_string()); + // if verbose { println!("definition without comments:"); } + for section in §ions[1..] { + let lines: Vec<&str> = section.lines().collect(); + let line0 = lines[0]; + if !line0.starts_with("MSG: ") { + log::error!("{}", format!("bad section {section} -> {line0} doesn't start with 'MSG: '")); + return Err("bad section".into()); + } + // TODO(lucasw) the full text definition doesn't always have the full message types with + // the package name, + // but I think this is only when the message type is Header or the package of the message + // being define is the same as the message in the field + let section_type = line0.split(" ").collect::>()[1].to_string(); + let body = lines[1..].join("\n").trim().to_string(); + sub_messages.insert(section_type, body); + + // print(repr(section)) + // print(section) + // print("---") + } + // if verbose {print(f"\nsub messages: {sub_messages.keys()}\n"); } + + let mut hashed: HashMap = HashMap::new(); + let mut old_hashed_len = -1; + // expect to process at least one definition every pass + while hashed.len() as i64 > old_hashed_len { + old_hashed_len = hashed.len() as i64; + for (name, msg) in &sub_messages { + let pkg_name = name.split("/").collect::>()[0]; + if hashed.contains_key(name) { + continue; + } + let mut do_hash = true; + let mut field_def = "".to_string(); + for line_raw in msg.lines() { + let line_split = line_raw.trim().split_whitespace().collect::>(); + if line_split.len() < 2 { + log::error!("bad line to split '{line_raw}'"); + // TODO(lucasw) or error out + continue; + } + let (raw_field_type, _field_name) = (line_split[0], line_split[1]); + // leave array characters alone, could be [] [C] where C is a constant + let field_type = raw_field_type.split("[").collect::>()[0].to_string(); + + let full_field_type; + let line; + if base_types.contains(&field_type) { + line = line_raw.to_string(); + } else { + // TODO(lucasw) are there other special message types besides eader- or is it anything in std_msgs? + if field_type == "Header" { + full_field_type = "std_msgs/Header".to_string(); + } else if !field_type.contains("/") { + full_field_type = format!("{pkg_name}/{field_type}"); + } else { + full_field_type = field_type; + } + + match hashed.get(&full_field_type) { + Some(hash_value) => { + line = line_raw.replace(raw_field_type, hash_value).to_string(); + } + None => { + // if verbose: + // print(f"can't find {full_field_type}, loop until no progress") + do_hash = false; + break; + } + } + } + field_def += &format!("{line}\n"); + } + if !do_hash { + continue; + } + field_def = field_def.trim().to_string(); + // if verbose: + // print(f"getting hash {name}: {repr(field_def)}") + let md5sum = md5::compute(field_def.trim_end().as_bytes()); + let md5sum_text = format!("{md5sum:x}"); + hashed.insert((*name).clone(), md5sum_text); + } // go through sub-messages and hash more or them + } // keep looping until no progress or all sub-messages are hashed + /* + if verbose: + print("<<<<<<<<<<<<\n") + for k, v in hashed.items(): + print(f"{k}: {v}") + */ + + match hashed.get(&msg_type) { + Some(hash) => Ok((*hash).clone()), + None => { + log::error!("couldn't get {msg_type}"); + Err("couldn't get hash".into()) + } + } +} + #[derive(Clone, Debug)] pub struct MessageFile { pub(crate) parsed: ParsedMessageFile, @@ -792,4 +968,490 @@ mod test { assert!(!source.is_empty()); assert!(!paths.is_empty()); } + + /// Confirm md5sum from the connection header message definition matches normally + /// generated checksums + #[test_log::test] + fn msg_def_to_md5() { + + { + let def = "byte DEBUG=1\nbyte INFO=2\nbyte WARN=4\nbyte ERROR=8\nbyte FATAL=16\n\ + 2176decaecbce78abc3b96ef049fabed header\n\ + byte level\nstring name\nstring msg\nstring file\nstring function\nuint32 line\nstring[] topics"; + let expected = "acffd30cd6b6de30f120938c17c593fb"; + let md5sum = format!("{:x}", md5::compute(def.trim_end().as_bytes())); + assert_eq!(md5sum, expected, "partially checksumed rosgraph_msgs/Log"); + } + + // TODO(lucasw) it would be nice to pull these out of the real messages, but to avoid + // dependencies just storing expected definition and md5sum + // from roslib.message import get_message_class + // msg = get_message_class("std_msgs/Header") + // md5 = msg._md5sum + // def = msg._full_text + // + + { + let msg_type = "sensor_msgs/CameraInfo"; + // This definition contains double quotes, so representing it with r# and newlines which is nicer + // for limited width text editing anyhow + let def = r#" +# This message defines meta information for a camera. It should be in a +# camera namespace on topic "camera_info" and accompanied by up to five +# image topics named: +# +# image_raw - raw data from the camera driver, possibly Bayer encoded +# image - monochrome, distorted +# image_color - color, distorted +# image_rect - monochrome, rectified +# image_rect_color - color, rectified +# +# The image_pipeline contains packages (image_proc, stereo_image_proc) +# for producing the four processed image topics from image_raw and +# camera_info. The meaning of the camera parameters are described in +# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. +# +# The image_geometry package provides a user-friendly interface to +# common operations using this meta information. If you want to, e.g., +# project a 3d point into image coordinates, we strongly recommend +# using image_geometry. +# +# If the camera is uncalibrated, the matrices D, K, R, P should be left +# zeroed out. In particular, clients may assume that K[0] == 0.0 +# indicates an uncalibrated camera. + +####################################################################### +# Image acquisition info # +####################################################################### + +# Time of image acquisition, camera coordinate frame ID +Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of camera + # +x should point to the right in the image + # +y should point down in the image + # +z should point into the plane of the image + + +####################################################################### +# Calibration Parameters # +####################################################################### +# These are fixed during camera calibration. Their values will be the # +# same in all messages until the camera is recalibrated. Note that # +# self-calibrating systems may "recalibrate" frequently. # +# # +# The internal parameters can be used to warp a raw (distorted) image # +# to: # +# 1. An undistorted image (requires D and K) # +# 2. A rectified image (requires D, K, R) # +# The projection matrix P projects 3D points into the rectified image.# +####################################################################### + +# The image dimensions with which the camera was calibrated. Normally +# this will be the full camera resolution in pixels. +uint32 height +uint32 width + +# The distortion model used. Supported models are listed in +# sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a +# simple model of radial and tangential distortion - is sufficient. +string distortion_model + +# The distortion parameters, size depending on the distortion model. +# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). +float64[] D + +# Intrinsic camera matrix for the raw (distorted) images. +# [fx 0 cx] +# K = [ 0 fy cy] +# [ 0 0 1] +# Projects 3D points in the camera coordinate frame to 2D pixel +# coordinates using the focal lengths (fx, fy) and principal point +# (cx, cy). +float64[9] K # 3x3 row-major matrix + +# Rectification matrix (stereo cameras only) +# A rotation matrix aligning the camera coordinate system to the ideal +# stereo image plane so that epipolar lines in both stereo images are +# parallel. +float64[9] R # 3x3 row-major matrix + +# Projection/camera matrix +# [fx' 0 cx' Tx] +# P = [ 0 fy' cy' Ty] +# [ 0 0 1 0] +# By convention, this matrix specifies the intrinsic (camera) matrix +# of the processed (rectified) image. That is, the left 3x3 portion +# is the normal camera intrinsic matrix for the rectified image. +# It projects 3D points in the camera coordinate frame to 2D pixel +# coordinates using the focal lengths (fx', fy') and principal point +# (cx', cy') - these may differ from the values in K. +# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will +# also have R = the identity and P[1:3,1:3] = K. +# For a stereo pair, the fourth column [Tx Ty 0]' is related to the +# position of the optical center of the second camera in the first +# camera's frame. We assume Tz = 0 so both cameras are in the same +# stereo image plane. The first camera always has Tx = Ty = 0. For +# the right (second) camera of a horizontal stereo pair, Ty = 0 and +# Tx = -fx' * B, where B is the baseline between the cameras. +# Given a 3D point [X Y Z]', the projection (x, y) of the point onto +# the rectified image is given by: +# [u v w]' = P * [X Y Z 1]' +# x = u / w +# y = v / w +# This holds for both images of a stereo pair. +float64[12] P # 3x4 row-major matrix + + +####################################################################### +# Operational Parameters # +####################################################################### +# These define the image region actually captured by the camera # +# driver. Although they affect the geometry of the output image, they # +# may be changed freely without recalibrating the camera. # +####################################################################### + +# Binning refers here to any camera setting which combines rectangular +# neighborhoods of pixels into larger "super-pixels." It reduces the +# resolution of the output image to +# (width / binning_x) x (height / binning_y). +# The default values binning_x = binning_y = 0 is considered the same +# as binning_x = binning_y = 1 (no subsampling). +uint32 binning_x +uint32 binning_y + +# Region of interest (subwindow of full camera resolution), given in +# full resolution (unbinned) image coordinates. A particular ROI +# always denotes the same window of pixels on the camera sensor, +# regardless of binning settings. +# The default setting of roi (all values 0) is considered the same as +# full resolution (roi.width = width, roi.height = height). +RegionOfInterest roi + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: sensor_msgs/RegionOfInterest +# This message is used to specify a region of interest within an image. +# +# When used to specify the ROI setting of the camera when the image was +# taken, the height and width fields should either match the height and +# width fields for the associated image; or height = width = 0 +# indicates that the full resolution image was captured. + +uint32 x_offset # Leftmost pixel of the ROI + # (0 if the ROI includes the left edge of the image) +uint32 y_offset # Topmost pixel of the ROI + # (0 if the ROI includes the top edge of the image) +uint32 height # Height of ROI +uint32 width # Width of ROI + +# True if a distinct rectified ROI should be calculated from the "raw" +# ROI in this message. Typically this should be False if the full image +# is captured (ROI not used), and True if a subwindow is captured (ROI +# used). +bool do_rectify + +"#; + let expected = "c9a58c1b0b154e0e6da7578cb991d214"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "std_msgs/Header"; + let def = "# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; + let expected = "2176decaecbce78abc3b96ef049fabed"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "rosgraph_msgs/Log"; + let def = "##\n## Severity level constants\n##\nbyte DEBUG=1 #debug level\nbyte INFO=2 #general level\nbyte WARN=4 #warning level\nbyte ERROR=8 #error level\nbyte FATAL=16 #fatal/critical level\n##\n## Fields\n##\nHeader header\nbyte level\nstring name # name of the node\nstring msg # message \nstring file # file the message came from\nstring function # function the message came from\nuint32 line # line the message came from\nstring[] topics # topic names that the node publishes\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; + let expected = "acffd30cd6b6de30f120938c17c593fb"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "nav_msgs/Odometry"; + let def = "# This represents an estimate of a position and velocity in free space. \n# The pose in this message should be specified in the coordinate frame given by header.frame_id.\n# The twist in this message should be specified in the coordinate frame given by the child_frame_id\nHeader header\nstring child_frame_id\ngeometry_msgs/PoseWithCovariance pose\ngeometry_msgs/TwistWithCovariance twist\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n================================================================================\nMSG: geometry_msgs/TwistWithCovariance\n# This expresses velocity in free space with uncertainty.\n\nTwist twist\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Twist\n# This expresses velocity in free space broken into its linear and angular parts.\nVector3 linear\nVector3 angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n# It is only meant to represent a direction. Therefore, it does not\n# make sense to apply a translation to it (e.g., when applying a \n# generic rigid transformation to a Vector3, tf2 will only apply the\n# rotation). If you want your data to be translatable too, use the\n# geometry_msgs/Point message instead.\n\nfloat64 x\nfloat64 y\nfloat64 z"; + let expected = "cd5e73d190d741a2f92e81eda573aca7"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected); + } + + { + let msg_type = "tf2_msgs/TFMessage"; + let def = r#" +geometry_msgs/TransformStamped[] transforms + +================================================================================ +MSG: geometry_msgs/TransformStamped +# This expresses a transform from coordinate frame header.frame_id +# to the coordinate frame child_frame_id +# +# This message is mostly used by the +# tf package. +# See its documentation for more information. + +Header header +string child_frame_id # the frame id of the child frame +Transform transform + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Transform +# This represents the transform between two coordinate frames in free space. + +Vector3 translation +Quaternion rotation + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +"#; + let expected = "94810edda583a504dfda3829e70d7eec"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected); + } + + { + let msg_type = "vision_msgs/Detection3DArray"; + let def = r#" +# A list of 3D detections, for a multi-object 3D detector. + +Header header + +# A list of the detected proposals. A multi-proposal detector might generate +# this list with many candidate detections generated from a single input. +Detection3D[] detections + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: vision_msgs/Detection3D +# Defines a 3D detection result. +# +# This extends a basic 3D classification by including position information, +# allowing a classification result for a specific position in an image to +# to be located in the larger image. + +Header header + +# Class probabilities. Does not have to include hypotheses for all possible +# object ids, the scores for any ids not listed are assumed to be 0. +ObjectHypothesisWithPose[] results + +# 3D bounding box surrounding the object. +BoundingBox3D bbox + +# The 3D data that generated these results (i.e. region proposal cropped out of +# the image). This information is not required for all detectors, so it may +# be empty. +sensor_msgs/PointCloud2 source_cloud + +================================================================================ +MSG: vision_msgs/ObjectHypothesisWithPose +# An object hypothesis that contains position information. + +# The unique numeric ID of object detected. To get additional information about +# this ID, such as its human-readable name, listeners should perform a lookup +# in a metadata database. See vision_msgs/VisionInfo.msg for more detail. +int64 id + +# The probability or confidence value of the detected object. By convention, +# this value should lie in the range [0-1]. +float64 score + +# The 6D pose of the object hypothesis. This pose should be +# defined as the pose of some fixed reference point on the object, such a +# the geometric center of the bounding box or the center of mass of the +# object. +# Note that this pose is not stamped; frame information can be defined by +# parent messages. +# Also note that different classes predicted for the same input data may have +# different predicted 6D poses. +geometry_msgs/PoseWithCovariance pose +================================================================================ +MSG: geometry_msgs/PoseWithCovariance +# This represents a pose in free space with uncertainty. + +Pose pose + +# Row-major representation of the 6x6 covariance matrix +# The orientation parameters use a fixed-axis representation. +# In order, the parameters are: +# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) +float64[36] covariance + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: vision_msgs/BoundingBox3D +# A 3D bounding box that can be positioned and rotated about its center (6 DOF) +# Dimensions of this box are in meters, and as such, it may be migrated to +# another package, such as geometry_msgs, in the future. + +# The 3D position and orientation of the bounding box center +geometry_msgs/Pose center + +# The size of the bounding box, in meters, surrounding the object's center +# pose. +geometry_msgs/Vector3 size + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. +# It is only meant to represent a direction. Therefore, it does not +# make sense to apply a translation to it (e.g., when applying a +# generic rigid transformation to a Vector3, tf2 will only apply the +# rotation). If you want your data to be translatable too, use the +# geometry_msgs/Point message instead. + +float64 x +float64 y +float64 z +================================================================================ +MSG: sensor_msgs/PointCloud2 +# This message holds a collection of N-dimensional points, which may +# contain additional information such as normals, intensity, etc. The +# point data is stored as a binary blob, its layout described by the +# contents of the "fields" array. + +# The point cloud data may be organized 2d (image-like) or 1d +# (unordered). Point clouds organized as 2d images may be produced by +# camera depth sensors such as stereo or time-of-flight. + +# Time of sensor data acquisition, and the coordinate frame ID (for 3d +# points). +Header header + +# 2D structure of the point cloud. If the cloud is unordered, height is +# 1 and width is the length of the point cloud. +uint32 height +uint32 width + +# Describes the channels and their layout in the binary data blob. +PointField[] fields + +bool is_bigendian # Is this data bigendian? +uint32 point_step # Length of a point in bytes +uint32 row_step # Length of a row in bytes +uint8[] data # Actual point data, size is (row_step*height) + +bool is_dense # True if there are no invalid points + +================================================================================ +MSG: sensor_msgs/PointField +# This message holds the description of one point entry in the +# PointCloud2 message format. +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +string name # Name of field +uint32 offset # Offset from start of point struct +uint8 datatype # Datatype enumeration, see above +uint32 count # How many elements in the field + +"#; + let expected = "05c51d9aea1fb4cfdc8effb94f197b6f"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + } } From dcd7b788ff89f76b1bc6ca8332fe10f4363e8a37 Mon Sep 17 00:00:00 2001 From: carter Date: Wed, 4 Sep 2024 09:45:30 -0600 Subject: [PATCH 04/11] Some documentation tweaks and Cargo fmt --- roslibrust/src/ros1/node/handle.rs | 21 +++++++++----- roslibrust/src/ros1/publisher.rs | 4 +++ roslibrust_codegen/src/lib.rs | 46 ++++++++++++------------------ 3 files changed, 37 insertions(+), 34 deletions(-) diff --git a/roslibrust/src/ros1/node/handle.rs b/roslibrust/src/ros1/node/handle.rs index 073baf7..98332f2 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust/src/ros1/node/handle.rs @@ -7,7 +7,7 @@ use crate::{ ServiceFn, }; -/// Represents a handle to an underlying [Node]. NodeHandle's can be freely cloned, moved, copied, etc. +/// Represents a handle to an underlying Node. NodeHandle's can be freely cloned, moved, copied, etc. /// This class provides the user facing API for interacting with ROS. #[derive(Clone)] pub struct NodeHandle { @@ -55,13 +55,13 @@ impl NodeHandle { self.inner.get_client_uri().await } - /// Create a new publisher for the given type. + /// Create a new publisher any arbitrary message type. /// - /// This function can be called multiple times to create multiple publishers for the same topic, - /// however the FIRST call will establish the queue size and latching behavior for the topic. - /// Subsequent calls will simply be given additional handles to the underlying publication. - /// This behavior was chosen to mirror ROS1's API, however it is recommended to .clone() the returned publisher - /// instead of calling this function multiple times. + /// This function is intended to be used when a message definition was not available at compile time, + /// such as when playing back data from a bag file. + /// This function requires the text of the expanded message definition as would be produced by `gendeps --cat`. + /// See for what this should like. + /// Messages autogenerated with roslibrust_codegen will include this information in their DEFINITION constant. pub async fn advertise_any( &self, topic_name: &str, @@ -77,6 +77,13 @@ impl NodeHandle { Ok(PublisherAny::new(topic_name, sender)) } + /// Create a new publisher for the given type. + /// + /// This function can be called multiple times to create multiple publishers for the same topic, + /// however the FIRST call will establish the queue size and latching behavior for the topic. + /// Subsequent calls will simply be given additional handles to the underlying publication. + /// This behavior was chosen to mirror ROS1's API, however it is recommended to .clone() the returned publisher + /// instead of calling this function multiple times. pub async fn advertise( &self, topic_name: &str, diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index 7c3fcee..d791545 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -16,6 +16,7 @@ use tokio::{ sync::{mpsc, RwLock}, }; +/// The regular Publisher representation returned by calling advertise on a [crate::ros1::NodeHandle]. pub struct Publisher { topic_name: String, sender: mpsc::Sender>, @@ -49,6 +50,9 @@ impl Publisher { } } +/// A specialty publisher used when message type is not known at compile time. +/// +/// Relies on user to provide serialized data. Typically used with playback from bag files. pub struct PublisherAny { topic_name: String, sender: mpsc::Sender>, diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index 349c9cd..cede7a0 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -72,23 +72,13 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul } // the types that don't need to be defined within the definition - let base_types: HashSet = HashSet::from_iter([ - "bool", - "byte", - "int8", - "int16", - "int32", - "int64", - "uint8", - "uint16", - "uint32", - "uint64", - "float32", - "float64", - "time", - "duration", - "string", - ].map(|name| name.to_string())); + let base_types: HashSet = HashSet::from_iter( + [ + "bool", "byte", "int8", "int16", "int32", "int64", "uint8", "uint16", "uint32", + "uint64", "float32", "float64", "time", "duration", "string", + ] + .map(|name| name.to_string()), + ); let mut clean_def = String::new(); @@ -119,7 +109,7 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul continue; } // println!(" -> {line}"); - clean_def += &format!("{line}\n"); // &(line.to_owned() + "\n"); + clean_def += &format!("{line}\n"); // &(line.to_owned() + "\n"); } // strip final newline clean_def = clean_def.trim().to_string(); @@ -145,7 +135,10 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul let lines: Vec<&str> = section.lines().collect(); let line0 = lines[0]; if !line0.starts_with("MSG: ") { - log::error!("{}", format!("bad section {section} -> {line0} doesn't start with 'MSG: '")); + log::error!( + "{}", + format!("bad section {section} -> {line0} doesn't start with 'MSG: '") + ); return Err("bad section".into()); } // TODO(lucasw) the full text definition doesn't always have the full message types with @@ -222,14 +215,14 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul let md5sum = md5::compute(field_def.trim_end().as_bytes()); let md5sum_text = format!("{md5sum:x}"); hashed.insert((*name).clone(), md5sum_text); - } // go through sub-messages and hash more or them + } // go through sub-messages and hash more or them } // keep looping until no progress or all sub-messages are hashed - /* - if verbose: - print("<<<<<<<<<<<<\n") - for k, v in hashed.items(): - print(f"{k}: {v}") - */ + /* + if verbose: + print("<<<<<<<<<<<<\n") + for k, v in hashed.items(): + print(f"{k}: {v}") + */ match hashed.get(&msg_type) { Some(hash) => Ok((*hash).clone()), @@ -973,7 +966,6 @@ mod test { /// generated checksums #[test_log::test] fn msg_def_to_md5() { - { let def = "byte DEBUG=1\nbyte INFO=2\nbyte WARN=4\nbyte ERROR=8\nbyte FATAL=16\n\ 2176decaecbce78abc3b96ef049fabed header\n\ From 996a2053449d7696c4311be1886e46c42ad4a226 Mon Sep 17 00:00:00 2001 From: carter Date: Wed, 4 Sep 2024 11:41:21 -0600 Subject: [PATCH 05/11] First clean-up pass --- roslibrust_codegen/src/lib.rs | 198 ++++++++++++++++++++-------------- 1 file changed, 117 insertions(+), 81 deletions(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index cede7a0..2712afb 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -64,9 +64,54 @@ pub trait RosServiceType { type Response: RosMessageType; } +/// Taking in a message definition +/// reformat it according to the md5sum algorithm: +/// - Comments removed +/// - Extra whitespace removed +/// - package names of dependencies removed +/// - constants reordered to be at the front +fn clean_msg(msg: &str) -> String { + let mut result_params = vec![]; + let mut result_constants = vec![]; + for line in msg.lines() { + let line = line.trim(); + // Skip comment lines + if line.starts_with("#") { + continue; + } + // Strip comment from the end of the line (if present) + let line = line.split("#").collect::>()[0].trim(); + // Remove any extra whitespace from inside the line + let line = line.split_whitespace().collect::>().join(" "); + // Remove any whitespace on either side of the "=" for constants + let line = line.replace(" = ", "="); + // Skip any empty lines + if line == "" { + continue; + } + // Determine if constant or not + if line.contains("=") { + result_constants.push(line); + } else { + result_params.push(line); + } + } + format!( + "{}\n{}", + result_constants.join("\n"), + result_params.join("\n") + ) + .trim() + .to_string() // Last trim here is lazy, but gets job done +} + // TODO(lucasw) this deserves a lot of str vs String cleanup // TODO(lucasw) the msg_type isn't actually needed -pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Result { +/// This function will calculate the md5sum of an expanded message definition. +/// The expanded message definition is the output of `gendeps --cat` see: +/// This definition is typically sent in the connection header of a ros1 topic and is also stored in bag files. +/// This can be used to calculate the md5sum when message definitions aren't available at compile time. +pub fn message_definition_to_md5sum(msg_name: &str, full_def: String) -> Result { if full_def == "".to_string() { return Err("empty input definition".into()); } @@ -80,61 +125,27 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul .map(|name| name.to_string()), ); - let mut clean_def = String::new(); - - for line_raw in full_def.lines() { - if line_raw.starts_with("#") { - continue; - } - // println!("{line_raw}"); - - // if there is a comment at the end of the line cut it off - let line = line_raw.split("#").collect::>()[0].trim(); - - // replace multiple whitespaces with one space - // https://stackoverflow.com/questions/73962868/how-to-remove-useless-space-in-rust-string-without-using-regex - let line: String = line - // trim leading and trailing space - .trim() - // split on whitespace - // including the space where the string was split - .split_inclusive(char::is_whitespace) - // filter out substrings containing only whitespace - .filter(|part| !part.trim().is_empty()) - .collect(); - - // oddly 'uint8 FOO=1' becomes 'uint8 FOO = 1', so restore it - let line = line.replace(" = ", "="); - if line == "" { - continue; - } - // println!(" -> {line}"); - clean_def += &format!("{line}\n"); // &(line.to_owned() + "\n"); - } - // strip final newline - clean_def = clean_def.trim().to_string(); - // if verbose { println!(clean_def); } - // - if clean_def == "".to_string() { - return Err("empty cleaned definition".into()); - } - - // println!("\n**** {msg_type} ****\n{}", clean_def); - + // Split the full definition into sections per message let sep: &str = "================================================================================\n"; - let sections = clean_def.split(sep).collect::>(); + let sections = full_def.split(sep).collect::>(); if sections.len() == 0 { + // Carter: this error is impossible, split only gives empty iterator when input string is empty + // which we've already checked for above return Err("empty sections".into()); } - let mut sub_messages: HashMap = HashMap::new(); - sub_messages.insert(msg_type.clone(), sections[0].trim().to_string()); - // if verbose { println!("definition without comments:"); } + // Split the overall definition into seperate sub-messages sorted by message type (incluidng package name) + let mut sub_messages: HashMap<&str, String> = HashMap::new(); + // Note: the first section doesn't contain the "MSG: " line so we don't need to strip it here + let clean_root = clean_msg(sections[0]); + sub_messages.insert(msg_name, clean_root); + for section in §ions[1..] { - let lines: Vec<&str> = section.lines().collect(); - let line0 = lines[0]; + let line0 = section.lines().next().ok_or("empty section")?; if !line0.starts_with("MSG: ") { + // TODO(carter) this function is using a mix of logging and error returns? + // Should either always log, or never log log::error!( "{}", format!("bad section {section} -> {line0} doesn't start with 'MSG: '") @@ -145,15 +156,12 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul // the package name, // but I think this is only when the message type is Header or the package of the message // being define is the same as the message in the field - let section_type = line0.split(" ").collect::>()[1].to_string(); - let body = lines[1..].join("\n").trim().to_string(); + // Carter: I agree with this, we found the same when dealing with this previously + let section_type = line0.split(" ").collect::>()[1]; + let end_of_first_line = section.find("\n").ok_or("No body found in section")?; + let body = clean_msg(§ion[end_of_first_line + 1..]); sub_messages.insert(section_type, body); - - // print(repr(section)) - // print(section) - // print("---") } - // if verbose {print(f"\nsub messages: {sub_messages.keys()}\n"); } let mut hashed: HashMap = HashMap::new(); let mut old_hashed_len = -1; @@ -161,6 +169,7 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul while hashed.len() as i64 > old_hashed_len { old_hashed_len = hashed.len() as i64; for (name, msg) in &sub_messages { + let name = *name; let pkg_name = name.split("/").collect::>()[0]; if hashed.contains_key(name) { continue; @@ -183,7 +192,7 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul if base_types.contains(&field_type) { line = line_raw.to_string(); } else { - // TODO(lucasw) are there other special message types besides eader- or is it anything in std_msgs? + // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? if field_type == "Header" { full_field_type = "std_msgs/Header".to_string(); } else if !field_type.contains("/") { @@ -197,8 +206,7 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul line = line_raw.replace(raw_field_type, hash_value).to_string(); } None => { - // if verbose: - // print(f"can't find {full_field_type}, loop until no progress") + error!("Failed to find full_field_type: {full_field_type} with processing {name}"); do_hash = false; break; } @@ -210,27 +218,16 @@ pub fn message_definition_to_md5sum(msg_type: String, full_def: String) -> Resul continue; } field_def = field_def.trim().to_string(); - // if verbose: - // print(f"getting hash {name}: {repr(field_def)}") let md5sum = md5::compute(field_def.trim_end().as_bytes()); let md5sum_text = format!("{md5sum:x}"); - hashed.insert((*name).clone(), md5sum_text); + hashed.insert(name.to_string(), md5sum_text); } // go through sub-messages and hash more or them } // keep looping until no progress or all sub-messages are hashed - /* - if verbose: - print("<<<<<<<<<<<<\n") - for k, v in hashed.items(): - print(f"{k}: {v}") - */ - - match hashed.get(&msg_type) { - Some(hash) => Ok((*hash).clone()), - None => { - log::error!("couldn't get {msg_type}"); - Err("couldn't get hash".into()) - } - } + + Ok(hashed + .get(msg_name) + .ok_or("Couldn't get root message type")? + .to_string()) } #[derive(Clone, Debug)] @@ -1160,7 +1157,7 @@ bool do_rectify "#; let expected = "c9a58c1b0b154e0e6da7578cb991d214"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected, "{msg_type}"); } @@ -1169,7 +1166,7 @@ bool do_rectify let msg_type = "std_msgs/Header"; let def = "# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; let expected = "2176decaecbce78abc3b96ef049fabed"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected, "{msg_type}"); } @@ -1178,7 +1175,7 @@ bool do_rectify let msg_type = "rosgraph_msgs/Log"; let def = "##\n## Severity level constants\n##\nbyte DEBUG=1 #debug level\nbyte INFO=2 #general level\nbyte WARN=4 #warning level\nbyte ERROR=8 #error level\nbyte FATAL=16 #fatal/critical level\n##\n## Fields\n##\nHeader header\nbyte level\nstring name # name of the node\nstring msg # message \nstring file # file the message came from\nstring function # function the message came from\nuint32 line # line the message came from\nstring[] topics # topic names that the node publishes\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n"; let expected = "acffd30cd6b6de30f120938c17c593fb"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected, "{msg_type}"); } @@ -1187,7 +1184,7 @@ bool do_rectify let msg_type = "nav_msgs/Odometry"; let def = "# This represents an estimate of a position and velocity in free space. \n# The pose in this message should be specified in the coordinate frame given by header.frame_id.\n# The twist in this message should be specified in the coordinate frame given by the child_frame_id\nHeader header\nstring child_frame_id\ngeometry_msgs/PoseWithCovariance pose\ngeometry_msgs/TwistWithCovariance twist\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n================================================================================\nMSG: geometry_msgs/TwistWithCovariance\n# This expresses velocity in free space with uncertainty.\n\nTwist twist\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Twist\n# This expresses velocity in free space broken into its linear and angular parts.\nVector3 linear\nVector3 angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n# It is only meant to represent a direction. Therefore, it does not\n# make sense to apply a translation to it (e.g., when applying a \n# generic rigid transformation to a Vector3, tf2 will only apply the\n# rotation). If you want your data to be translatable too, use the\n# geometry_msgs/Point message instead.\n\nfloat64 x\nfloat64 y\nfloat64 z"; let expected = "cd5e73d190d741a2f92e81eda573aca7"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected); } @@ -1256,7 +1253,7 @@ float64 w "#; let expected = "94810edda583a504dfda3829e70d7eec"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected); } @@ -1441,9 +1438,48 @@ uint32 count # How many elements in the field "#; let expected = "05c51d9aea1fb4cfdc8effb94f197b6f"; - let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + let md5sum = crate::message_definition_to_md5sum(msg_type, def.into()).unwrap(); println!("{msg_type}, computed {md5sum}, expected {expected}"); assert_eq!(md5sum, expected, "{msg_type}"); } } + + // Basic test of clean_msg function + #[test] + fn clean_msg_test() { + let test_msg = r#" +# This message holds the description of one point entry in the +# PointCloud2 message format. +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 # Random Comment + +string name # Name of field +uint32 offset # Offset from start of point struct +uint8 datatype # Datatype enumeration, see above +uint32 count # How many elements in the field + + +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +"#; + let result = crate::clean_msg(test_msg); + let expected = r#"uint8 INT8=1 +uint8 UINT8=2 +uint8 INT16=3 +uint8 UINT16=4 +uint8 INT32=5 +uint8 UINT32=6 +uint8 FLOAT32=7 +uint8 FLOAT64=8 +string name +uint32 offset +uint8 datatype +uint32 count"#; + assert_eq!(result, expected); + } } From 952cc4179737314a41ef45e868cb6bb0a8219a55 Mon Sep 17 00:00:00 2001 From: carter Date: Wed, 4 Sep 2024 12:05:12 -0600 Subject: [PATCH 06/11] Cleanup round 2 --- roslibrust_codegen/src/lib.rs | 150 ++++++++++++++++++---------------- 1 file changed, 78 insertions(+), 72 deletions(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index 2712afb..6ac4e4b 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -106,25 +106,16 @@ fn clean_msg(msg: &str) -> String { } // TODO(lucasw) this deserves a lot of str vs String cleanup -// TODO(lucasw) the msg_type isn't actually needed +// TODO(lucasw) the msg_type isn't actually needed - Carter (it actually is, or we need to know the package name at least) /// This function will calculate the md5sum of an expanded message definition. /// The expanded message definition is the output of `gendeps --cat` see: /// This definition is typically sent in the connection header of a ros1 topic and is also stored in bag files. /// This can be used to calculate the md5sum when message definitions aren't available at compile time. -pub fn message_definition_to_md5sum(msg_name: &str, full_def: String) -> Result { +pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result { if full_def == "".to_string() { return Err("empty input definition".into()); } - // the types that don't need to be defined within the definition - let base_types: HashSet = HashSet::from_iter( - [ - "bool", "byte", "int8", "int16", "int32", "int64", "uint8", "uint16", "uint32", - "uint64", "float32", "float64", "time", "duration", "string", - ] - .map(|name| name.to_string()), - ); - // Split the full definition into sections per message let sep: &str = "================================================================================\n"; @@ -163,71 +154,86 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: String) -> Result< sub_messages.insert(section_type, body); } - let mut hashed: HashMap = HashMap::new(); - let mut old_hashed_len = -1; - // expect to process at least one definition every pass - while hashed.len() as i64 > old_hashed_len { - old_hashed_len = hashed.len() as i64; - for (name, msg) in &sub_messages { - let name = *name; - let pkg_name = name.split("/").collect::>()[0]; - if hashed.contains_key(name) { - continue; + // TODO MAJOR(carter): I'd like to convert this loop to a recursive function where we pass in the map of hashes + // and update them as we go, this tripple loop is stinky to my eye. + // TODO(carter) we should be able to do this in close to one pass if we iterate the full_def backwards + let mut hashed = HashMap::new(); + let hash = message_definition_to_md5sum_recursive(msg_name, &sub_messages, &mut hashed)?; + + Ok(hash) +} + +/// Calculates the hash of the specified message type by recursively calling itself on all dependencies +/// Uses defs as the list of message definitions available for it (expects them to already be cleaned) +/// Uses hashes as the cache of already calculated hashes so we don't redo work +fn message_definition_to_md5sum_recursive( + msg_type: &str, + defs: &HashMap<&str, String>, + mut hashes: &mut HashMap, +) -> Result { + let base_types: HashSet = HashSet::from_iter( + [ + "bool", "byte", "int8", "int16", "int32", "int64", "uint8", "uint16", "uint32", + "uint64", "float32", "float64", "time", "duration", "string", + ] + .map(|name| name.to_string()), + ); + let def = defs.get(msg_type).ok_or(simple_error::simple_error!( + "Couldn't find message type: {msg_type}" + ))?; + let pkg_name = msg_type.split("/").collect::>()[0]; + // We'll store the expanded hash definition in this string as we go + let mut field_def = "".to_string(); + for line_raw in def.lines() { + let line_split = line_raw.trim().split_whitespace().collect::>(); + if line_split.len() < 2 { + log::error!("bad line to split '{line_raw}'"); + // TODO(lucasw) or error out + continue; + } + let (raw_field_type, _field_name) = (line_split[0], line_split[1]); + // leave array characters alone, could be [] [C] where C is a constant + let field_type = raw_field_type.split("[").collect::>()[0].to_string(); + + let full_field_type; + let line; + if base_types.contains(&field_type) { + line = line_raw.to_string(); + } else { + // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? + if field_type == "Header" { + full_field_type = "std_msgs/Header".to_string(); + } else if !field_type.contains("/") { + full_field_type = format!("{pkg_name}/{field_type}"); + } else { + full_field_type = field_type; } - let mut do_hash = true; - let mut field_def = "".to_string(); - for line_raw in msg.lines() { - let line_split = line_raw.trim().split_whitespace().collect::>(); - if line_split.len() < 2 { - log::error!("bad line to split '{line_raw}'"); - // TODO(lucasw) or error out - continue; + + match hashes.get(&full_field_type) { + Some(hash_value) => { + // Hash already exists in cache so we can use it + line = line_raw.replace(raw_field_type, hash_value).to_string(); } - let (raw_field_type, _field_name) = (line_split[0], line_split[1]); - // leave array characters alone, could be [] [C] where C is a constant - let field_type = raw_field_type.split("[").collect::>()[0].to_string(); - - let full_field_type; - let line; - if base_types.contains(&field_type) { - line = line_raw.to_string(); - } else { - // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? - if field_type == "Header" { - full_field_type = "std_msgs/Header".to_string(); - } else if !field_type.contains("/") { - full_field_type = format!("{pkg_name}/{field_type}"); - } else { - full_field_type = field_type; - } - - match hashed.get(&full_field_type) { - Some(hash_value) => { - line = line_raw.replace(raw_field_type, hash_value).to_string(); - } - None => { - error!("Failed to find full_field_type: {full_field_type} with processing {name}"); - do_hash = false; - break; - } - } + None => { + // Recurse! To calculate hash of this field type + let hash = message_definition_to_md5sum_recursive( + &full_field_type, + defs, + &mut hashes, + )?; + line = line_raw.replace(raw_field_type, &hash).to_string(); } - field_def += &format!("{line}\n"); - } - if !do_hash { - continue; } - field_def = field_def.trim().to_string(); - let md5sum = md5::compute(field_def.trim_end().as_bytes()); - let md5sum_text = format!("{md5sum:x}"); - hashed.insert(name.to_string(), md5sum_text); - } // go through sub-messages and hash more or them - } // keep looping until no progress or all sub-messages are hashed - - Ok(hashed - .get(msg_name) - .ok_or("Couldn't get root message type")? - .to_string()) + } + field_def += &format!("{line}\n"); + } + field_def = field_def.trim().to_string(); + let md5sum = md5::compute(field_def.trim_end().as_bytes()); + let md5sum_text = format!("{md5sum:x}"); + // Insert our hash into the cache before we return + hashes.insert(msg_type.to_string(), md5sum_text.clone()); + + Ok(md5sum_text) } #[derive(Clone, Debug)] From 8402625b411de7213bbdd964dfdac8ae48cc1fc1 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 31 Aug 2024 07:21:00 -0700 Subject: [PATCH 07/11] Several new tests to cover blank messages, messages that are blank after cleaning, or missing 'MSG:' in sub-message sections --- roslibrust_codegen/src/lib.rs | 89 ++++++++++++++++++++++++++++++++--- 1 file changed, 82 insertions(+), 7 deletions(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index 6ac4e4b..09b1ba2 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -130,18 +130,15 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result = HashMap::new(); // Note: the first section doesn't contain the "MSG: " line so we don't need to strip it here let clean_root = clean_msg(sections[0]); + if clean_root == "".to_string() { + return Err("empty cleaned root definition".into()); + } sub_messages.insert(msg_name, clean_root); for section in §ions[1..] { let line0 = section.lines().next().ok_or("empty section")?; if !line0.starts_with("MSG: ") { - // TODO(carter) this function is using a mix of logging and error returns? - // Should either always log, or never log - log::error!( - "{}", - format!("bad section {section} -> {line0} doesn't start with 'MSG: '") - ); - return Err("bad section".into()); + return Err("bad section {section} -> {line0} doesn't start with 'MSG: '".into()); } // TODO(lucasw) the full text definition doesn't always have the full message types with // the package name, @@ -978,6 +975,84 @@ mod test { assert_eq!(md5sum, expected, "partially checksumed rosgraph_msgs/Log"); } + { + let msg_type = "bad_msgs/Empty"; + let def = ""; + let _md5sum = + crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "bad_msgs/CommentSpacesOnly"; + let def = + "# message with only comments and whitespace\n# another line comment\n\n \n"; + let _md5sum = + crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "fake_msgs/MissingSectionMsg"; + let def = "string name\nstring msg\n================================================================================\n# message with only comments and whitespace\n# another line comment\n\n \n"; + let _md5sum = + crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + let msg_type = "bad_msgs/BadLog"; + let def = "## +## Severity level constants +byte DEUG=1 #debug level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +uint32 line # line the message came from +string[] topics # topic names that the node publishes + +================================================================================ +MSG: std_msgs/badHeader +# Standard metadata for higher-level stamped data types. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +time stamp +"; + let _md5sum = + crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + + { + // TODO(lucasw) not sure if this is an ok message, currently it passes + let expected = "96c44a027b586ee888fe95ac325151ae"; + let msg_type = "fake_msgs/CommentSpacesOnlySection"; + let def = "string name\nstring msg\n================================================================================\nMSG: foo/bar\n# message with only comments and whitespace\n# another line comment\n\n \n"; + let md5sum = crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap(); + println!("{msg_type}, computed {md5sum}, expected {expected}"); + assert_eq!(md5sum, expected, "{msg_type}"); + } + + { + let msg_type = "fake_msgs/Garbage"; + let def = r#" +fsdajklf + +== #fdjkl + +MSG: jklfd +# +================================================================================ +f + +vjk +"#; + let _md5sum = + crate::message_definition_to_md5sum(msg_type.into(), def.into()).unwrap_err(); + } + // TODO(lucasw) it would be nice to pull these out of the real messages, but to avoid // dependencies just storing expected definition and md5sum // from roslib.message import get_message_class From 88b6ebe57b50be4feba626f5d8ec324977a3087b Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sat, 31 Aug 2024 12:19:10 -0700 Subject: [PATCH 08/11] generate the md5sum from the message definition for publisher_any, and because of that restore rejecting mismatching md5sums --- roslibrust/src/ros1/node/actor.rs | 19 ++++++++++++++++++- roslibrust/src/ros1/publisher.rs | 3 ++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/roslibrust/src/ros1/node/actor.rs b/roslibrust/src/ros1/node/actor.rs index f2d7794..9c066b7 100644 --- a/roslibrust/src/ros1/node/actor.rs +++ b/roslibrust/src/ros1/node/actor.rs @@ -187,13 +187,30 @@ impl NodeServerHandle { latching: bool, ) -> Result>, NodeError> { let (sender, receiver) = oneshot::channel(); + + let md5sum; + let md5sum_res = + roslibrust_codegen::message_definition_to_md5sum(topic_type, msg_definition); + match md5sum_res { + // TODO(lucasw) make a new error type for this? + Err(err) => { + log::error!("{:?}", err); + return Err(NodeError::IoError(io::Error::from( + io::ErrorKind::ConnectionAborted, + ))); + } + Ok(md5sum_rv) => { + md5sum = md5sum_rv; + } + } + self.node_server_sender.send(NodeMsg::RegisterPublisher { reply: sender, topic: topic.to_owned(), topic_type: topic_type.to_owned(), queue_size, msg_definition: msg_definition.to_owned(), - md5sum: "*".to_string(), + md5sum, latching, })?; let received = receiver.await?; diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index d791545..39b6a1a 100644 --- a/roslibrust/src/ros1/publisher.rs +++ b/roslibrust/src/ros1/publisher.rs @@ -291,7 +291,8 @@ impl Publication { if connection_md5sum != "*" { if let Some(local_md5sum) = &responding_conn_header.md5sum { // TODO(lucasw) is it ok to match any with "*"? - if local_md5sum != "*" && connection_md5sum != *local_md5sum { + // if local_md5sum != "*" && connection_md5sum != *local_md5sum { + if connection_md5sum != *local_md5sum { warn!( "Got subscribe request for {}, but md5sums do not match. Expected {:?}, received {:?}", topic_name, From 90af33a9c5d08836b793c1b9467e732a83208ff6 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Mon, 2 Sep 2024 08:10:12 -0700 Subject: [PATCH 09/11] make PublisherAny public --- roslibrust/src/ros1/mod.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/roslibrust/src/ros1/mod.rs b/roslibrust/src/ros1/mod.rs index e65b60d..e82b4e1 100644 --- a/roslibrust/src/ros1/mod.rs +++ b/roslibrust/src/ros1/mod.rs @@ -12,6 +12,7 @@ pub use node::*; mod publisher; pub use publisher::Publisher; +pub use publisher::PublisherAny; mod service_client; pub use service_client::ServiceClient; mod subscriber; From 0c5b034eb6b2fe579fcff3f964bcda73209deea0 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 5 Sep 2024 07:40:44 -0700 Subject: [PATCH 10/11] cargo clippy suggestions --- roslibrust_codegen/src/lib.rs | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index 09b1ba2..331fb58 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -76,21 +76,21 @@ fn clean_msg(msg: &str) -> String { for line in msg.lines() { let line = line.trim(); // Skip comment lines - if line.starts_with("#") { + if line.starts_with('#') { continue; } // Strip comment from the end of the line (if present) - let line = line.split("#").collect::>()[0].trim(); + let line = line.split('#').collect::>()[0].trim(); // Remove any extra whitespace from inside the line let line = line.split_whitespace().collect::>().join(" "); // Remove any whitespace on either side of the "=" for constants let line = line.replace(" = ", "="); // Skip any empty lines - if line == "" { + if line.is_empty() { continue; } // Determine if constant or not - if line.contains("=") { + if line.contains('=') { result_constants.push(line); } else { result_params.push(line); @@ -112,7 +112,7 @@ fn clean_msg(msg: &str) -> String { /// This definition is typically sent in the connection header of a ros1 topic and is also stored in bag files. /// This can be used to calculate the md5sum when message definitions aren't available at compile time. pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result { - if full_def == "".to_string() { + if full_def.is_empty() { return Err("empty input definition".into()); } @@ -120,7 +120,7 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result>(); - if sections.len() == 0 { + if sections.is_empty() { // Carter: this error is impossible, split only gives empty iterator when input string is empty // which we've already checked for above return Err("empty sections".into()); @@ -130,7 +130,7 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result = HashMap::new(); // Note: the first section doesn't contain the "MSG: " line so we don't need to strip it here let clean_root = clean_msg(sections[0]); - if clean_root == "".to_string() { + if clean_root.is_empty() { return Err("empty cleaned root definition".into()); } sub_messages.insert(msg_name, clean_root); @@ -145,8 +145,8 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result>()[1]; - let end_of_first_line = section.find("\n").ok_or("No body found in section")?; + let section_type = line0.split_whitespace().collect::>()[1]; + let end_of_first_line = section.find('\n').ok_or("No body found in section")?; let body = clean_msg(§ion[end_of_first_line + 1..]); sub_messages.insert(section_type, body); } @@ -166,7 +166,7 @@ pub fn message_definition_to_md5sum(msg_name: &str, full_def: &str) -> Result, - mut hashes: &mut HashMap, + hashes: &mut HashMap, ) -> Result { let base_types: HashSet = HashSet::from_iter( [ @@ -178,11 +178,11 @@ fn message_definition_to_md5sum_recursive( let def = defs.get(msg_type).ok_or(simple_error::simple_error!( "Couldn't find message type: {msg_type}" ))?; - let pkg_name = msg_type.split("/").collect::>()[0]; + let pkg_name = msg_type.split('/').collect::>()[0]; // We'll store the expanded hash definition in this string as we go let mut field_def = "".to_string(); for line_raw in def.lines() { - let line_split = line_raw.trim().split_whitespace().collect::>(); + let line_split = line_raw.split_whitespace().collect::>(); if line_split.len() < 2 { log::error!("bad line to split '{line_raw}'"); // TODO(lucasw) or error out @@ -190,7 +190,7 @@ fn message_definition_to_md5sum_recursive( } let (raw_field_type, _field_name) = (line_split[0], line_split[1]); // leave array characters alone, could be [] [C] where C is a constant - let field_type = raw_field_type.split("[").collect::>()[0].to_string(); + let field_type = raw_field_type.split('[').collect::>()[0].to_string(); let full_field_type; let line; @@ -200,7 +200,7 @@ fn message_definition_to_md5sum_recursive( // TODO(lucasw) are there other special message types besides header- or is it anything in std_msgs? if field_type == "Header" { full_field_type = "std_msgs/Header".to_string(); - } else if !field_type.contains("/") { + } else if !field_type.contains('/') { full_field_type = format!("{pkg_name}/{field_type}"); } else { full_field_type = field_type; @@ -216,7 +216,7 @@ fn message_definition_to_md5sum_recursive( let hash = message_definition_to_md5sum_recursive( &full_field_type, defs, - &mut hashes, + hashes, )?; line = line_raw.replace(raw_field_type, &hash).to_string(); } From bfe8962a7c516cd335b6b490036008cb9a94eeeb Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Thu, 5 Sep 2024 07:50:36 -0700 Subject: [PATCH 11/11] cargo fmt to join multiple lines back into one --- roslibrust_codegen/src/lib.rs | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index 331fb58..54cfca0 100644 --- a/roslibrust_codegen/src/lib.rs +++ b/roslibrust_codegen/src/lib.rs @@ -213,11 +213,8 @@ fn message_definition_to_md5sum_recursive( } None => { // Recurse! To calculate hash of this field type - let hash = message_definition_to_md5sum_recursive( - &full_field_type, - defs, - hashes, - )?; + let hash = + message_definition_to_md5sum_recursive(&full_field_type, defs, hashes)?; line = line_raw.replace(raw_field_type, &hash).to_string(); } }