Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

use serialized message #441

Merged
merged 2 commits into from
Apr 23, 2020
Merged

use serialized message #441

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
08eadde
use serialized message
Karsten1987 Apr 22, 2020
7a35f52
publish serialized message
Karsten1987 Apr 22, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 11 additions & 16 deletions demo_nodes_cpp/src/topics/listener_serialized_message.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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<rmw_serialized_message_t> msg) -> void
[](const std::shared_ptr<rclcpp::SerializedMessage> 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<std_msgs::msg::String>();
auto string_ts =
rosidl_typesupport_cpp::get_message_type_support_handle<std_msgs::msg::String>();
// 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<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;
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.
Expand All @@ -75,7 +70,7 @@ class SerializedMessageListener : public rclcpp::Node
}

private:
rclcpp::Subscription<rmw_serialized_message_t>::SharedPtr sub_;
rclcpp::Subscription<rclcpp::SerializedMessage>::SharedPtr sub_;
};

} // namespace demo_nodes_cpp
Expand Down
52 changes: 9 additions & 43 deletions demo_nodes_cpp/src/topics/talker_serialized_message.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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
Expand Down Expand Up @@ -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<size_t>(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<std_msgs::msg::String>();
// 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<std_msgs::msg::String> 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");

Expand All @@ -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<std_msgs::msg::String>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
};
Expand Down