added utility function for converting only selected members of a message

Signed-off-by: Dominik Authaler <[email protected]>

Author: authaldo

dynmsg/include/dynmsg/message_reading.hpp

@@ -52,6 +52,19 @@ namespace cpp
  */
 YAML::Node message_to_yaml(const RosMessage_Cpp & message);
 
+/// Variant of dynmsg::cpp::message_to_yaml() for converting only selected members of a message into a YAML format.
+/**
+ * \details In order to decrease the overhead of converting full messages into their corresponding YAML format this
+ *          function uses the type introspection information to convert solely the specified member. This member
+ *          can either be a raw value or a message itself.
+ *
+ * \param message           ROS message to convert.
+ * \param member_identifier Identifier of the member, multiple levels within the message are separated by '/'.
+ * \return YAML representation of the selected member, will be null if the specified member is not part of the
+ *         provided message.
+ */
+YAML::Node selected_member_to_yaml(const RosMessage_Cpp & message, const std::string &member_identifier);
+
 }  // namespace cpp
 
 }  // namespace dynmsg

dynmsg/src/message_reading_cpp.cpp

@@ -637,5 +637,59 @@ message_to_yaml(const RosMessage_Cpp & message)
   return yaml_msg;
 }
 
+YAML::Node
+selected_member_to_yaml(const RosMessage_Cpp & message, const std::string &member_identifier)
+{
+  // in case no member identifier is provided the function defaults to converting the full message
+  if (member_identifier.empty()) {
+      return message_to_yaml(message);
+  }
+
+  RosMessage_Cpp tmp_msg = message;
+  std::size_t start = 0;
+  std::size_t end = 0;
+
+  const MemberInfo_Cpp *member_info = nullptr;
+  uint8_t * member_data = nullptr;
+
+  bool more_sublevels_available = true;
+
+  while (more_sublevels_available) {
+    std::string field_name;
+    if ((end = member_identifier.find('/', start)) != std::string::npos) {
+        field_name = member_identifier.substr(start, end - start);
+        start = end + 1;
+    } else {
+        field_name = member_identifier.substr(start);
+        more_sublevels_available = false;
+    }
+
+    // iterate over message and check whether the field is available
+    bool found = false;
+
+    for (uint32_t idx = 0; idx < tmp_msg.type_info->member_count_; idx++) {
+      member_info = &tmp_msg.type_info->members_[idx];
+      member_data = &tmp_msg.data[member_info->offset_];
+
+      if (std::string(member_info->name_) == field_name) {
+        if (member_info->type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) {
+          // update temporary message used for processing to the nested member
+          tmp_msg.type_info = reinterpret_cast<const TypeInfo_Cpp *>(member_info->members_->data);
+          tmp_msg.data = const_cast<uint8_t *>(member_data);
+        }
+
+        found = true;
+        break;
+      }
+    }
+
+    if (!found) {
+      return YAML::Node();
+    }
+  }
+
+  return impl::member_to_yaml(*member_info, member_data);
+}
+
 }  // namespace cpp
 }  // namespace dynmsg