diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp index 0da12050ab..13c2d1910b 100644 --- a/rclcpp/include/rclcpp/serialized_message.hpp +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -24,7 +24,7 @@ namespace rclcpp { /// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks -class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t +class RCLCPP_PUBLIC_TYPE SerializedMessage { public: /// Default constructor for a SerializedMessage @@ -52,16 +52,16 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t const rcl_allocator_t & allocator = rcl_get_default_allocator()); /// Copy Constructor for a SerializedMessage - SerializedMessage(const SerializedMessage & serialized_message); + SerializedMessage(const SerializedMessage & other); /// Constructor for a SerializedMessage from a rcl_serialized_message_t - explicit SerializedMessage(const rcl_serialized_message_t & serialized_message); + explicit SerializedMessage(const rcl_serialized_message_t & other); /// Move Constructor for a SerializedMessage - SerializedMessage(SerializedMessage && serialized_message); + SerializedMessage(SerializedMessage && other); /// Constructor for a SerializedMessage from a moved rcl_serialized_message_t - explicit SerializedMessage(rcl_serialized_message_t && serialized_message); + explicit SerializedMessage(rcl_serialized_message_t && other); /// Copy assignment operator SerializedMessage & operator=(const SerializedMessage & other); @@ -77,6 +77,29 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t /// Destructor for a SerializedMessage virtual ~SerializedMessage(); + + /// Get the underlying rcl_serialized_t handle + rcl_serialized_message_t & get_rcl_serialized_message(); + + // Get a const handle to the underlying rcl_serialized_message_t + const rcl_serialized_message_t & get_rcl_serialized_message() const; + + /// Get the size of the serialized data buffer + /** + * Note, this is different from the actual amount of allocated memory. + * This can be obtained via a call to `capacity`. + */ + size_t size() const; + + /// Get the size of allocated memory for the data buffer + /** + * Note, this is different from the amount of content in the buffer. + * This can be obtained via a call to `size`. + */ + size_t capacity() const; + +private: + rcl_serialized_message_t serialized_message_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/serialization.cpp b/rclcpp/src/rclcpp/serialization.cpp index bd987117a1..689757c98c 100644 --- a/rclcpp/src/rclcpp/serialization.cpp +++ b/rclcpp/src/rclcpp/serialization.cpp @@ -43,7 +43,7 @@ void SerializationBase::serialize_message( const auto ret = rmw_serialize( ros_message, type_support_, - serialized_message); + &serialized_message->get_rcl_serialized_message()); if (ret != RMW_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to serialize ROS message."); } @@ -55,12 +55,12 @@ void SerializationBase::deserialize_message( rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); rcpputils::check_true( - 0 != serialized_message->buffer_capacity && - 0 != serialized_message->buffer_length && - nullptr != serialized_message->buffer, "Serialized message is wrongly initialized."); + 0 != serialized_message->capacity() && 0 != serialized_message->size(), + "Serialized message is wrongly initialized."); rcpputils::check_true(nullptr != ros_message, "ROS message is a nullpointer."); - const auto ret = rmw_deserialize(serialized_message, type_support_, ros_message); + const auto ret = rmw_deserialize( + &serialized_message->get_rcl_serialized_message(), type_support_, ros_message); if (ret != RMW_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message."); } diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp index f1a4215ae0..a667977cea 100644 --- a/rclcpp/src/rclcpp/serialized_message.cpp +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -24,6 +24,21 @@ namespace rclcpp { +inline void copy_rcl_message(const rcl_serialized_message_t & from, rcl_serialized_message_t & to) +{ + const auto ret = rmw_serialized_message_init( + &to, from.buffer_capacity, &from.allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // do not call memcpy if the pointer is "static" + if (to.buffer != from.buffer) { + std::memcpy(to.buffer, from.buffer, from.buffer_length); + } + to.buffer_length = from.buffer_length; +} + /// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks SerializedMessage::SerializedMessage(const rcl_allocator_t & allocator) : SerializedMessage(0u, allocator) @@ -31,87 +46,66 @@ SerializedMessage::SerializedMessage(const rcl_allocator_t & allocator) SerializedMessage::SerializedMessage( size_t initial_capacity, const rcl_allocator_t & allocator) -: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message()) +: serialized_message_(rmw_get_zero_initialized_serialized_message()) { const auto ret = rmw_serialized_message_init( - this, initial_capacity, &allocator); + &serialized_message_, initial_capacity, &allocator); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } -SerializedMessage::SerializedMessage(const SerializedMessage & serialized_message) -: SerializedMessage(static_cast(serialized_message)) +SerializedMessage::SerializedMessage(const SerializedMessage & other) +: SerializedMessage(other.serialized_message_) {} -SerializedMessage::SerializedMessage(const rcl_serialized_message_t & serialized_message) -: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message()) +SerializedMessage::SerializedMessage(const rcl_serialized_message_t & other) +: serialized_message_(rmw_get_zero_initialized_serialized_message()) { - const auto ret = rmw_serialized_message_init( - this, serialized_message.buffer_capacity, &serialized_message.allocator); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - - // do not call memcpy if the pointer is "static" - if (buffer != serialized_message.buffer) { - std::memcpy(buffer, serialized_message.buffer, serialized_message.buffer_length); - } - buffer_length = serialized_message.buffer_length; + copy_rcl_message(other, serialized_message_); } -SerializedMessage::SerializedMessage(SerializedMessage && serialized_message) -: SerializedMessage( - std::forward( - static_cast(serialized_message))) -{} +SerializedMessage::SerializedMessage(SerializedMessage && other) +: SerializedMessage(other.serialized_message_) +{ + other.serialized_message_ = rmw_get_zero_initialized_serialized_message(); +} -SerializedMessage::SerializedMessage(rcl_serialized_message_t && serialized_message) -: rcl_serialized_message_t(serialized_message) +SerializedMessage::SerializedMessage(rcl_serialized_message_t && other) +: serialized_message_(other) { // reset buffer to prevent double free - serialized_message = rmw_get_zero_initialized_serialized_message(); + other = rmw_get_zero_initialized_serialized_message(); } SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other) { - *this = static_cast(other); + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other.serialized_message_, serialized_message_); return *this; } SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other) { - *this = static_cast(rmw_get_zero_initialized_serialized_message()); - - const auto ret = rmw_serialized_message_init( - this, other.buffer_capacity, &other.allocator); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - - // do not call memcpy if the pointer is "static" - if (buffer != other.buffer) { - std::memcpy(buffer, other.buffer, other.buffer_length); - } - buffer_length = other.buffer_length; + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other, serialized_message_); return *this; } SerializedMessage & SerializedMessage::operator=(SerializedMessage && other) { - *this = static_cast(other); + *this = other.serialized_message_; + other.serialized_message_ = rmw_get_zero_initialized_serialized_message(); return *this; } SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && other) { - this->buffer = other.buffer; - this->buffer_capacity = other.buffer_length; - this->buffer_length = other.buffer_length; - this->allocator = other.allocator; + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + serialized_message_ = other; // reset original to prevent double free other = rmw_get_zero_initialized_serialized_message(); @@ -121,8 +115,8 @@ SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && oth SerializedMessage::~SerializedMessage() { - if (nullptr != buffer) { - const auto fini_ret = rmw_serialized_message_fini(this); + if (nullptr != serialized_message_.buffer) { + const auto fini_ret = rmw_serialized_message_fini(&serialized_message_); if (RCL_RET_OK != fini_ret) { RCLCPP_ERROR( get_logger("rclcpp"), @@ -131,4 +125,23 @@ SerializedMessage::~SerializedMessage() } } +rcl_serialized_message_t & SerializedMessage::get_rcl_serialized_message() +{ + return serialized_message_; +} + +const rcl_serialized_message_t & SerializedMessage::get_rcl_serialized_message() const +{ + return serialized_message_; +} + +size_t SerializedMessage::size() const +{ + return serialized_message_.buffer_length; +} + +size_t SerializedMessage::capacity() const +{ + return serialized_message_.buffer_capacity; +} } // namespace rclcpp diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp index 9943685b02..0c8a037490 100644 --- a/rclcpp/test/test_serialized_message.cpp +++ b/rclcpp/test/test_serialized_message.cpp @@ -30,16 +30,14 @@ TEST(TestSerializedMessage, empty_initialize) { rclcpp::SerializedMessage serialized_message; - EXPECT_TRUE(serialized_message.buffer == nullptr); - EXPECT_EQ(0u, serialized_message.buffer_length); - EXPECT_EQ(0u, serialized_message.buffer_capacity); + EXPECT_EQ(0u, serialized_message.size()); + EXPECT_EQ(0u, serialized_message.capacity()); } TEST(TestSerializedMessage, initialize_with_capacity) { rclcpp::SerializedMessage serialized_message(13); - EXPECT_TRUE(serialized_message.buffer != nullptr); - EXPECT_EQ(0u, serialized_message.buffer_length); - EXPECT_EQ(13u, serialized_message.buffer_capacity); + EXPECT_EQ(0u, serialized_message.size()); + EXPECT_EQ(13u, serialized_message.capacity()); } TEST(TestSerializedMessage, various_constructors) { @@ -48,65 +46,72 @@ TEST(TestSerializedMessage, various_constructors) { rclcpp::SerializedMessage serialized_message(content_size); // manually copy some content - std::memcpy(serialized_message.buffer, content.c_str(), content.size()); - serialized_message.buffer[content.size()] = '\0'; - serialized_message.buffer_length = content_size; - EXPECT_STREQ(content.c_str(), reinterpret_cast(serialized_message.buffer)); - EXPECT_EQ(content_size, serialized_message.buffer_capacity); + auto & rcl_handle = serialized_message.get_rcl_serialized_message(); + std::memcpy(rcl_handle.buffer, content.c_str(), content.size()); + rcl_handle.buffer[content.size()] = '\0'; + rcl_handle.buffer_length = content_size; + EXPECT_STREQ(content.c_str(), reinterpret_cast(rcl_handle.buffer)); + EXPECT_EQ(content_size, serialized_message.capacity()); // Copy Constructor rclcpp::SerializedMessage other_serialized_message(serialized_message); - EXPECT_EQ(content_size, other_serialized_message.buffer_capacity); - EXPECT_EQ(content_size, other_serialized_message.buffer_length); + EXPECT_EQ(content_size, other_serialized_message.capacity()); + EXPECT_EQ(content_size, other_serialized_message.size()); + auto & other_rcl_handle = other_serialized_message.get_rcl_serialized_message(); EXPECT_STREQ( - reinterpret_cast(serialized_message.buffer), - reinterpret_cast(other_serialized_message.buffer)); + reinterpret_cast(rcl_handle.buffer), + reinterpret_cast(other_rcl_handle.buffer)); // Move Constructor rclcpp::SerializedMessage yet_another_serialized_message(std::move(other_serialized_message)); - EXPECT_TRUE(other_serialized_message.buffer == nullptr); - EXPECT_EQ(0u, other_serialized_message.buffer_capacity); - EXPECT_EQ(0u, other_serialized_message.buffer_length); + auto & yet_another_rcl_handle = yet_another_serialized_message.get_rcl_serialized_message(); + EXPECT_TRUE(nullptr == other_rcl_handle.buffer); + EXPECT_EQ(0u, other_serialized_message.capacity()); + EXPECT_EQ(0u, other_serialized_message.size()); + EXPECT_TRUE(nullptr != yet_another_rcl_handle.buffer); + EXPECT_EQ(content_size, yet_another_serialized_message.size()); + EXPECT_EQ(content_size, yet_another_serialized_message.capacity()); +} + +TEST(TestSerializedMessage, various_constructors_from_rcl) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); - auto yet_another_rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); ASSERT_EQ(RCL_RET_OK, ret); - ret = rmw_serialized_message_init(&yet_another_rcl_serialized_msg, 13, &default_allocator); - ASSERT_EQ(RCL_RET_OK, ret); - // manually copy some content std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content.size()); rcl_serialized_msg.buffer[content.size()] = '\0'; rcl_serialized_msg.buffer_length = content_size; EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); - std::memcpy(yet_another_rcl_serialized_msg.buffer, content.c_str(), content.size()); - yet_another_rcl_serialized_msg.buffer[content.size()] = '\0'; - yet_another_rcl_serialized_msg.buffer_length = content_size; - EXPECT_EQ(13u, yet_another_rcl_serialized_msg.buffer_capacity); // Copy Constructor from rcl_serialized_message_t - rclcpp::SerializedMessage from_rcl_msg(rcl_serialized_msg); - EXPECT_EQ(13u, from_rcl_msg.buffer_capacity); - EXPECT_EQ(content_size, from_rcl_msg.buffer_length); + rclcpp::SerializedMessage serialized_message(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); // Move Constructor from rcl_serialized_message_t - rclcpp::SerializedMessage yet_another_moved_serialized_message(std::move( - yet_another_rcl_serialized_msg)); - EXPECT_TRUE(yet_another_rcl_serialized_msg.buffer == nullptr); - EXPECT_EQ(0u, yet_another_rcl_serialized_msg.buffer_capacity); - EXPECT_EQ(0u, yet_another_rcl_serialized_msg.buffer_length); + rclcpp::SerializedMessage another_serialized_message(std::move(rcl_serialized_msg)); + EXPECT_TRUE(nullptr == rcl_serialized_msg.buffer); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_length); + EXPECT_EQ(13u, another_serialized_message.capacity()); + EXPECT_EQ(content_size, another_serialized_message.size()); // Verify that despite being fini'd, the copy is real ret = rmw_serialized_message_fini(&rcl_serialized_msg); - ASSERT_EQ(RCL_RET_OK, ret); + ASSERT_EQ(RCUTILS_RET_INVALID_ARGUMENT, ret); // Buffer is null, because it was moved EXPECT_EQ(nullptr, rcl_serialized_msg.buffer); EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); EXPECT_EQ(0u, rcl_serialized_msg.buffer_length); - EXPECT_TRUE(nullptr != from_rcl_msg.buffer); - EXPECT_EQ(13u, from_rcl_msg.buffer_capacity); - EXPECT_EQ(content_size, from_rcl_msg.buffer_length); + EXPECT_EQ(13u, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); + + auto rcl_handle = serialized_message.get_rcl_serialized_message(); + EXPECT_TRUE(nullptr != rcl_handle.buffer); } TEST(TestSerializedMessage, serialization) {