Fixed code generation to include fully expanded message definition

CHANGELOG.md

@@ -24,7 +24,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 ### Fixed
-- Bug with ros1 native publishers not parsing connection headers correctly
+- Bug with message_definitions provided by Publisher in the connection header not being the fully expanded definition.
 
 ### Changed
 

roslibrust/examples/ros1_talker.rs

@@ -1,4 +1,4 @@
-roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces/std_msgs");
+roslibrust_codegen_macro::find_and_generate_ros_messages!("assets/ros1_common_interfaces");
 
 #[cfg(feature = "ros1")]
 #[tokio::main]
@@ -14,14 +14,14 @@ async fn main() -> Result<(), anyhow::Error> {
     let nh = NodeHandle::new("http://localhost:11311", "talker_rs")
         .await
         .map_err(|err| err)?;
-    let publisher = nh.advertise::<std_msgs::String>("/chatter", 1).await?;
+    let publisher = nh
+        .advertise::<geometry_msgs::PointStamped>("/my_point", 1)
+        .await?;
 
     for count in 0..50 {
-        publisher
-            .publish(&std_msgs::String {
-                data: format!("hello world from rust {count}"),
-            })
-            .await?;
+        let mut msg = geometry_msgs::PointStamped::default();
+        msg.point.x = count as f64;
+        publisher.publish(&msg).await?;
         tokio::time::sleep(tokio::time::Duration::from_secs(1)).await;
     }
 

roslibrust/src/ros1/publisher.rs

@@ -10,7 +10,7 @@ use std::{
     sync::Arc,
 };
 use tokio::{
-    io::{AsyncReadExt, AsyncWriteExt},
+    io::AsyncWriteExt,
     sync::{mpsc, RwLock},
 };
 
@@ -82,6 +82,7 @@ impl Publication {
             tcp_nodelay: false,
             service: None,
         };
+        log::trace!("Publisher connection header: {responding_conn_header:?}");
 
         let subscriber_streams = Arc::new(RwLock::new(Vec::new()));
 

roslibrust/src/ros1/tcpros.rs

@@ -176,7 +176,7 @@ pub async fn establish_connection(
     server_uri: &str,
     conn_header: ConnectionHeader,
 ) -> Result<TcpStream, std::io::Error> {
-    use tokio::io::{AsyncReadExt, AsyncWriteExt};
+    use tokio::io::AsyncWriteExt;
 
     // Okay in Shane's version of this the server_uri is coming in as "rosrpc://localhost:41105"
     // which is causing this to break...

roslibrust_codegen/src/gen.rs

@@ -50,6 +50,12 @@ pub fn generate_service(service: ServiceFile) -> Result<TokenStream, Error> {
     })
 }
 
+/// Turns a string into a TokenStream that represents a raw string literal of the string
+pub fn generate_raw_string_literal(value: &str) -> TokenStream {
+    let wrapped = format!("r#\"{}\"#", value);
+    TokenStream::from_str(&wrapped).unwrap()
+}
+
 pub fn generate_struct(msg: MessageFile) -> Result<TokenStream, Error> {
     let ros_type_name = msg.get_full_name();
     let attrs = derive_attrs();
@@ -80,7 +86,10 @@ pub fn generate_struct(msg: MessageFile) -> Result<TokenStream, Error> {
 
     let struct_name = format_ident!("{}", msg.parsed.name);
     let md5sum = msg.md5sum;
-    let definition = msg.parsed.source.trim();
+    let definition = msg.definition;
+
+    // Raw here is only used to make the generated code look better.
+    let raw_message_definition = generate_raw_string_literal(&definition);
 
     let mut base = quote! {
         #[allow(non_snake_case)]
@@ -92,7 +101,7 @@ pub fn generate_struct(msg: MessageFile) -> Result<TokenStream, Error> {
         impl ::roslibrust_codegen::RosMessageType for #struct_name {
             const ROS_TYPE_NAME: &'static str = #ros_type_name;
             const MD5SUM: &'static str = #md5sum;
-            const DEFINITION: &'static str = #definition;
+            const DEFINITION: &'static str = #raw_message_definition;
         }
     };
 

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, VecDeque};
+use std::collections::{BTreeMap, BTreeSet, VecDeque};
 use std::fmt::{Debug, Display};
 use std::path::PathBuf;
 use utils::Package;
@@ -68,16 +68,23 @@ pub trait RosServiceType {
 pub struct MessageFile {
     pub(crate) parsed: ParsedMessageFile,
     pub(crate) md5sum: String,
+    // This is the expanded definition of the message for use in message_definition field of
+    // a connection header.
+    // See how https://wiki.ros.org/ROS/TCPROS references gendeps --cat
+    // See https://wiki.ros.org/roslib/gentools for an example of the output
+    pub(crate) definition: String,
     pub(crate) is_fixed_length: bool,
 }
 
 impl MessageFile {
     fn resolve(parsed: ParsedMessageFile, graph: &BTreeMap<String, MessageFile>) -> Option<Self> {
         let md5sum = Self::compute_md5sum(&parsed, graph)?;
+        let definition = Self::compute_full_definition(&parsed, graph)?;
         let is_fixed_length = Self::determine_if_fixed_length(&parsed, graph)?;
         Some(MessageFile {
             parsed,
             md5sum,
+            definition,
             is_fixed_length,
         })
     }
@@ -111,7 +118,7 @@ impl MessageFile {
     }
 
     pub fn get_definition(&self) -> &str {
-        &self.parsed.source
+        &self.definition
     }
 
     fn compute_md5sum(
@@ -159,6 +166,54 @@ impl MessageFile {
         Some(md5sum_content)
     }
 
+    /// Returns the set of all referenced non-intrinsic field types in this type or any of its dependencies
+    fn get_unique_field_types(
+        parsed: &ParsedMessageFile,
+        graph: &BTreeMap<String, MessageFile>,
+    ) -> Option<BTreeSet<String>> {
+        let mut unique_field_types = BTreeSet::new();
+        for field in &parsed.fields {
+            let field_type = field.field_type.field_type.as_str();
+            if is_intrinsic_type(parsed.version.unwrap_or(RosVersion::ROS1), field_type) {
+                continue;
+            }
+            let sub_message = graph.get(field.get_full_name().as_str())?;
+            // Note: need to add both the field that is referenced AND its sub-dependencies
+            unique_field_types.insert(field.get_full_name());
+            let mut sub_deps = Self::get_unique_field_types(&sub_message.parsed, graph)?;
+            unique_field_types.append(&mut sub_deps);
+        }
+        Some(unique_field_types)
+    }
+
+    /// Computes the full definition of the message, including all referenced custom types
+    /// For reference see: https://wiki.ros.org/roslib/gentools
+    /// Implementation in gentools: https://github.com/strawlab/ros/blob/c3a8785f9d9551cc05cd74000c6536e2244bb1b1/core/roslib/src/roslib/gentools.py#L245
+    fn compute_full_definition(
+        parsed: &ParsedMessageFile,
+        graph: &BTreeMap<String, MessageFile>,
+    ) -> Option<String> {
+        let mut definition_content = String::new();
+        definition_content.push_str(&format!("{}\n", parsed.source.trim()));
+        let sep: &str =
+            "================================================================================\n";
+        for field in Self::get_unique_field_types(parsed, graph)? {
+            let Some(sub_message) = graph.get(&field) else {
+                log::error!(
+                    "Unable to find message type: {field:?}, while computing full definition of {}",
+                    parsed.get_full_name()
+                );
+                return None;
+            };
+            definition_content.push_str(sep);
+            definition_content.push_str(&format!("MSG: {}\n", sub_message.get_full_name()));
+            definition_content.push_str(&format!("{}\n", sub_message.get_definition().trim()));
+        }
+        // Remove trailing \n added by concatenation logic
+        definition_content.pop();
+        Some(definition_content)
+    }
+
     fn determine_if_fixed_length(
         parsed: &ParsedMessageFile,
         graph: &BTreeMap<String, MessageFile>,
@@ -278,8 +333,11 @@ impl From<String> for RosLiteral {
 #[derive(PartialEq, Eq, Hash, Debug, Clone)]
 pub struct FieldType {
     // Present when an externally referenced package is used
-    // Note: support for messages within same package is spotty...
     pub package_name: Option<String>,
+    // Redundantly store the name of the package the field is in
+    // This is so that when an external package_name is not present
+    // we can still construct the full name of the field "package/field_type"
+    pub source_package: String,
     // Explicit text of type without array specifier
     pub field_type: String,
     // Metadata indicating whether the field is a collection.
@@ -321,7 +379,7 @@ impl FieldInfo {
             .field_type
             .package_name
             .as_ref()
-            .expect(&format!("Expected package name for field {self:#?}"));
+            .unwrap_or(&self.field_type.source_package);
         format!("{field_package}/{}", self.field_type.field_type)
     }
 }
@@ -648,7 +706,6 @@ fn parse_ros_files(
             "srv" => {
                 let srv_file = parse_ros_service_file(&contents, name, &pkg, &path)?;
                 parsed_services.push(srv_file);
-                // TODO ask shane, shouldn't we be pushing request and response to messages here?
             }
             "msg" => {
                 let msg = parse_ros_message_file(&contents, name, &pkg, &path)?;

roslibrust_codegen/src/parse/mod.rs

@@ -156,6 +156,7 @@ fn parse_field_type(type_str: &str, array_info: Option<Option<usize>>, pkg: &Pac
                     Some(pkg.name.clone())
                 }
             },
+            source_package: pkg.name.clone(),
             field_type: items[0].to_string(),
             array_info,
         }
@@ -166,12 +167,14 @@ fn parse_field_type(type_str: &str, array_info: Option<Option<usize>>, pkg: &Pac
         if items[0] == "builtin_interfaces" {
             FieldType {
                 package_name: None,
+                source_package: pkg.name.clone(),
                 field_type: type_str.to_string(),
                 array_info,
             }
         } else {
             FieldType {
                 package_name: Some(items[0].to_string()),
+                source_package: pkg.name.clone(),
                 field_type: items[1].to_string(),
                 array_info,
             }

roslibrust_genmsg/test_package/include/geometry_msgs/Polygon.h

@@ -133,7 +133,19 @@ struct Definition< ::geometry_msgs::Polygon_<ContainerAllocator>>
   {
     return "#A specification of a polygon where the first and last points are assumed to be connected"
 "Point32[] points"
-"";
+"================================================================================"
+"MSG: geometry_msgs/Point32"
+"# This contains the position of a point in free space(with 32 bits of precision)."
+"# It is recommeded to use Point wherever possible instead of Point32.  "
+"# "
+"# This recommendation is to promote interoperability.  "
+"#"
+"# This message is designed to take up less space when sending"
+"# lots of points at once, as in the case of a PointCloud.  "
+""
+"float32 x"
+"float32 y"
+"float32 z";
   }
 
   static const char* value(const ::geometry_msgs::Polygon_<ContainerAllocator>&) { return value(); }

roslibrust_genmsg/test_package/include/sensor_msgs/BatteryState.h

@@ -244,8 +244,7 @@ struct Definition< ::sensor_msgs::BatteryState_<ContainerAllocator>>
 {
   static constexpr char const* value()
   {
-    return ""
-"# Constants are chosen to match the enums in the linux kernel"
+    return "# Constants are chosen to match the enums in the linux kernel"
 "# defined in include/linux/power_supply.h as of version 3.7"
 "# The one difference is for style reasons the constants are"
 "# all uppercase not mixed case."
@@ -296,7 +295,21 @@ struct Definition< ::sensor_msgs::BatteryState_<ContainerAllocator>>
 "                            # If individual temperatures unknown but number of cells known set each to NaN"
 "string location          # The location into which the battery is inserted. (slot number or plug)"
 "string serial_number     # The best approximation of the battery serial number"
-"";
+"================================================================================"
+"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";
   }
 
   static const char* value(const ::sensor_msgs::BatteryState_<ContainerAllocator>&) { return value(); }

roslibrust_genmsg/test_package/include/sensor_msgs/CameraInfo.h

@@ -323,7 +323,42 @@ struct Definition< ::sensor_msgs::CameraInfo_<ContainerAllocator>>
 "# The default setting of roi (all values 0) is considered the same as"
 "#  full resolution (roi.width = width, roi.height = height)."
 "RegionOfInterest roi"
-"";
+"================================================================================"
+"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"
+"================================================================================"
+"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";
   }
 
   static const char* value(const ::sensor_msgs::CameraInfo_<ContainerAllocator>&) { return value(); }

roslibrust_genmsg/test_package/include/std_msgs/Header.h

@@ -154,8 +154,7 @@ struct Definition< ::std_msgs::Header_<ContainerAllocator>>
 "# time-handling sugar is provided by the client library"
 "time stamp"
 "#Frame this data is associated with"
-"string frame_id"
-"";
+"string frame_id";
   }
 
   static const char* value(const ::std_msgs::Header_<ContainerAllocator>&) { return value(); }

roslibrust_genmsg/test_package/include/std_srvs/TriggerResponse.h

@@ -137,8 +137,7 @@ struct Definition< ::std_srvs::TriggerResponse_<ContainerAllocator>>
   static constexpr char const* value()
   {
     return "bool success   # indicate successful run of triggered service"
-"string message # informational, e.g. for error messages"
-"";
+"string message # informational, e.g. for error messages";
   }
 
   static const char* value(const ::std_srvs::TriggerResponse_<ContainerAllocator>&) { return value(); }