Deserialise a `SerializedMessage` into something other than a ROS2 message. E.g JSON

asked 2021-10-30 06:46:15 -0500

CraigH100 gravatar image

updated 2021-10-30 10:55:12 -0500

You can use rclcpp::Serialization<MessageT>::deserialize_message to deserialise a SerializedMessage into MessageT where MessageT is any ROS2 message. E.g.

std::shared_ptr<rclcpp::SerializedMessage> msg;
using MessageT = std_msgs::msg::String;
        MessageT string_msg;
        auto serializer = rclcpp::Serialization<MessageT>();
        serializer.deserialize_message(msg.get(), &string_msg);
        // Finally print the ROS2 message data
        std::cout << "serialized data after deserialization: " << string_msg.data << std::endl;

(Example from https://github.com/ros2/demos/blob/ma...)

But what if I wanted to deserialise into something other than a ROS2 message?

E.g

#include <nlohmann/json.hpp>
using nlohmann::json;

rclcpp::Serialization<json>().deserialize_message()

Or

rclcpp::Serialization<QVariantMap>().deserialize_message()

What would be the best way to go about doing this? Could I create a template specialisation? And what would be the easiest way to inspect the SerializedMessage and generate my own class from this?

Are there any functions from the implementation of rclcpp::Serialization<MessageT>::deserialize_message (where MessageT is a ROS2 message) that I could use in my template specialisation for when MessageT is not a ROS2 message?

Edit - Extra info in response to gvdhooms comment :

The reason why I want to do this is I am trying to create a topic echo node in C++. After I have done this, I then want to expose the rclcpp::create_generic_subscription to QML, so I can create subscriptions to topics in the QML language.

Using the resource you provided, here is my code for the cpp topic echo node:

#include <iostream>
#include "rclcpp/node.hpp"
#include "rclcpp/serialization.hpp"
#include "std_msgs/msg/string.hpp"
#include "dynmsg_demo/typesupport_utils.hpp"
#include "dynmsg_demo/message_reading.hpp"
#include <memory.h>


int main(int argc, char* argv[])
{
  std::vector<std::string> arguments(argv + 1, argv + argc);

  auto topic_name = arguments.at(0);
  auto topic_type = arguments.at(1);

  rclcpp::init(argc, argv);
  auto node = rclcpp::Node("topic_echo_cpp");

  auto callback = [&topic_type](std::shared_ptr<rclcpp::SerializedMessage> msg){

    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_result = message_to_yaml(result);
    std::cout << yaml_result << std::endl;

  };

  node.create_generic_subscription(topic_name, topic_type, 10, callback);

  return 0;
}

Here is my prototype for exposing rclcpp::create_generic_subscription to QML. It is currently in python, because type introspection is built into the language in python, but I would like to do this in C++.

@Slot(str, str, QJSValue)
def create_subscriber(self, topic_type: str, topic_name: str, js_callback: QJSValue):
    #Called from MainThread (QML)

    MsgType = get_message(topic_type)
    self.node.create_subscription(MsgType, topic_name, self.make_subscription_callback(js_callback), 10)
    self.node.get_logger().info(f"created subscriber to topic {topic_name}")

def make_subscription_callback(self, js_callback: QJSValue):
    #Called from MainThread (QML)
    def subscription_callback(msg: Ros2MessageProtocol):
        #Called from ThreadPoolExecutorTheaed (ROS2)
        self.node.get_logger().debug(f'executing js callback with {msg}')
        od = message_to_ordereddict(msg)
        data: str = json.dumps(od)
        #Emit signal so this can be done in the QML thread
        self.execute_subscription_js_callback_sig.emit(js_callback, data)
    return subscription_callback

@Slot(QJSValue, str)
def execute_subscription_js_callback(self, js_callback: QJSValue, data: str):
    #Called from MainThread (QML)
    converter = self.engine.evaluate("JSON.parse")
    qjsvalue = converter.call([QJSValue(data)])
    js_callback.call([qjsvalue])

And how this is ... (more)

edit retag flag offensive close merge delete

Comments

Without knowing why you want to do this (and so perhaps helping you with an xy-problem): I'd suggest to take a look at this ROS Discourse post: ROS 2 type support introspection and conversion of C/C++ messages to/from YAML. It discusses a similar topic and links to a set of packages/libraries which go in the direction you seem to want to go.

gvdhoorn gravatar image gvdhoorn  ( 2021-10-30 07:13:11 -0500 )edit

Thanks for the reply. As for why I want to do this, I want to expose the rclcpp::create_generic_subscription function to QML. I have already done this in python, but to be able to do that in C++ I need to be able to deserialise a SerializedMessage into a QJSVaue. Before doing this, I want to create a ros2 topic echo node in C++ to practice with type introspection.

CraigH100 gravatar image CraigH100  ( 2021-10-30 10:58:31 -0500 )edit
1

As for why I want to do this, I want to expose the rclcpp::create_generic_subscription function to QML.

you might want to take a look at StefanFabian/qml_ros_plugin. In QML ROS Plugin Release the author mentions they're working on a ROS 2 compatible version as well.

I would really recommend to not duplicate effort here.

gvdhoorn gravatar image gvdhoorn  ( 2021-10-31 06:33:32 -0500 )edit

I had no idea this existed! And has done for 3 years! Neither searching google for ROS2 QML nor rclqml returns this plugin.

I may send the author a message with my python prototype which works with ROS2. It currently can create subscribers to a topic of any type, publish messages, access the ros2 logger, call services, and call actions (which also sets up subscribers to the feedback and action status messages). I also have figured out how to safely pass data between the ros2 thread and qml thread

Maybe some of this will be useful for the author.

I only wanted to migrate it to C++ so I could compare performance, and if it is not significantly different, I would have kept it in python.

CraigH100 gravatar image CraigH100  ( 2021-10-31 16:07:51 -0500 )edit

Neither searching google for ROS2 QML nor rclqml returns this plugin.

it's about the first result for me. So either I am or you are in a search bubble.

There have actually been more ROS<->QML integrations, but most were ROS 1.

gvdhoorn gravatar image gvdhoorn  ( 2021-11-01 02:49:43 -0500 )edit