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/src/ros1/publisher.rs

@@ -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_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())?;
+            unique_field_types.append(&mut Self::get_unique_field_types(
+                &sub_message.parsed,
+                graph,
+            )?);
+        }
+        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>,
@@ -648,7 +703,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_genmsg/test_package/include/geometry_msgs/Polygon.h

@@ -132,8 +132,7 @@ struct Definition< ::geometry_msgs::Polygon_<ContainerAllocator>>
   static constexpr char const* value()
   {
     return "#A specification of a polygon where the first and last points are assumed to be connected"
-"Point32[] points"
-"";
+"Point32[] points";
   }
 
   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."
@@ -295,8 +294,7 @@ struct Definition< ::sensor_msgs::BatteryState_<ContainerAllocator>>
 "float32[] cell_temperature  # An array of individual cell temperatures for each cell in the pack"
 "                            # 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"
-"";
+"string serial_number     # The best approximation of the battery serial number";
   }
 
   static const char* value(const ::sensor_msgs::BatteryState_<ContainerAllocator>&) { return value(); }

roslibrust_genmsg/test_package/include/sensor_msgs/CameraInfo.h

@@ -322,8 +322,7 @@ struct Definition< ::sensor_msgs::CameraInfo_<ContainerAllocator>>
 "#  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"
-"";
+"RegionOfInterest roi";
   }
 
   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(); }