diff --git a/dynmsg/include/dynmsg/message_reading.hpp b/dynmsg/include/dynmsg/message_reading.hpp index 20ea7bb..40fbd1a 100644 --- a/dynmsg/include/dynmsg/message_reading.hpp +++ b/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 diff --git a/dynmsg/src/message_reading_cpp.cpp b/dynmsg/src/message_reading_cpp.cpp index 072819c..fcaeb0f 100644 --- a/dynmsg/src/message_reading_cpp.cpp +++ b/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(member_info->members_->data); + tmp_msg.data = const_cast(member_data); + } + + found = true; + break; + } + } + + if (!found) { + return YAML::Node(); + } + } + + return impl::member_to_yaml(*member_info, member_data); +} + } // namespace cpp } // namespace dynmsg diff --git a/test_dynmsg/examples/conversion_cpp.cpp b/test_dynmsg/examples/conversion_cpp.cpp index 0389fcc..f26ab80 100644 --- a/test_dynmsg/examples/conversion_cpp.cpp +++ b/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; } diff --git a/test_dynmsg/test/test_conversion.cpp b/test_dynmsg/test/test_conversion.cpp index ea42866..ff9206f 100644 --- a/test_dynmsg/test/test_conversion.cpp +++ b/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(&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(), 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(&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().c_str(), "the_param_name"); + EXPECT_EQ(yaml_msg[0]["value"]["type"].as(), 1u); + EXPECT_EQ(yaml_msg[0]["value"]["bool_value"].as(), true); +}