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/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/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; diff --git a/roslibrust/src/ros1/node/actor.rs b/roslibrust/src/ros1/node/actor.rs index 5d83fc9..9c066b7 100644 --- a/roslibrust/src/ros1/node/actor.rs +++ b/roslibrust/src/ros1/node/actor.rs @@ -176,6 +176,49 @@ 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(); + + 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, + 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..98332f2 100644 --- a/roslibrust/src/ros1/node/handle.rs +++ b/roslibrust/src/ros1/node/handle.rs @@ -1,13 +1,13 @@ 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, }; -/// 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,12 +55,34 @@ impl NodeHandle { self.inner.get_client_uri().await } + /// Create a new publisher any arbitrary message type. + /// + /// 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, + 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)) + } + /// 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 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( &self, diff --git a/roslibrust/src/ros1/publisher.rs b/roslibrust/src/ros1/publisher.rs index aef331d..39b6a1a 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,45 @@ 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>, + 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. + /// + /// 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 + // 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,6 +290,8 @@ impl Publication { if let Some(connection_md5sum) = connection_header.md5sum { 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 connection_md5sum != *local_md5sum { warn!( "Got subscribe request for {}, but md5sums do not match. Expected {:?}, received {:?}", 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 diff --git a/roslibrust_codegen/src/lib.rs b/roslibrust_codegen/src/lib.rs index c9b3b2e..54cfca0 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,172 @@ 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.is_empty() { + 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 - 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: &str) -> Result { + if full_def.is_empty() { + return Err("empty input definition".into()); + } + + // Split the full definition into sections per message + let sep: &str = + "================================================================================\n"; + let sections = full_def.split(sep).collect::>(); + 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()); + } + + // 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]); + if clean_root.is_empty() { + 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: ") { + 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, + // 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 + // Carter: I agree with this, we found the same when dealing with this previously + 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); + } + + // 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>, + 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.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; + } + + 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(); + } + None => { + // Recurse! To calculate hash of this field type + let hash = + message_definition_to_md5sum_recursive(&full_field_type, defs, hashes)?; + line = line_raw.replace(raw_field_type, &hash).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)] pub struct MessageFile { pub(crate) parsed: ParsedMessageFile, @@ -792,4 +958,606 @@ 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"); + } + + { + 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 + // 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, 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, 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, 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, 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, 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, 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); + } }