diff --git a/demo_nodes_cpp/src/topics/listener_serialized_message.cpp b/demo_nodes_cpp/src/topics/listener_serialized_message.cpp index 9cb695a9d..0b3afd6e1 100644 --- a/demo_nodes_cpp/src/topics/listener_serialized_message.cpp +++ b/demo_nodes_cpp/src/topics/listener_serialized_message.cpp @@ -17,6 +17,7 @@ #include "rcl/types.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "std_msgs/msg/string.hpp" @@ -41,31 +42,25 @@ class SerializedMessageListener : public rclcpp::Node // message to the callback. We can then further deserialize it and convert it into // a ros2 compliant message. auto callback = - [](const std::shared_ptr msg) -> void + [](const std::shared_ptr msg) -> void { // Print the serialized data message in HEX representation // This output corresponds to what you would see in e.g. Wireshark // when tracing the RTPS packets. - std::cout << "I heard data of length: " << msg->buffer_length << std::endl; - for (size_t i = 0; i < msg->buffer_length; ++i) { - printf("%02x ", msg->buffer[i]); + std::cout << "I heard data of length: " << msg->size() << std::endl; + for (size_t i = 0; i < msg->size(); ++i) { + printf("%02x ", msg->get_rcl_serialized_message().buffer[i]); } printf("\n"); // In order to deserialize the message we have to manually create a ROS2 // message in which we want to convert the serialized data. - auto string_msg = std::make_shared(); - auto string_ts = - rosidl_typesupport_cpp::get_message_type_support_handle(); - // The rmw_deserialize function takes the serialized data and a corresponding typesupport - // which is responsible on how to convert this data into a ROS2 message. - auto ret = rmw_deserialize(msg.get(), string_ts, string_msg.get()); - if (ret != RMW_RET_OK) { - fprintf(stderr, "failed to deserialize serialized message\n"); - return; - } + using MessageT = std_msgs::msg::String; + MessageT string_msg; + auto serializer = rclcpp::Serialization(); + serializer.deserialize_message(msg.get(), &string_msg); // Finally print the ROS2 message data - std::cout << "serialized data after deserialization: " << string_msg->data << std::endl; + std::cout << "serialized data after deserialization: " << string_msg.data << std::endl; }; // Create a subscription to the topic which can be matched with one or more compatible ROS // publishers. @@ -75,7 +70,7 @@ class SerializedMessageListener : public rclcpp::Node } private: - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; }; } // namespace demo_nodes_cpp diff --git a/demo_nodes_cpp/src/topics/talker_serialized_message.cpp b/demo_nodes_cpp/src/topics/talker_serialized_message.cpp index b9315a656..acf79605c 100644 --- a/demo_nodes_cpp/src/topics/talker_serialized_message.cpp +++ b/demo_nodes_cpp/src/topics/talker_serialized_message.cpp @@ -23,7 +23,7 @@ #include "std_msgs/msg/string.hpp" -#include "rmw/serialized_message.h" +#include "rclcpp/serialization.hpp" #include "demo_nodes_cpp/visibility_control.h" @@ -37,23 +37,9 @@ class SerializedMessageTalker : public rclcpp::Node public: DEMO_NODES_CPP_PUBLIC explicit SerializedMessageTalker(const rclcpp::NodeOptions & options) - : Node("serialized_message_talker", options) + : Node("serialized_message_talker", options), + serialized_msg_(0u) { - // In this example we send serialized data (serialized data). - // For this we initially allocate a container message - // which can hold the data. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - serialized_msg_ = rmw_get_zero_initialized_serialized_message(); - auto allocator = rcutils_get_default_allocator(); - auto initial_capacity = 0u; - auto ret = rmw_serialized_message_init( - &serialized_msg_, - initial_capacity, - &allocator); - if (ret != RCL_RET_OK) { - throw std::runtime_error("failed to initialize serialized message"); - } - // Create a function for when messages are to be sent. auto publish_message = [this]() -> void @@ -82,29 +68,18 @@ class SerializedMessageTalker : public rclcpp::Node // dynamically allocated before sending it to the wire. auto message_header_length = 8u; auto message_payload_length = static_cast(string_msg->data.size()); - auto ret = rmw_serialized_message_resize( - &serialized_msg_, message_header_length + message_payload_length); - if (ret != RCL_RET_OK) { - throw std::runtime_error("failed to resize serialized message"); - } + serialized_msg_.reserve(message_header_length + message_payload_length); - auto string_ts = - rosidl_typesupport_cpp::get_message_type_support_handle(); - // Given the correct typesupport, we can convert our ROS2 message into - // its binary representation (serialized_msg) - ret = rmw_serialize(string_msg.get(), string_ts, &serialized_msg_); - if (ret != RMW_RET_OK) { - fprintf(stderr, "failed to serialize serialized message\n"); - return; - } + static rclcpp::Serialization serializer; + serializer.serialize_message(string_msg.get(), &serialized_msg_); // For demonstration we print the ROS2 message format printf("ROS message:\n"); printf("%s\n", string_msg->data.c_str()); // And after the corresponding binary representation printf("serialized message:\n"); - for (size_t i = 0; i < serialized_msg_.buffer_length; ++i) { - printf("%02x ", serialized_msg_.buffer[i]); + for (size_t i = 0; i < serialized_msg_.size(); ++i) { + printf("%02x ", serialized_msg_.get_rcl_serialized_message().buffer[i]); } printf("\n"); @@ -118,18 +93,9 @@ class SerializedMessageTalker : public rclcpp::Node timer_ = this->create_wall_timer(1s, publish_message); } - DEMO_NODES_CPP_PUBLIC - ~SerializedMessageTalker() - { - auto ret = rmw_serialized_message_fini(&serialized_msg_); - if (ret != RCL_RET_OK) { - fprintf(stderr, "could not clean up memory for serialized message"); - } - } - private: size_t count_ = 1; - rcl_serialized_message_t serialized_msg_; + rclcpp::SerializedMessage serialized_msg_; rclcpp::Publisher::SharedPtr pub_; rclcpp::TimerBase::SharedPtr timer_; };