Dnae adas/serialized message (#1075)

* Addes SerializedMessage and helper class for serialization to rcl_serialized_message

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Updateds subscription traits for SerializedMessage

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Addes tests SerializedMessage and subscription traits

@Karsten1987 Thank you for your support

Signed-off-by: Joshua Hampp <j.hampp@denso-adas.de>

* Update rclcpp/include/rclcpp/serialization.hpp

Co-Authored-By: Karsten Knese <Karsten1987@users.noreply.github.com>

* Update rclcpp/test/test_serialized_message.cpp

Co-Authored-By: Karsten Knese <Karsten1987@users.noreply.github.com>

* fix windows compilation

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* cosmetic touchups

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

Co-authored-by: Joshua Hampp <j.hampp@denso-adas.de>
Co-authored-by: Karsten Knese <Karsten1987@users.noreply.github.com>
Co-authored-by: Karsten Knese <karsten@openrobotics.org>
This commit is contained in:
DensoADAS 2020-04-22 02:30:35 +02:00 committed by GitHub
parent 649d72f835
commit 0f0a4a8e39
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 587 additions and 0 deletions

View file

@ -79,6 +79,8 @@ set(${PROJECT_NAME}_SRCS
src/rclcpp/publisher_base.cpp src/rclcpp/publisher_base.cpp
src/rclcpp/qos.cpp src/rclcpp/qos.cpp
src/rclcpp/qos_event.cpp src/rclcpp/qos_event.cpp
src/rclcpp/serialization.cpp
src/rclcpp/serialized_message.cpp
src/rclcpp/service.cpp src/rclcpp/service.cpp
src/rclcpp/signal_handler.cpp src/rclcpp/signal_handler.cpp
src/rclcpp/subscription_base.cpp src/rclcpp/subscription_base.cpp
@ -399,6 +401,15 @@ if(BUILD_TESTING)
${PROJECT_NAME} ${PROJECT_NAME}
) )
endif() 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) ament_add_gtest(test_service test/test_service.cpp)
if(TARGET test_service) if(TARGET test_service)
ament_target_dependencies(test_service ament_target_dependencies(test_service
@ -437,6 +448,7 @@ if(BUILD_TESTING)
"rcl" "rcl"
"test_msgs" "test_msgs"
) )
target_link_libraries(test_subscription_traits ${PROJECT_NAME})
endif() endif()
ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
if(TARGET test_find_weak_nodes) if(TARGET test_find_weak_nodes)

View file

@ -0,0 +1,102 @@
// 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 <memory>
#include <stdexcept>
#include <string>
#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;
namespace serialization_traits
{
// trait to check if type is the object oriented serialized message
template<typename T>
struct is_serialized_message_class : std::false_type
{};
template<>
struct is_serialized_message_class<rcl_serialized_message_t>: std::true_type
{};
template<>
struct is_serialized_message_class<SerializedMessage>: std::true_type
{};
} // namespace serialization_traits
/// Interface to (de)serialize a message
class RCLCPP_PUBLIC_TYPE SerializationBase
{
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
/**
* \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;
private:
const rosidl_message_type_support_t * type_support_;
};
/// Default implementation to (de)serialize a message by using rmw_(de)serialize
template<typename MessageT>
class Serialization : public SerializationBase
{
public:
/// Constructor of Serialization
Serialization()
: SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>())
{
static_assert(
!serialization_traits::is_serialized_message_class<MessageT>::value,
"Serialization of serialized message to serialized message is not possible.");
}
};
} // namespace rclcpp
#endif // RCLCPP__SERIALIZATION_HPP_

View file

@ -0,0 +1,84 @@
// 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"
#include "rclcpp/visibility_control.hpp"
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
{
public:
/// Default constructor for a SerializedMessage
/**
* 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.
*/
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 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.
*/
SerializedMessage(
size_t initial_capacity,
const rcl_allocator_t & allocator = rcl_get_default_allocator());
/// Copy Constructor for a SerializedMessage
SerializedMessage(const SerializedMessage & serialized_message);
/// 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);
/// 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
virtual ~SerializedMessage();
};
} // namespace rclcpp
#endif // RCLCPP__SERIALIZED_MESSAGE_HPP_

View file

@ -18,6 +18,8 @@
#include <memory> #include <memory>
#include "rclcpp/function_traits.hpp" #include "rclcpp/function_traits.hpp"
#include "rclcpp/serialized_message.hpp"
#include "rclcpp/subscription_options.hpp"
#include "rcl/types.h" #include "rcl/types.h"
namespace rclcpp namespace rclcpp
@ -49,6 +51,15 @@ struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_messag
: std::true_type : std::true_type
{}; {};
template<>
struct is_serialized_subscription_argument<SerializedMessage>: std::true_type
{};
template<>
struct is_serialized_subscription_argument<std::shared_ptr<SerializedMessage>>
: std::true_type
{};
template<typename T> template<typename T>
struct is_serialized_subscription : is_serialized_subscription_argument<T> struct is_serialized_subscription : is_serialized_subscription_argument<T>
{}; {};
@ -75,6 +86,7 @@ struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message
template< template<
typename CallbackT, typename CallbackT,
typename AllocatorT = std::allocator<void>,
// Do not attempt if CallbackT is an integer (mistaken for depth) // Do not attempt if CallbackT is an integer (mistaken for depth)
typename = std::enable_if_t<!std::is_integral< typename = std::enable_if_t<!std::is_integral<
std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>, std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
@ -85,6 +97,10 @@ template<
// Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile)
typename = std::enable_if_t<!std::is_same< typename = std::enable_if_t<!std::is_same<
rmw_qos_profile_t, rmw_qos_profile_t,
std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>,
// Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator
typename = std::enable_if_t<!std::is_same<
rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>,
std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value> std::remove_cv_t<std::remove_reference_t<CallbackT>>>::value>
> >
struct has_message_type : extract_message_type< struct has_message_type : extract_message_type<

View file

@ -0,0 +1,69 @@
// 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 <memory>
#include <string>
#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)
{
rcpputils::check_true(nullptr != type_support, "Typesupport is nullpointer.");
}
void SerializationBase::serialize_message(
const void * ros_message, SerializedMessage * serialized_message) const
{
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,
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(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.");
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) {
rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message.");
}
}
} // namespace rclcpp

View file

@ -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 <cstring>
#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 (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
}
SerializedMessage::SerializedMessage(const SerializedMessage & serialized_message)
: SerializedMessage(static_cast<const rcl_serialized_message_t &>(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 (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;
}
SerializedMessage::SerializedMessage(SerializedMessage && serialized_message)
: SerializedMessage(
std::forward<rcl_serialized_message_t>(
static_cast<rcl_serialized_message_t &&>(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<const rcl_serialized_message_t &>(other);
return *this;
}
SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other)
{
*this = static_cast<SerializedMessage>(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;
return *this;
}
SerializedMessage & SerializedMessage::operator=(SerializedMessage && other)
{
*this = static_cast<rcl_serialized_message_t &&>(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 (RCL_RET_OK != fini_ret) {
RCLCPP_ERROR(
get_logger("rclcpp"),
"Failed to destroy serialized message: %s", rcl_get_error_string().str);
}
}
}
} // namespace rclcpp

View file

@ -0,0 +1,149 @@
// 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 <gtest/gtest.h>
#include <cstring>
#include <memory>
#include <string>
#include <utility>
#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<char *>(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<char *>(serialized_message.buffer),
reinterpret_cast<char *>(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<MessageT> 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.get(), &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<typename MessageT>
void test_empty_msg_serialize()
{
rclcpp::Serialization<MessageT> serializer;
MessageT ros_msg;
rclcpp::SerializedMessage serialized_msg;
serializer.serialize_message(&ros_msg, &serialized_msg);
}
template<typename MessageT>
void test_empty_msg_deserialize()
{
rclcpp::Serialization<MessageT> serializer;
MessageT ros_msg;
rclcpp::SerializedMessage serialized_msg;
serializer.deserialize_message(&serialized_msg, &ros_msg);
}

View file

@ -21,6 +21,7 @@
#include "rcl/types.h" #include "rcl/types.h"
#include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/serialized_message.hpp"
#include "rclcpp/subscription_traits.hpp" #include "rclcpp/subscription_traits.hpp"
#include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.hpp"
@ -52,6 +53,16 @@ void not_serialized_unique_ptr_callback(
(void) unused; (void) unused;
} }
void rclcpp_serialized_callback_copy(rclcpp::SerializedMessage unused)
{
(void) unused;
}
void rclcpp_serialized_callback_shared_ptr(std::shared_ptr<rclcpp::SerializedMessage> unused)
{
(void) unused;
}
TEST(TestSubscriptionTraits, is_serialized_callback) { TEST(TestSubscriptionTraits, is_serialized_callback) {
// Test regular functions // Test regular functions
auto cb1 = &serialized_callback_copy; auto cb1 = &serialized_callback_copy;
@ -97,6 +108,16 @@ TEST(TestSubscriptionTraits, is_serialized_callback) {
static_assert( static_assert(
rclcpp::subscription_traits::is_serialized_callback<decltype(cb7)>::value == false, rclcpp::subscription_traits::is_serialized_callback<decltype(cb7)>::value == false,
"passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); "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<decltype(cb8)>::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<decltype(cb9)>::value == true,
"std::shared_ptr<rclcpp::SerializedMessage> in a callback makes it a serialized callback");
} }
TEST(TestSubscriptionTraits, callback_messages) { TEST(TestSubscriptionTraits, callback_messages) {