From 43102e3bf3d7877e1f7eb317ac8f51fc71ef1c7c Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:47:56 +0200 Subject: [PATCH 1/7] Addes SerializedMessage and helper class for serialization to rcl_serialized_message @Karsten1987 Thank you for your support Signed-off-by: Joshua Hampp --- rclcpp/CMakeLists.txt | 2 + rclcpp/include/rclcpp/serialization.hpp | 159 +++++++++++++++++++ rclcpp/include/rclcpp/serialized_message.hpp | 76 +++++++++ rclcpp/src/rclcpp/serialization.cpp | 68 ++++++++ rclcpp/src/rclcpp/serialized_message.cpp | 134 ++++++++++++++++ 5 files changed, 439 insertions(+) create mode 100644 rclcpp/include/rclcpp/serialization.hpp create mode 100644 rclcpp/include/rclcpp/serialized_message.hpp create mode 100644 rclcpp/src/rclcpp/serialization.cpp create mode 100644 rclcpp/src/rclcpp/serialized_message.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 51b028f090..240499de92 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -79,6 +79,8 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp src/rclcpp/qos_event.cpp + src/rclcpp/serialization.cpp + src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp src/rclcpp/subscription_base.cpp diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp new file mode 100644 index 0000000000..df2e548cc8 --- /dev/null +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -0,0 +1,159 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SERIALIZATION_HPP_ +#define RCLCPP__SERIALIZATION_HPP_ + +#include +#include + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rcl/types.h" +#include "rosidl_runtime_c/message_type_support_struct.h" + +namespace rclcpp +{ + +class SerializedMessage; + +/// Interface to (de)serialize a message +class SerializationBase +{ +protected: + explicit SerializationBase(const rosidl_message_type_support_t * type_support); + + virtual ~SerializationBase() = default; + + /// Serialize a ROS2 message to a serialized stream + /** + * \param[in] message The ROS2 message which is read and serialized by rmw. + * \param[out] serialized_message The serialized message. + */ + void serialize_message( + const void * ros_message, SerializedMessage * serialized_message) const; + + /// Deserialize a serialized stream to a ROS message + /** + * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw. + * \param[out] message The deserialized ROS2 message. + */ + void deserialize_message( + const SerializedMessage * serialized_message, void * ros_message) const; + + const rosidl_message_type_support_t * type_support_; +}; + +/// Default implementation to (de)serialize a message by using rmw_(de)serialize +template +class Serialization : public SerializationBase +{ +public: + Serialization() + : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle()) + {} + + void serialize_message( + const MessageT & ros_message, SerializedMessage & serialized_message) const + { + SerializationBase::serialize_message( + reinterpret_cast(&ros_message), + &serialized_message); + } + + void deserialize_message( + const SerializedMessage & serialized_message, MessageT & ros_message) const + { + SerializationBase::deserialize_message( + &serialized_message, + reinterpret_cast(&ros_message)); + } +}; + +template<> +class Serialization: public SerializationBase +{ +public: + Serialization() + : SerializationBase(nullptr) + {} + + void serialize_message( + const SerializedMessage & ros_message, + SerializedMessage & serialized_message) const + { + (void)ros_message; + (void)serialized_message; + throw std::runtime_error( + "Serialization of serialized message to serialized message is not possible."); + } + + void deserialize_message( + const SerializedMessage & serialized_message, + SerializedMessage & ros_message) const + { + (void)ros_message; + (void)serialized_message; + throw std::runtime_error( + "Deserialization of serialized message to serialized message is not possible."); + } +}; + +template<> +class Serialization: public SerializationBase +{ +public: + Serialization() + : SerializationBase(nullptr) + {} + + void serialize_message( + rcl_serialized_message_t & ros_message, + const SerializedMessage & serialized_message) const + { + (void)ros_message; + (void)serialized_message; + throw std::runtime_error( + "Serialization of serialized message to serialized message is not possible."); + } + + void deserialize_message( + const SerializedMessage & serialized_message, + rcl_serialized_message_t & ros_message) const + { + (void)ros_message; + (void)serialized_message; + throw std::runtime_error( + "Deserialization of serialized message to serialized message is not possible."); + } +}; + +// trait to check if type is the object oriented serialized message +template +struct is_serialized_message_class : std::false_type +{}; + +template<> +struct is_serialized_message_class: std::false_type +{}; + +template<> +struct is_serialized_message_class + : std::true_type +{}; + + +} // namespace rclcpp + +#endif // RCLCPP__SERIALIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp new file mode 100644 index 0000000000..3264d38c84 --- /dev/null +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -0,0 +1,76 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SERIALIZED_MESSAGE_HPP_ +#define RCLCPP__SERIALIZED_MESSAGE_HPP_ + +#include "rcl/allocator.h" +#include "rcl/types.h" + +namespace rclcpp +{ + +/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks +class SerializedMessage : public rcl_serialized_message_t +{ +public: + /// Default constructor for a SerializedMessage + /** + * Default constructs a serialized message and initalizes it + * with initial capacity of 0. + * + * \param[in] allocator The allocator to be used for the initialization. + */ + explicit SerializedMessage( + const rcl_allocator_t & allocator = rcl_get_default_allocator()); + + /// Default constructor for a SerializedMessage + /** + * Default constructs a serialized message and initalizes it + * with initial capacity of 0. + * + * \param[in] initial_capacity The amount of memory to be allocated. + * \param[in] allocator The allocator to be used for the initialization. + */ + SerializedMessage( + size_t initial_capacity, + const rcl_allocator_t & allocator = rcl_get_default_allocator()); + + /// Copy Constructor for a SerializedMessage + SerializedMessage(const SerializedMessage & serialized_message); + + /// Copy Constructor for a SerializedMessage from a rcl_serialized_message_t + explicit SerializedMessage(const rcl_serialized_message_t & serialized_message); + + /// Move Constructor for a SerializedMessage + SerializedMessage(SerializedMessage && serialized_message); + + /// Move Constructor for a SerializedMessage from a rcl_serialized_message_t + explicit SerializedMessage(rcl_serialized_message_t && serialized_message); + + SerializedMessage & operator=(const SerializedMessage & other); + + SerializedMessage & operator=(const rcl_serialized_message_t & other); + + SerializedMessage & operator=(SerializedMessage && other); + + SerializedMessage & operator=(rcl_serialized_message_t && other); + + /// Destructor for a SerializedMessage + ~SerializedMessage(); +}; + +} // namespace rclcpp + +#endif // RCLCPP__SERIALIZED_MESSAGE_HPP_ diff --git a/rclcpp/src/rclcpp/serialization.cpp b/rclcpp/src/rclcpp/serialization.cpp new file mode 100644 index 0000000000..120d68926a --- /dev/null +++ b/rclcpp/src/rclcpp/serialization.cpp @@ -0,0 +1,68 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/serialization.hpp" + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/serialized_message.hpp" + +#include "rcpputils/asserts.hpp" + +#include "rmw/rmw.h" + +namespace rclcpp +{ + +SerializationBase::SerializationBase(const rosidl_message_type_support_t * type_support) +: type_support_(type_support) +{} + +void SerializationBase::serialize_message( + const void * ros_message, SerializedMessage * serialized_message) const +{ + rcpputils::check_true(type_support_ != nullptr, "Typesupport is nullpointer."); + rcpputils::check_true(ros_message != nullptr, "ROS message is nullpointer."); + rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer."); + + const auto ret = rmw_serialize( + ros_message, + type_support_, + serialized_message); + if (ret != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to serialize ROS message."); + } +} + +void +SerializationBase::deserialize_message( + const SerializedMessage * serialized_message, void * ros_message) const +{ + rcpputils::check_true(type_support_ != nullptr, "Typesupport is nullpointer."); + rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer."); + rcpputils::check_true( + serialized_message->buffer_capacity != 0 && + serialized_message->buffer_length != 0 && + serialized_message->buffer != nullptr, "Serialized message is wrongly initialized."); + rcpputils::check_true(ros_message != nullptr, "ROS message is a nullpointer."); + + const auto ret = rmw_deserialize(serialized_message, type_support_, ros_message); + if (ret != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message."); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp new file mode 100644 index 0000000000..bda5de49f6 --- /dev/null +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -0,0 +1,134 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/serialized_message.hpp" + +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +#include "rmw/types.h" + +namespace rclcpp +{ + +/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks +SerializedMessage::SerializedMessage(const rcl_allocator_t & allocator) +: SerializedMessage(0u, allocator) +{} + +SerializedMessage::SerializedMessage( + size_t initial_capacity, const rcl_allocator_t & allocator) +: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message()) +{ + const auto ret = rmw_serialized_message_init( + this, initial_capacity, &allocator); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +SerializedMessage::SerializedMessage(const SerializedMessage & serialized_message) +: SerializedMessage(static_cast(serialized_message)) +{} + +SerializedMessage::SerializedMessage(const rcl_serialized_message_t & serialized_message) +: rcl_serialized_message_t(rmw_get_zero_initialized_serialized_message()) +{ + const auto ret = rmw_serialized_message_init( + this, serialized_message.buffer_capacity, &serialized_message.allocator); + if (ret != RCL_RET_OK) { + 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; +} + +SerializedMessage::SerializedMessage(SerializedMessage && serialized_message) +: SerializedMessage( + std::forward( + static_cast(serialized_message))) +{} + +SerializedMessage::SerializedMessage(rcl_serialized_message_t && serialized_message) +: rcl_serialized_message_t(serialized_message) +{ + // reset buffer to prevent double free + serialized_message = rmw_get_zero_initialized_serialized_message(); +} + +SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other) +{ + *this = static_cast(other); + + 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 (ret != RCL_RET_OK) { + 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; + + return *this; +} + +SerializedMessage & SerializedMessage::operator=(SerializedMessage && other) +{ + *this = static_cast(other); + + 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; + + // reset original to prevent double free + other = rmw_get_zero_initialized_serialized_message(); + + return *this; +} + +SerializedMessage::~SerializedMessage() +{ + if (nullptr != buffer) { + const auto fini_ret = rmw_serialized_message_fini(this); + if (fini_ret != RCL_RET_OK) { + RCLCPP_ERROR( + get_logger("rclcpp"), + "Failed to destroy serialized message: %s", rcl_get_error_string().str); + } + } +} + +} // namespace rclcpp From b548b3e093a260de50bdc36c7c30bb0eed629d42 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:48:23 +0200 Subject: [PATCH 2/7] Updateds subscription traits for SerializedMessage @Karsten1987 Thank you for your support Signed-off-by: Joshua Hampp --- rclcpp/include/rclcpp/subscription_traits.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index ecab458bd7..3a84764a75 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -18,6 +18,8 @@ #include #include "rclcpp/function_traits.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_options.hpp" #include "rcl/types.h" namespace rclcpp @@ -49,6 +51,15 @@ struct is_serialized_subscription_argument +struct is_serialized_subscription_argument: std::true_type +{}; + +template<> +struct is_serialized_subscription_argument> + : std::true_type +{}; + template struct is_serialized_subscription : is_serialized_subscription_argument {}; @@ -75,6 +86,7 @@ struct extract_message_type>: extract_message template< typename CallbackT, + typename AllocatorT = std::allocator, // Do not attempt if CallbackT is an integer (mistaken for depth) typename = std::enable_if_t>>::value>, @@ -85,6 +97,10 @@ template< // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator + typename = std::enable_if_t, std::remove_cv_t>>::value> > struct has_message_type : extract_message_type< From fcc5baa235f5b715411ec30451b73b0aeb16f409 Mon Sep 17 00:00:00 2001 From: Joshua Hampp Date: Tue, 21 Apr 2020 14:48:50 +0200 Subject: [PATCH 3/7] Addes tests SerializedMessage and subscription traits @Karsten1987 Thank you for your support Signed-off-by: Joshua Hampp --- rclcpp/CMakeLists.txt | 9 ++ rclcpp/test/test_serialized_message.cpp | 167 +++++++++++++++++++++++ rclcpp/test/test_subscription_traits.cpp | 20 +++ 3 files changed, 196 insertions(+) create mode 100644 rclcpp/test/test_serialized_message.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 240499de92..d149d852b1 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -401,6 +401,15 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) endif() + ament_add_gtest(test_serialized_message test/test_serialized_message.cpp) + if(TARGET test_serialized_message) + ament_target_dependencies(test_serialized_message + test_msgs + ) + target_link_libraries(test_serialized_message + ${PROJECT_NAME} + ) + endif() ament_add_gtest(test_service test/test_service.cpp) if(TARGET test_service) ament_target_dependencies(test_service diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp new file mode 100644 index 0000000000..c5938a4e7a --- /dev/null +++ b/rclcpp/test/test_serialized_message.cpp @@ -0,0 +1,167 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/asserts.hpp" + +#include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" + +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); +} + +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); +} + +TEST(TestSerializedMessage, various_constructors) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + + 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); + + // 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_STREQ( + reinterpret_cast(serialized_message.buffer), + reinterpret_cast(other_serialized_message.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 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); + + // 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); + + // Verify that despite being fini'd, the copy is real + ret = rmw_serialized_message_fini(&rcl_serialized_msg); + ASSERT_EQ(RCL_RET_OK, ret); + 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); +} + +TEST(TestSerializedMessage, serialization) { + using MessageT = test_msgs::msg::BasicTypes; + + rclcpp::Serialization serializer; + + auto basic_type_ros_msgs = get_messages_basic_types(); + for (const auto & ros_msg : basic_type_ros_msgs) { + // convert ros msg to serialized msg + rclcpp::SerializedMessage serialized_msg; + serializer.serialize_message(*ros_msg, serialized_msg); + + // convert serialized msg back to ros msg + MessageT deserialized_ros_msg; + serializer.deserialize_message(serialized_msg, deserialized_ros_msg); + + EXPECT_EQ(*ros_msg, deserialized_ros_msg); + } +} + +template +void test_empty_msg_serialize() +{ + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.serialize_message(ros_msg, serialized_msg); +} + +template +void test_empty_msg_deserialize() +{ + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.deserialize_message(serialized_msg, ros_msg); +} + +TEST(TestSerializedMessage, serialization_invalid) { + // serialized messages (used as ROS message) should throw an exception while (de)serialization + + EXPECT_THROW( + test_empty_msg_serialize(), + std::runtime_error); + EXPECT_THROW( + test_empty_msg_serialize(), + std::runtime_error); + + EXPECT_THROW( + test_empty_msg_deserialize(), + std::runtime_error); + EXPECT_THROW( + test_empty_msg_deserialize(), + std::runtime_error); +} diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index 3112cafffd..926b157a72 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -52,6 +52,16 @@ void not_serialized_unique_ptr_callback( (void) unused; } +void rclcpp_serialized_callback_copy(rclcpp::SerializedMessage unused) +{ + (void) unused; +} + +void rclcpp_serialized_callback_shared_ptr(std::shared_ptr unused) +{ + (void) unused; +} + TEST(TestSubscriptionTraits, is_serialized_callback) { // Test regular functions auto cb1 = &serialized_callback_copy; @@ -97,6 +107,16 @@ TEST(TestSubscriptionTraits, is_serialized_callback) { static_assert( rclcpp::subscription_traits::is_serialized_callback::value == false, "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); + + auto cb8 = &rclcpp_serialized_callback_copy; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rclcpp::SerializedMessage in a first argument callback makes it a serialized callback"); + + auto cb9 = &rclcpp_serialized_callback_shared_ptr; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "std::shared_ptr in a callback makes it a serialized callback"); } TEST(TestSubscriptionTraits, callback_messages) { From f503af50d6d06cd830314cb06b9f99db0a6f0e59 Mon Sep 17 00:00:00 2001 From: DensoADAS <46967124+DensoADAS@users.noreply.github.com> Date: Tue, 21 Apr 2020 19:09:00 +0200 Subject: [PATCH 4/7] Update rclcpp/include/rclcpp/serialization.hpp Co-Authored-By: Karsten Knese --- rclcpp/include/rclcpp/serialization.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index df2e548cc8..747bce4d4f 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -153,7 +153,6 @@ struct is_serialized_message_class : std::true_type {}; - } // namespace rclcpp #endif // RCLCPP__SERIALIZATION_HPP_ From 359184123e7c15f1e3a9cd80eb49228f1bf54c22 Mon Sep 17 00:00:00 2001 From: DensoADAS <46967124+DensoADAS@users.noreply.github.com> Date: Tue, 21 Apr 2020 19:18:41 +0200 Subject: [PATCH 5/7] Update rclcpp/test/test_serialized_message.cpp Co-Authored-By: Karsten Knese --- rclcpp/test/test_serialized_message.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp index c5938a4e7a..57142602ba 100644 --- a/rclcpp/test/test_serialized_message.cpp +++ b/rclcpp/test/test_serialized_message.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 1b87de01a963511371219dd08694fb4f3b9387a5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 Apr 2020 12:58:44 -0700 Subject: [PATCH 6/7] fix windows compilation Signed-off-by: Karsten Knese --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/serialization.hpp | 9 +++++++-- rclcpp/include/rclcpp/serialized_message.hpp | 6 ++++-- rclcpp/test/test_subscription_traits.cpp | 1 + 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d149d852b1..3918120e5a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -448,6 +448,7 @@ if(BUILD_TESTING) "rcl" "test_msgs" ) + target_link_libraries(test_subscription_traits ${PROJECT_NAME}) endif() ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index 747bce4d4f..bf27725494 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -16,20 +16,25 @@ #define RCLCPP__SERIALIZATION_HPP_ #include +#include #include -#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rclcpp/visibility_control.hpp" #include "rcl/types.h" + #include "rosidl_runtime_c/message_type_support_struct.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" + + namespace rclcpp { class SerializedMessage; /// Interface to (de)serialize a message -class SerializationBase +class RCLCPP_PUBLIC_TYPE SerializationBase { protected: explicit SerializationBase(const rosidl_message_type_support_t * type_support); diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp index 3264d38c84..6ec52e46c7 100644 --- a/rclcpp/include/rclcpp/serialized_message.hpp +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -18,11 +18,13 @@ #include "rcl/allocator.h" #include "rcl/types.h" +#include "rclcpp/visibility_control.hpp" + namespace rclcpp { /// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks -class SerializedMessage : public rcl_serialized_message_t +class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t { public: /// Default constructor for a SerializedMessage @@ -68,7 +70,7 @@ class SerializedMessage : public rcl_serialized_message_t SerializedMessage & operator=(rcl_serialized_message_t && other); /// Destructor for a SerializedMessage - ~SerializedMessage(); + virtual ~SerializedMessage(); }; } // namespace rclcpp diff --git a/rclcpp/test/test_subscription_traits.cpp b/rclcpp/test/test_subscription_traits.cpp index 926b157a72..c99f25bb97 100644 --- a/rclcpp/test/test_subscription_traits.cpp +++ b/rclcpp/test/test_subscription_traits.cpp @@ -21,6 +21,7 @@ #include "rcl/types.h" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/subscription_traits.hpp" #include "test_msgs/msg/empty.hpp" From 306d58c72474442293debb14de42ca52f8e267e0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 21 Apr 2020 15:07:01 -0700 Subject: [PATCH 7/7] cosmetic touchups Signed-off-by: Karsten Knese --- rclcpp/include/rclcpp/serialization.hpp | 117 +++++-------------- rclcpp/include/rclcpp/serialized_message.hpp | 12 +- rclcpp/src/rclcpp/serialization.cpp | 25 ++-- rclcpp/src/rclcpp/serialized_message.cpp | 8 +- rclcpp/test/test_serialized_message.cpp | 26 +---- 5 files changed, 58 insertions(+), 130 deletions(-) diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp index bf27725494..53fe59dc8b 100644 --- a/rclcpp/include/rclcpp/serialization.hpp +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -27,18 +27,39 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" - namespace rclcpp { class SerializedMessage; +namespace serialization_traits +{ +// trait to check if type is the object oriented serialized message +template +struct is_serialized_message_class : std::false_type +{}; + +template<> +struct is_serialized_message_class: std::true_type +{}; + +template<> +struct is_serialized_message_class: std::true_type +{}; +} // namespace serialization_traits + /// Interface to (de)serialize a message class RCLCPP_PUBLIC_TYPE SerializationBase { -protected: +public: + /// Constructor of SerializationBase + /** + * \param[in] type_support handle for the message type support + * to be used for serialization and deserialization. + */ explicit SerializationBase(const rosidl_message_type_support_t * type_support); + /// Destructor of SerializationBase virtual ~SerializationBase() = default; /// Serialize a ROS2 message to a serialized stream @@ -57,6 +78,7 @@ class RCLCPP_PUBLIC_TYPE SerializationBase void deserialize_message( const SerializedMessage * serialized_message, void * ros_message) const; +private: const rosidl_message_type_support_t * type_support_; }; @@ -65,99 +87,16 @@ template class Serialization : public SerializationBase { public: + /// Constructor of Serialization Serialization() : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle()) - {} - - void serialize_message( - const MessageT & ros_message, SerializedMessage & serialized_message) const - { - SerializationBase::serialize_message( - reinterpret_cast(&ros_message), - &serialized_message); - } - - void deserialize_message( - const SerializedMessage & serialized_message, MessageT & ros_message) const { - SerializationBase::deserialize_message( - &serialized_message, - reinterpret_cast(&ros_message)); + static_assert( + !serialization_traits::is_serialized_message_class::value, + "Serialization of serialized message to serialized message is not possible."); } }; -template<> -class Serialization: public SerializationBase -{ -public: - Serialization() - : SerializationBase(nullptr) - {} - - void serialize_message( - const SerializedMessage & ros_message, - SerializedMessage & serialized_message) const - { - (void)ros_message; - (void)serialized_message; - throw std::runtime_error( - "Serialization of serialized message to serialized message is not possible."); - } - - void deserialize_message( - const SerializedMessage & serialized_message, - SerializedMessage & ros_message) const - { - (void)ros_message; - (void)serialized_message; - throw std::runtime_error( - "Deserialization of serialized message to serialized message is not possible."); - } -}; - -template<> -class Serialization: public SerializationBase -{ -public: - Serialization() - : SerializationBase(nullptr) - {} - - void serialize_message( - rcl_serialized_message_t & ros_message, - const SerializedMessage & serialized_message) const - { - (void)ros_message; - (void)serialized_message; - throw std::runtime_error( - "Serialization of serialized message to serialized message is not possible."); - } - - void deserialize_message( - const SerializedMessage & serialized_message, - rcl_serialized_message_t & ros_message) const - { - (void)ros_message; - (void)serialized_message; - throw std::runtime_error( - "Deserialization of serialized message to serialized message is not possible."); - } -}; - -// trait to check if type is the object oriented serialized message -template -struct is_serialized_message_class : std::false_type -{}; - -template<> -struct is_serialized_message_class: std::false_type -{}; - -template<> -struct is_serialized_message_class - : std::true_type -{}; - } // namespace rclcpp #endif // RCLCPP__SERIALIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp index 6ec52e46c7..0da12050ab 100644 --- a/rclcpp/include/rclcpp/serialized_message.hpp +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -31,6 +31,7 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t /** * Default constructs a serialized message and initalizes it * with initial capacity of 0. + * The allocator defaults to `rcl_get_default_allocator()`. * * \param[in] allocator The allocator to be used for the initialization. */ @@ -40,7 +41,8 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t /// Default constructor for a SerializedMessage /** * Default constructs a serialized message and initalizes it - * with initial capacity of 0. + * with the provided capacity. + * The allocator defaults to `rcl_get_default_allocator()`. * * \param[in] initial_capacity The amount of memory to be allocated. * \param[in] allocator The allocator to be used for the initialization. @@ -52,21 +54,25 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage : public rcl_serialized_message_t /// Copy Constructor for a SerializedMessage SerializedMessage(const SerializedMessage & serialized_message); - /// Copy Constructor for a SerializedMessage from a rcl_serialized_message_t + /// Constructor for a SerializedMessage from a rcl_serialized_message_t explicit SerializedMessage(const rcl_serialized_message_t & serialized_message); /// Move Constructor for a SerializedMessage SerializedMessage(SerializedMessage && serialized_message); - /// Move Constructor for a SerializedMessage from a rcl_serialized_message_t + /// Constructor for a SerializedMessage from a moved rcl_serialized_message_t explicit SerializedMessage(rcl_serialized_message_t && serialized_message); + /// Copy assignment operator SerializedMessage & operator=(const SerializedMessage & other); + /// Copy assignment operator from a rcl_serialized_message_t SerializedMessage & operator=(const rcl_serialized_message_t & other); + /// Move assignment operator SerializedMessage & operator=(SerializedMessage && other); + /// Move assignment operator from a rcl_serialized_message_t SerializedMessage & operator=(rcl_serialized_message_t && other); /// Destructor for a SerializedMessage diff --git a/rclcpp/src/rclcpp/serialization.cpp b/rclcpp/src/rclcpp/serialization.cpp index 120d68926a..bd987117a1 100644 --- a/rclcpp/src/rclcpp/serialization.cpp +++ b/rclcpp/src/rclcpp/serialization.cpp @@ -29,14 +29,16 @@ namespace rclcpp SerializationBase::SerializationBase(const rosidl_message_type_support_t * type_support) : type_support_(type_support) -{} +{ + rcpputils::check_true(nullptr != type_support, "Typesupport is nullpointer."); +} void SerializationBase::serialize_message( const void * ros_message, SerializedMessage * serialized_message) const { - rcpputils::check_true(type_support_ != nullptr, "Typesupport is nullpointer."); - rcpputils::check_true(ros_message != nullptr, "ROS message is nullpointer."); - rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer."); + rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); + rcpputils::check_true(nullptr != ros_message, "ROS message is nullpointer."); + rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); const auto ret = rmw_serialize( ros_message, @@ -47,17 +49,16 @@ void SerializationBase::serialize_message( } } -void -SerializationBase::deserialize_message( +void SerializationBase::deserialize_message( const SerializedMessage * serialized_message, void * ros_message) const { - rcpputils::check_true(type_support_ != nullptr, "Typesupport is nullpointer."); - rcpputils::check_true(serialized_message != nullptr, "Serialized message is nullpointer."); + rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); + rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); rcpputils::check_true( - serialized_message->buffer_capacity != 0 && - serialized_message->buffer_length != 0 && - serialized_message->buffer != nullptr, "Serialized message is wrongly initialized."); - rcpputils::check_true(ros_message != nullptr, "ROS message is a nullpointer."); + 0 != serialized_message->buffer_capacity && + 0 != serialized_message->buffer_length && + nullptr != serialized_message->buffer, "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); if (ret != RMW_RET_OK) { diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp index bda5de49f6..f1a4215ae0 100644 --- a/rclcpp/src/rclcpp/serialized_message.cpp +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -35,7 +35,7 @@ SerializedMessage::SerializedMessage( { const auto ret = rmw_serialized_message_init( this, initial_capacity, &allocator); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } @@ -49,7 +49,7 @@ SerializedMessage::SerializedMessage(const rcl_serialized_message_t & serialized { const auto ret = rmw_serialized_message_init( this, serialized_message.buffer_capacity, &serialized_message.allocator); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -86,7 +86,7 @@ SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t const auto ret = rmw_serialized_message_init( this, other.buffer_capacity, &other.allocator); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -123,7 +123,7 @@ SerializedMessage::~SerializedMessage() { if (nullptr != buffer) { const auto fini_ret = rmw_serialized_message_fini(this); - if (fini_ret != RCL_RET_OK) { + if (RCL_RET_OK != fini_ret) { RCLCPP_ERROR( get_logger("rclcpp"), "Failed to destroy serialized message: %s", rcl_get_error_string().str); diff --git a/rclcpp/test/test_serialized_message.cpp b/rclcpp/test/test_serialized_message.cpp index 57142602ba..9943685b02 100644 --- a/rclcpp/test/test_serialized_message.cpp +++ b/rclcpp/test/test_serialized_message.cpp @@ -118,11 +118,11 @@ TEST(TestSerializedMessage, serialization) { for (const auto & ros_msg : basic_type_ros_msgs) { // convert ros msg to serialized msg rclcpp::SerializedMessage serialized_msg; - serializer.serialize_message(*ros_msg, serialized_msg); + serializer.serialize_message(ros_msg.get(), &serialized_msg); // convert serialized msg back to ros msg MessageT deserialized_ros_msg; - serializer.deserialize_message(serialized_msg, deserialized_ros_msg); + serializer.deserialize_message(&serialized_msg, &deserialized_ros_msg); EXPECT_EQ(*ros_msg, deserialized_ros_msg); } @@ -135,7 +135,7 @@ void test_empty_msg_serialize() MessageT ros_msg; rclcpp::SerializedMessage serialized_msg; - serializer.serialize_message(ros_msg, serialized_msg); + serializer.serialize_message(&ros_msg, &serialized_msg); } template @@ -145,23 +145,5 @@ void test_empty_msg_deserialize() MessageT ros_msg; rclcpp::SerializedMessage serialized_msg; - serializer.deserialize_message(serialized_msg, ros_msg); -} - -TEST(TestSerializedMessage, serialization_invalid) { - // serialized messages (used as ROS message) should throw an exception while (de)serialization - - EXPECT_THROW( - test_empty_msg_serialize(), - std::runtime_error); - EXPECT_THROW( - test_empty_msg_serialize(), - std::runtime_error); - - EXPECT_THROW( - test_empty_msg_deserialize(), - std::runtime_error); - EXPECT_THROW( - test_empty_msg_deserialize(), - std::runtime_error); + serializer.deserialize_message(&serialized_msg, &ros_msg); }