Deserialize message with topic name (ROS2)
I am trying to create a template node that takes as an input a string being the message type, then subscribe to it. To give some context, the reason is I have a 3rd party library (in C) that is not in ROS and only accepts JSON. This work would allow to wrap my node and forward it and I'd create as many nodes as I need
Currently, I have a minimal code example that works but I can't make a generic serializer. :
#include "rclcpp/rclcpp.hpp"
#include <iostream>
#include "rclcpp/node.hpp"
#include "rclcpp/serialization.hpp"
#include "std_msgs/msg/string.hpp"
#include "dynmsg_demo/typesupport_utils.hpp"
#include "dynmsg/message_reading.hpp"
#include <memory.h>
int main(int argc, char* argv[])
{
auto topic_name = "/dc/group/system";
auto topic_type = "std_msgs/String";
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("topic_echo_cpp");
auto callback = [&topic_type](std::shared_ptr<rclcpp::SerializedMessage> msg) {
// Init Serializer. those 2 work, but require the type
auto deserializer = rclcpp::Serialization<std_msgs::msg::String>();
std_msgs::msg::String des_ros_msg;
deserializer.deserialize_message(msg.get(), &des_ros_msg);
// Create message to parse
RosMessage_Cpp ros_msg;
ros_msg.type_info = dynmsg::cpp::get_type_info(InterfaceTypeName{ "std_msgs", "String" });
ros_msg.data = reinterpret_cast<uint8_t*>(&des_ros_msg);
// OK Transform into YAML
auto yaml_msg = dynmsg::cpp::message_to_yaml(ros_msg);
YAML::Emitter emitter;
emitter << yaml_msg;
std::cout << emitter.c_str() << std::endl;
};
auto sub = node->create_generic_subscription(topic_name, topic_type, 10, callback);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
From this conversation: https://answers.ros.org/question/389683/deserialise-a-serializedmessage-into-something-other-than-a-ros2-message-eg-json/, I tried to include a way to create a general subscriber
#include "dynmsg/message_reading.hpp"
#include "dynmsg/msg_parser.hpp"
InterfaceTypeName topic_type_name = get_topic_type_from_string_type("std_msgs/String");
const rosidl_message_type_support_t* ts = get_type_support(topic_type_name);
auto deserializer = rclcpp::SerializationBase(ts);
But without success, I can't manage to "grab" the type and pass it directly. I get a realloc(): invalid pointer
Maybe something else that can help, this compiles but the std::cout is empty.
InterfaceTypeName topic_type_name = get_topic_type_from_string_type(topic_type);
const rosidl_message_type_support_t* ts = get_type_support(topic_type_name);
auto deserializer = rclcpp::SerializationBase(ts);
auto result = RosMessage();
deserializer.deserialize_message(msg.get(), &result);
auto yaml_msg = dynmsg::c::message_to_yaml(result);
YAML::Emitter emitter;
emitter << yaml_msg;
std::cout << emitter.c_str() << std::endl;
Any idea on how to proceed? I am using Galactic
PS: An alternative for me would be to only transfer std_mgs/String, make validation without ros messages and transform it with nlohman, but sounds very hacky. PS2: I am aware I can achieve my goal easily in python but the 3rd party library is huge and in c, doing bindings would take me ages...
Thank you
Asked by Dben on 2022-08-31 10:49:11 UTC
Comments