Converting only select message members

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

@@ -630,12 +630,66 @@ message_to_yaml(const RosMessage_Cpp & message)
     DYNMSG_DEBUG(std::cout << "DEBUG: member_info name: " << member_info.name_ << std::endl);
     // Get a pointer to the member's data in the binary buffer
     uint8_t * member_data = &message.data[member_info.offset_];
-    // Recursively (because some members may be non-primitive types themeslves) convert the member
+    // Recursively (because some members may be non-primitive types themselves) convert the member
     // to YAML
     yaml_msg[member_info.name_] = impl::member_to_yaml(member_info, member_data);
   }
   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

test_dynmsg/examples/conversion_cpp.cpp

@@ -62,5 +62,20 @@ int main()
   printf("%s\n", msg_from_yaml.frame_id.c_str());
   printf("%d s, %d ns\n", msg_from_yaml.stamp.sec, msg_from_yaml.stamp.nanosec);
 
+  printf("\n");
+
+  // Note: In case only a portion of the message is of interest, it is also possible to limit the conversion
+  //       to this part of the message. This can be beneficial if only a few members of large messages are
+  //       further processed (e.g. width and height fields of a Pointcloud2 message without further need for obtaining
+  //       detailed information about the individual points).
+  YAML::Node partial_yaml_msg = dynmsg::cpp::selected_member_to_yaml(ros_msg, "stamp");
+  const std::string partial_yaml_string = dynmsg::yaml_to_string(partial_yaml_msg);
+
+  // Prints solely the 'stamp' sub-message (but also works for extracting values of individual message fields
+  // which are not a message on their own)
+  // sec: 4
+  // nanosec: 20
+  printf("%s\n", partial_yaml_string.c_str());
+
   return 0;
 }

test_dynmsg/test/test_conversion.cpp

@@ -932,3 +932,62 @@ TEST(TestConversion, rcl_interfaces__ParameterEvent_cpp)
   EXPECT_EQ(msg_from_yaml.new_parameters[0].value.type, 1u);
   EXPECT_EQ(msg_from_yaml.new_parameters[0].value.bool_value, true);
 }
+
+TEST(TestConversion, PartialConversion_std_msgs__Header_cpp)
+{
+  // Start with a ROS message, like a std_msgs/Header
+  std_msgs::msg::Header msg;
+  msg.frame_id = "my_frame";
+  builtin_interfaces::msg::Time stamp;
+  stamp.sec = 4;
+  stamp.nanosec = 20u;
+  msg.stamp = stamp;
+
+  // Convert solely the 'sec' member of the timestamp to a YAML representation
+  InterfaceTypeName interface{"std_msgs", "Header"};
+  RosMessage_Cpp ros_msg;
+  ros_msg.type_info = dynmsg::cpp::get_type_info(interface);
+  ros_msg.data = reinterpret_cast<uint8_t *>(&msg);
+  YAML::Node yaml_msg = dynmsg::cpp::selected_member_to_yaml(ros_msg, "stamp/sec");
+  std::cout << "'sec' as YAML:" << std::endl;
+  std::cout << yaml_msg << std::endl << std::endl;
+
+  EXPECT_EQ(yaml_msg.as<int>(), 4);
+}
+
+TEST(TestConversion, PartialConversion_rcl_interfaces__ParameterEvent_cpp)
+{
+  // Start with a ROS message, like a rcl_interfaces/ParameterEvent
+  rcl_interfaces::msg::ParameterEvent msg;
+
+  builtin_interfaces::msg::Time stamp;
+  stamp.sec = 4;
+  stamp.nanosec = 20u;
+
+  rcl_interfaces::msg::ParameterValue param_value;
+  param_value.type = 1u;
+  param_value.bool_value = true;
+
+  rcl_interfaces::msg::Parameter param;
+  param.name = "the_param_name";
+  param.value = param_value;
+
+  msg.stamp = stamp;
+  msg.node = "/my/node";
+  msg.new_parameters.push_back(param);
+
+  // Convert solely the parameter array to a YAML representation
+  InterfaceTypeName interface{"rcl_interfaces", "ParameterEvent"};
+  RosMessage_Cpp ros_msg;
+  ros_msg.type_info = dynmsg::cpp::get_type_info(interface);
+  ros_msg.data = reinterpret_cast<uint8_t *>(&msg);
+  YAML::Node yaml_msg = dynmsg::cpp::selected_member_to_yaml(ros_msg, "new_parameters");
+  std::cout << "partial message to YAML:" << std::endl;
+  std::cout << yaml_msg << std::endl << std::endl;
+
+  EXPECT_TRUE(yaml_msg.IsSequence());
+  EXPECT_EQ(yaml_msg.size(), 1);
+  EXPECT_STREQ(yaml_msg[0]["name"].as<std::string>().c_str(), "the_param_name");
+  EXPECT_EQ(yaml_msg[0]["value"]["type"].as<int>(), 1u);
+  EXPECT_EQ(yaml_msg[0]["value"]["bool_value"].as<bool>(), true);
+}