From 10795a4aae0bb4d755de0675ce29e43989eedbf3 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 10 Nov 2021 17:42:01 -0300 Subject: [PATCH 01/20] Added test for publisher and subscription with same type adapter Signed-off-by: Gonzalo de Pedro --- .../experimental/intra_process_manager.hpp | 14 ++ rclcpp/include/rclcpp/publisher.hpp | 39 +++- rclcpp/include/rclcpp/subscription.hpp | 7 + rclcpp/test/rclcpp/CMakeLists.txt | 18 ++ ...ption_publisher_with_same_type_adapter.cpp | 196 ++++++++++++++++++ ...ption_publisher_with_same_type_adapter.cpp | 194 +++++++++++++++++ 6 files changed, 463 insertions(+), 5 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp create mode 100644 rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index a927a81856..86e6acd2d3 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" @@ -184,6 +185,9 @@ class IntraProcessManager std::unique_ptr message, typename allocator::AllocRebind::allocator_type & allocator) { + + std::cout << "do_intra_process_publish --- " << std::endl; + using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; @@ -208,9 +212,13 @@ class IntraProcessManager } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) { + + std::cout << " to all the buffers requiring ownership --- " << std::endl; // There is at maximum 1 buffer that does not require ownership. // So this case is equivalent to all the buffers requiring ownership + std::cout << "message has type : " << typeid(message).name() << std::endl; + // Merge the two vector of ids into a unique one std::vector concatenated_vector(sub_ids.take_shared_subscriptions); concatenated_vector.insert( @@ -379,6 +387,11 @@ class IntraProcessManager std::vector subscription_ids, typename allocator::AllocRebind::allocator_type & allocator) { + + std::cout << "add owned msg to buffers" << std::endl; + + std::cout << "message has type : " << typeid(message).name() << std::endl; + using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; @@ -388,6 +401,7 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); + std::cout << "subscription_base has type : " << typeid(subscription_base).name() << std::endl; if (subscription_base) { auto subscription = std::dynamic_pointer_cast< rclcpp::experimental::SubscriptionIntraProcessBuffer diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index bebedda845..f91983dcdd 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -323,9 +323,11 @@ class Publisher : public PublisherBase // process manager and let it decide if it needs to be converted or not. // For now, convert it unconditionally and pass it the ROSMessageType // publish function specialization. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + std::cout << "Publisher LALA" << std::endl; + // auto unique_ros_msg = this->create_ros_message_unique_ptr(); + // rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); + //this->publish(std::move(unique_ros_msg)); + this->do_intra_process_publish(std::move(msg)); } /// Publish a message on the topic. @@ -490,8 +492,12 @@ class Publisher : public PublisherBase } } - void - do_intra_process_publish(std::unique_ptr msg) + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -508,6 +514,29 @@ class Publisher : public PublisherBase ros_message_type_allocator_); } + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + do_intra_process_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); + } + + std::shared_ptr do_intra_process_publish_and_return_shared( std::unique_ptr msg) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a77f4f8ef1..c889a77386 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -411,6 +411,13 @@ class Subscription : public SubscriptionBase ROSMessageTypeDeleter, MessageT>; std::shared_ptr subscription_intra_process_; + + // using SubscriptionIntraProcessSubscribedT = rclcpp::experimental::SubscriptionIntraProcess< + // SubscribedT, + // AllocatorT, + // SubscribedTypeDeleter, + // MessageT>; + // std::shared_ptr raw_type_subscription_intra_process_; }; } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d78a1adef5..c8292a530b 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -400,6 +400,24 @@ if(TARGET test_subscription_with_type_adapter) ${cpp_typesupport_target}) endif() +ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_publisher_with_same_type_adapter) + ament_target_dependencies(test_subscription_publisher_with_same_type_adapter + "rcutils" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription_publisher_with_same_type_adapter + ${PROJECT_NAME} + mimick + ${cpp_typesupport_target}) +endif() + ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) ament_target_dependencies(test_publisher_subscription_count_api diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp new file mode 100644 index 0000000000..9ae90e61d6 --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -0,0 +1,196 @@ +// Copyright 2021 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" + + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestSubscriptionPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + check_type_adapted_messages_are_received_by_intra_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + + + { + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::cout << "LALA" << std::endl; + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + std::cout << "LALA TO BE RECEIVED" << std::endl; + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} diff --git a/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp new file mode 100644 index 0000000000..84699b3816 --- /dev/null +++ b/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp @@ -0,0 +1,194 @@ +// Copyright 2021 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" + + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestSubscriptionPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + check_type_adapted_messages_are_received_by_intra_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + RCLCPP_WARN(rclcpp::get_node_logger(nullptr), "BLASSS"); + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + auto pub = node->create_publisher(topic_name, 1); + + { + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + + RCLCPP_WARN(rclcpp::get_node_logger(nullptr), "BLASSS"); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} From 066c79bf87e68b838136aec118e7c32e675e2841 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Mon, 15 Nov 2021 18:56:06 -0300 Subject: [PATCH 02/20] Added subscription type buffer Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 41 ++++--- .../experimental/intra_process_manager.hpp | 111 ++++++++++++++---- .../ros_message_intra_process_buffer.hpp | 70 +++++++++++ .../subscription_intra_process.hpp | 39 +++--- .../subscription_intra_process_buffer.hpp | 75 ++++++++++-- rclcpp/include/rclcpp/publisher.hpp | 38 +----- rclcpp/include/rclcpp/subscription.hpp | 11 +- 7 files changed, 271 insertions(+), 114 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 62e116af56..728da63e73 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -658,7 +658,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -677,39 +677,37 @@ class AnySubscriptionCallback // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } // conditions for ros message type else if constexpr (std::is_same_v) { // NOLINT @@ -762,9 +760,9 @@ class AnySubscriptionCallback TRACEPOINT(callback_end, static_cast(this)); } - void + void dispatch_intra_process( - std::unique_ptr message, + std::unique_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -783,39 +781,44 @@ class AnySubscriptionCallback // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + //We won't need to convert + //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } // conditions for ros message type else if constexpr (std::is_same_v) { // NOLINT diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 86e6acd2d3..f60c796886 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -33,6 +33,7 @@ #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/logger.hpp" @@ -363,14 +364,39 @@ class IntraProcessManager rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::ROSMessageIntraProcessBuffer + >(subscription_base); + + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, which " + "can happen when the publisher and subscription use different " + "allocator types, which is not supported"); + } else { + + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + // auto ros_message_subscription = std::dynamic_pointer_cast< + // rclcpp::experimental::ROSMessageIntraProcessBuffer + // >(subscription_base); + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); + ros_message_subscription->provide_intra_process_message(ros_msg); + } else { + ros_message_subscription->provide_intra_process_message(message); + } + + } + } else { + subscription->provide_intra_process_data(message); } - subscription->provide_intra_process_message(message); + } else { subscriptions_.erase(id); } @@ -401,32 +427,69 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - std::cout << "subscription_base has type : " << typeid(subscription_base).name() << std::endl; if (subscription_base) { auto subscription = std::dynamic_pointer_cast< rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } - if (std::next(it) == subscription_ids.end()) { - // If this is the last subscription, give up ownership - subscription->provide_intra_process_message(std::move(message)); + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::ROSMessageIntraProcessBuffer + >(subscription_base); + + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, which " + "can happen when the publisher and subscription use different " + "allocator types, which is not supported"); + } else { + + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1); + ROSMessageTypeAllocatorTraits::construct(allocator, ptr); + Deleter deleter = message.get_deleter(); + auto ros_msg = std::unique_ptr(ptr, deleter); + rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); + ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); + } else { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + + ros_message_subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + ros_message_subscription->provide_intra_process_message(std::move(copy_message)); + } + } + + } } else { - // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; - Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(allocator, 1); - MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - - subscription->provide_intra_process_message(std::move(copy_message)); + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_data(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_data(std::move(copy_message)); + } } + } else { subscriptions_.erase(subscription_it); } diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp new file mode 100644 index 0000000000..30f9d08efd --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -0,0 +1,70 @@ +// Copyright 2021 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__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename RosMessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete +> +class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ROSMessageIntraProcessBuffer) + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + ROSMessageIntraProcessBuffer( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + {} + + virtual ~ROSMessageIntraProcessBuffer() + {} + + virtual void + provide_intra_process_message(ConstMessageSharedPtr message) = 0; + + virtual void + provide_intra_process_message(MessageUniquePtr message) = 0; + +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index f174cd5c07..679aff1644 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -42,40 +42,43 @@ namespace experimental { template< - typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete, - typename CallbackMessageT = MessageT> +/// MessageT::custom_type if MessageT is a TypeAdapter, +/// otherwise just MessageT. + typename SubscribedType, + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename CallbackMessageT = SubscribedType + > class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter - > + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter + > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter >; public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits; - using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc; + using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; + using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( - AnySubscriptionCallback callback, - std::shared_ptr allocator, + AnySubscriptionCallback callback, + std::shared_ptr allocator, rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( + : SubscriptionIntraProcessBuffer( allocator, context, topic_name, @@ -117,7 +120,7 @@ class SubscriptionIntraProcess void execute(std::shared_ptr & data) { - execute_impl(data); + execute_impl(data); } protected: @@ -154,7 +157,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3ec335f9f6..954d6ea4a4 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -43,22 +44,40 @@ namespace experimental template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete + typename Deleter = std::default_delete, + /// MessageT::custom_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type > -class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase +class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + using SubscribedType = SubscribedT; + using ROSMessageType = ROSMessageT; + + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using ConstDataSharedPtr = std::shared_ptr; + using DataUniquePtr = std::unique_ptr; using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< - MessageT, + SubscribedType, Alloc, - Deleter + SubscribedTypeDeleter >::UniquePtr; SubscriptionIntraProcessBuffer( @@ -67,10 +86,10 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + : ROSMessageIntraProcessBuffer(context, topic_name, qos_profile) { // Create the intra-process buffer. - buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, allocator); @@ -85,13 +104,45 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase void provide_intra_process_message(ConstMessageSharedPtr message) + { + + if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } else { + // auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + // SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); + // buffer_->add_shared(std::unique_ptr(ptr, subscribed_type_deleter_)); + // trigger_guard_condition(); + } + } + + void + provide_intra_process_message(MessageUniquePtr message) + { + + if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } else { + // auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + // SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + // rclcpp::TypeAdapter::convert_to_custom(*message, *ptr); + // buffer_->add_unique(std::unique_ptr(ptr, subscribed_type_deleter_)); + // trigger_guard_condition(); + } + + } + + void + provide_intra_process_data(ConstDataSharedPtr message) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); } void - provide_intra_process_message(MessageUniquePtr message) + provide_intra_process_data(DataUniquePtr message) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); @@ -107,7 +158,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase void trigger_guard_condition() { - gc_.trigger(); + this->gc_.trigger(); } BufferUniquePtr buffer_; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f91983dcdd..73d76e46dc 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -492,34 +492,8 @@ class Publisher : public PublisherBase } } - template - typename std::enable_if_t< - rosidl_generator_traits::is_message::value && - std::is_same::value - > - do_intra_process_publish(std::unique_ptr msg) - { - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publish msg which is a null pointer"); - } - - ipm->template do_intra_process_publish( - intra_process_publisher_id_, - std::move(msg), - ros_message_type_allocator_); - } - - template - typename std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value - > - do_intra_process_publish(std::unique_ptr msg) +void +do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -537,9 +511,9 @@ class Publisher : public PublisherBase } - std::shared_ptr + std::shared_ptr do_intra_process_publish_and_return_shared( - std::unique_ptr msg) + std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -550,11 +524,11 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_sharedtemplate do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), - ros_message_type_allocator_); + published_type_allocator_); } /// Return a new unique_ptr using the ROSMessageType of the publisher. diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index c889a77386..55c49fe135 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -406,18 +406,11 @@ class Subscription : public SubscriptionBase SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - ROSMessageType, + SubscribedType, AllocatorT, - ROSMessageTypeDeleter, - MessageT>; + SubscribedTypeDeleter>; std::shared_ptr subscription_intra_process_; - // using SubscriptionIntraProcessSubscribedT = rclcpp::experimental::SubscriptionIntraProcess< - // SubscribedT, - // AllocatorT, - // SubscribedTypeDeleter, - // MessageT>; - // std::shared_ptr raw_type_subscription_intra_process_; }; } // namespace rclcpp From 1ceabc8d540a5226666319ab5cd4afbe9d8b0df1 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Mon, 6 Dec 2021 12:04:49 -0300 Subject: [PATCH 03/20] test_any_subscription_callback isolated test working Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 50 +- .../experimental/intra_process_manager.hpp | 11 +- .../subscription_intra_process_buffer.hpp | 9 +- rclcpp/include/rclcpp/subscription.hpp | 3 +- rclcpp/test/rclcpp/CMakeLists.txt | 725 ------------------ ...ption_publisher_with_same_type_adapter.cpp | 194 ----- 6 files changed, 50 insertions(+), 942 deletions(-) delete mode 100644 rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 728da63e73..061ded9c4b 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -16,6 +16,7 @@ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #include +#include //TODO remove when removed std::cout s #include #include #include @@ -686,21 +687,21 @@ class AnySubscriptionCallback std::is_same_v )) { - callback(message); + // callback(message); //TODO Convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(message, message_info); + // callback(message, message_info); //TODO Convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(message); + // callback(message); //TODO Convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| @@ -711,29 +712,29 @@ class AnySubscriptionCallback } // conditions for ros message type else if constexpr (std::is_same_v) { // NOLINT - callback(*message); + //callback(*message); //TODO convert to rosMessage } else if constexpr (std::is_same_v) { - callback(*message, message_info); + //callback(*message, message_info); //TODO convert to rosMessage } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + // callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + // callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message); + // callback(message); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message, message_info); + // callback(message, message_info); } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] @@ -765,6 +766,9 @@ class AnySubscriptionCallback std::unique_ptr message, const rclcpp::MessageInfo & message_info) { + + std::cout << "ASC dispatch_intra_process" << std::endl; + TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { @@ -779,13 +783,17 @@ class AnySubscriptionCallback using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + std::cout << "IS TA: " << is_ta << std::endl; + // conditions for custom type if constexpr (is_ta && std::is_same_v) { //We won't need to convert //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + std::cout << "ASC crcb" << std::endl; callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + std::cout << "ASC crcb+info" << std::endl; callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( @@ -793,58 +801,62 @@ class AnySubscriptionCallback std::is_same_v )) { + std::cout << "ASC unorshcb" << std::endl; //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); - callback(message); + callback(std::move(message)); //TODO convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { + std::cout << "ASC unorshcb+info" << std::endl; //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); - callback(message, message_info); + callback(std::move(message), message_info); //TODO convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { + std::cout << "ASC shconsrref" << std::endl; //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); - callback(message); + // callback(message); //TODO Convert } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { + std::cout << "ASC shconsrref+info" << std::endl; //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); - callback(message, message_info); + //callback(message, message_info); //TODO Convert } // conditions for ros message type else if constexpr (std::is_same_v) { // NOLINT - callback(*message); + //callback(*message); } else if constexpr (std::is_same_v) { - callback(*message, message_info); + // callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + // callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + // callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + // callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + // callback(std::move(message), message_info); } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index f60c796886..5cc0b83db7 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -427,12 +427,18 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); + std::cout << "Subscription Base?" << std::endl; if (subscription_base) { + + std::cout << "Typed Subscription" << std::endl; + auto subscription = std::dynamic_pointer_cast< rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { + std::cout << "ROSMessage Subscription" << std::endl; + auto ros_message_subscription = std::dynamic_pointer_cast< rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); @@ -445,6 +451,8 @@ class IntraProcessManager "allocator types, which is not supported"); } else { + std::cout << "ROSMessage TypeAdapted Subscription" << std::endl; + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; if constexpr (rclcpp::TypeAdapter::is_specialized::value) { @@ -474,7 +482,7 @@ class IntraProcessManager } } else { - + std::cout << "Typed Subscription" << std::endl; if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership subscription->provide_intra_process_data(std::move(message)); @@ -491,6 +499,7 @@ class IntraProcessManager } } else { + std::cout << "Erasing subscription" << std::endl; subscriptions_.erase(subscription_it); } } diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 954d6ea4a4..2874c321b8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -18,10 +18,11 @@ #include #include +#include //TODO remove #include #include -#include #include +#include #include #include "rcl/error_handling.h" @@ -106,6 +107,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::is_specialized::value) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); @@ -120,7 +123,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::is_specialized::value) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); @@ -137,6 +140,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferadd_shared(std::move(message)); trigger_guard_condition(); } @@ -144,6 +148,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferadd_unique(std::move(message)); trigger_guard_condition(); } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 55c49fe135..3e6ca1599d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -408,7 +408,8 @@ class Subscription : public SubscriptionBase using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< SubscribedType, AllocatorT, - SubscribedTypeDeleter>; + SubscribedTypeDeleter, + MessageT>; std::shared_ptr subscription_intra_process_; }; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c8292a530b..4762bc5d27 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -15,391 +15,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs # Need the target name to depend on generated interface libraries rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") -ament_add_gtest( - test_allocator_common - allocator/test_allocator_common.cpp) -if(TARGET test_allocator_common) - target_link_libraries(test_allocator_common ${PROJECT_NAME}) -endif() -ament_add_gtest( - test_allocator_deleter - allocator/test_allocator_deleter.cpp) -if(TARGET test_allocator_deleter) - target_link_libraries(test_allocator_deleter ${PROJECT_NAME}) -endif() -ament_add_gtest( - test_exceptions - exceptions/test_exceptions.cpp) -if(TARGET test_exceptions) - target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) -endif() - -# Increasing timeout because connext can take a long time to destroy nodes -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 -ament_add_gtest( - test_allocator_memory_strategy - strategies/test_allocator_memory_strategy.cpp - TIMEOUT 360) -if(TARGET test_allocator_memory_strategy) - ament_target_dependencies(test_allocator_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) -endif() -ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) -if(TARGET test_message_pool_memory_strategy) - ament_target_dependencies(test_message_pool_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) -endif() -ament_add_gtest(test_any_service_callback test_any_service_callback.cpp) -if(TARGET test_any_service_callback) - ament_target_dependencies(test_any_service_callback - "test_msgs" - ) - target_link_libraries(test_any_service_callback ${PROJECT_NAME}) -endif() -ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) -if(TARGET test_any_subscription_callback) - ament_target_dependencies(test_any_subscription_callback - "test_msgs" - ) - target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) -endif() -ament_add_gtest(test_client test_client.cpp) -if(TARGET test_client) - ament_target_dependencies(test_client - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_client ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_create_timer test_create_timer.cpp) -if(TARGET test_create_timer) - ament_target_dependencies(test_create_timer - "rcl_interfaces" - "rmw" - "rcl" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_create_timer ${PROJECT_NAME}) - target_include_directories(test_create_timer PRIVATE ./) -endif() -ament_add_gtest(test_create_subscription test_create_subscription.cpp) -if(TARGET test_create_subscription) - target_link_libraries(test_create_subscription ${PROJECT_NAME}) - ament_target_dependencies(test_create_subscription - "test_msgs" - ) -endif() -ament_add_gtest(test_add_callback_groups_to_executor - test_add_callback_groups_to_executor.cpp - TIMEOUT 120) -if(TARGET test_add_callback_groups_to_executor) - target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor - "test_msgs" - ) -endif() -ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) -if(TARGET test_expand_topic_or_service_name) - ament_target_dependencies(test_expand_topic_or_service_name - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_function_traits test_function_traits.cpp) -if(TARGET test_function_traits) - target_include_directories(test_function_traits PUBLIC ../../include) - ament_target_dependencies(test_function_traits - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) -endif() -ament_add_gtest( - test_future_return_code - test_future_return_code.cpp) -if(TARGET test_future_return_code) - target_link_libraries(test_future_return_code ${PROJECT_NAME}) -endif() -ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) -if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) -endif() -ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) -if(TARGET test_intra_process_manager_with_allocators) - ament_target_dependencies(test_intra_process_manager_with_allocators - "test_msgs" - ) - target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) -endif() -ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) -if(TARGET test_ring_buffer_implementation) - ament_target_dependencies(test_ring_buffer_implementation - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) -endif() -ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp) -if(TARGET test_intra_process_buffer) - ament_target_dependencies(test_intra_process_buffer - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_loaned_message test_loaned_message.cpp) -ament_target_dependencies(test_loaned_message - "test_msgs" -) -target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) - -ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) -ament_target_dependencies(test_memory_strategy - "test_msgs" -) -target_link_libraries(test_memory_strategy ${PROJECT_NAME}) - -ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) -ament_target_dependencies(test_message_memory_strategy - "test_msgs" -) -target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) - -ament_add_gtest(test_node test_node.cpp TIMEOUT 240) -if(TARGET test_node) - ament_target_dependencies(test_node - "rcl_interfaces" - "rcpputils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_node ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_node_interfaces__get_node_interfaces - node_interfaces/test_get_node_interfaces.cpp) -if(TARGET test_node_interfaces__get_node_interfaces) - target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) -endif() -ament_add_gtest(test_node_interfaces__node_base - node_interfaces/test_node_base.cpp) -if(TARGET test_node_interfaces__node_base) - target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_clock - node_interfaces/test_node_clock.cpp) -if(TARGET test_node_interfaces__node_clock) - target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) -endif() -ament_add_gtest(test_node_interfaces__node_graph - node_interfaces/test_node_graph.cpp - TIMEOUT 120) -if(TARGET test_node_interfaces__node_graph) - ament_target_dependencies( - test_node_interfaces__node_graph - "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_parameters - node_interfaces/test_node_parameters.cpp) -if(TARGET test_node_interfaces__node_parameters) - target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_services - node_interfaces/test_node_services.cpp) -if(TARGET test_node_interfaces__node_services) - target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_timers - node_interfaces/test_node_timers.cpp) -if(TARGET test_node_interfaces__node_timers) - target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_topics - node_interfaces/test_node_topics.cpp) -if(TARGET test_node_interfaces__node_topics) - ament_target_dependencies( - test_node_interfaces__node_topics - "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_node_interfaces__node_waitables - node_interfaces/test_node_waitables.cpp) -if(TARGET test_node_interfaces__node_waitables) - target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) -endif() - -# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output -# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node -# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) -# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node -# ${PROJECT_NAME}) - -# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node -# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) -# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node -# ${PROJECT_NAME}) - -# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node -# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) -# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node -# ${PROJECT_NAME}) - -# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node -# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) -# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node -# ${PROJECT_NAME}) - -ament_add_gtest(test_node_global_args test_node_global_args.cpp) -if(TARGET test_node_global_args) - ament_target_dependencies(test_node_global_args - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_node_global_args ${PROJECT_NAME}) -endif() -ament_add_gtest(test_node_options test_node_options.cpp) -if(TARGET test_node_options) - ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_init_options test_init_options.cpp) -if(TARGET test_init_options) - ament_target_dependencies(test_init_options "rcl") - target_link_libraries(test_init_options ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_parameter_client test_parameter_client.cpp) -if(TARGET test_parameter_client) - ament_target_dependencies(test_parameter_client - "rcl_interfaces" - ) - target_link_libraries(test_parameter_client ${PROJECT_NAME}) -endif() -ament_add_gtest(test_parameter_service test_parameter_service.cpp) -if(TARGET test_parameter_service) - ament_target_dependencies(test_parameter_service - "rcl_interfaces" - ) - target_link_libraries(test_parameter_service ${PROJECT_NAME}) -endif() -ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp) -if(TARGET test_parameter_events_filter) - ament_target_dependencies(test_parameter_events_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) -endif() -ament_add_gtest(test_parameter test_parameter.cpp) -if(TARGET test_parameter) - ament_target_dependencies(test_parameter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter ${PROJECT_NAME}) -endif() -ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) -if(TARGET test_parameter_event_handler) - ament_target_dependencies(test_parameter_event_handler - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) -endif() -ament_add_gtest(test_parameter_map test_parameter_map.cpp) -if(TARGET test_parameter_map) - target_link_libraries(test_parameter_map ${PROJECT_NAME}) -endif() -ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) -if(TARGET test_publisher) - ament_target_dependencies(test_publisher - "rcl" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher ${PROJECT_NAME} mimick) -endif() - -set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") -if(WIN32) - set(append_library_dirs "${append_library_dirs}/$") -endif() - -ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" -) -if(TARGET test_publisher_with_type_adapter) - ament_target_dependencies(test_publisher_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_with_type_adapter - ${PROJECT_NAME} - mimick - ${cpp_typesupport_target}) -endif() - -ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" -) -if(TARGET test_subscription_with_type_adapter) - ament_target_dependencies(test_subscription_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_with_type_adapter - ${PROJECT_NAME} - mimick - ${cpp_typesupport_target}) -endif() - ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) @@ -417,343 +32,3 @@ if(TARGET test_subscription_publisher_with_same_type_adapter) mimick ${cpp_typesupport_target}) endif() - -ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) -if(TARGET test_publisher_subscription_count_api) - ament_target_dependencies(test_publisher_subscription_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) -endif() -ament_add_gtest(test_qos test_qos.cpp) -if(TARGET test_qos) - ament_target_dependencies(test_qos - "rmw" - ) - target_link_libraries(test_qos - ${PROJECT_NAME} - ) -endif() -function(test_generic_pubsub_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp - ENV ${rmw_implementation_env_var} - ) - if(TARGET test_generic_pubsub${target_suffix}) - target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_generic_pubsub${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) - endif() -endfunction() -call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) -ament_add_gtest(test_qos_event test_qos_event.cpp) -if(TARGET test_qos_event) - ament_target_dependencies(test_qos_event - "rmw" - "test_msgs" - ) - target_link_libraries(test_qos_event - ${PROJECT_NAME} - mimick - ) -endif() -ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) -if(TARGET test_qos_overriding_options) - target_link_libraries(test_qos_overriding_options - ${PROJECT_NAME} - ) -endif() -ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) -if(TARGET test_qos_parameters) - target_link_libraries(test_qos_parameters - ${PROJECT_NAME} - ) -endif() -ament_add_gtest(test_rate test_rate.cpp) -if(TARGET test_rate) - ament_target_dependencies(test_rate - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) -endif() -ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp) -if(TARGET test_serialized_message_allocator) - ament_target_dependencies(test_serialized_message_allocator - test_msgs - ) - target_link_libraries(test_serialized_message_allocator - ${PROJECT_NAME} - ) -endif() -ament_add_gtest(test_serialized_message 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_service.cpp) -if(TARGET test_service) - ament_target_dependencies(test_service - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service ${PROJECT_NAME} mimick) -endif() -# Creating and destroying nodes is slow with Connext, so this needs larger timeout. -ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) -if(TARGET test_subscription) - ament_target_dependencies(test_subscription - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription ${PROJECT_NAME} mimick) -endif() -ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp) -if(TARGET test_subscription_publisher_count_api) - ament_target_dependencies(test_subscription_publisher_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) -endif() -ament_add_gtest(test_subscription_traits test_subscription_traits.cpp) -if(TARGET test_subscription_traits) - ament_target_dependencies(test_subscription_traits - "rcl" - "test_msgs" - ) - target_link_libraries(test_subscription_traits ${PROJECT_NAME}) -endif() -ament_add_gtest(test_type_support test_type_support.cpp) -if(TARGET test_type_support) - ament_target_dependencies(test_type_support - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_type_support ${PROJECT_NAME}) -endif() -ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) -if(TARGET test_typesupport_helpers) - target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) -endif() -ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) -if(TARGET test_find_weak_nodes) - ament_target_dependencies(test_find_weak_nodes - "rcl" - ) - target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) -ament_target_dependencies(test_externally_defined_services - "rcl" - "test_msgs" -) -target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) - -ament_add_gtest(test_duration test_duration.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_duration) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_logger test_logger.cpp) -target_link_libraries(test_logger ${PROJECT_NAME}) - -ament_add_gmock(test_logging test_logging.cpp) -target_link_libraries(test_logging ${PROJECT_NAME}) - -ament_add_gtest(test_time test_time.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_time) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_timer test_timer.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_timer) - ament_target_dependencies(test_timer - "rcl") - target_link_libraries(test_timer ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_time_source test_time_source.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_time_source) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_utilities test_utilities.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_utilities) - ament_target_dependencies(test_utilities - "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_wait_for_message test_wait_for_message.cpp) -if(TARGET test_wait_for_message) - ament_target_dependencies(test_wait_for_message - "test_msgs") - target_link_libraries(test_wait_for_message ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_interface_traits test_interface_traits.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_interface_traits) - ament_target_dependencies(test_interface_traits - "rcl") - target_link_libraries(test_interface_traits ${PROJECT_NAME}) -endif() - -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 -ament_add_gtest( - test_executors - executors/test_executors.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" - TIMEOUT 180) -if(TARGET test_executors) - ament_target_dependencies(test_executors - "rcl" - "test_msgs") - target_link_libraries(test_executors ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_static_single_threaded_executor) - ament_target_dependencies(test_static_single_threaded_executor - "test_msgs") - target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_multi_threaded_executor) - ament_target_dependencies(test_multi_threaded_executor - "rcl") - target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) -if(TARGET test_static_executor_entities_collector) - ament_target_dependencies(test_static_executor_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_guard_condition test_guard_condition.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_guard_condition) - target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_wait_set test_wait_set.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" -) -if(TARGET test_subscription_topic_statistics) - ament_target_dependencies(test_subscription_topic_statistics - "builtin_interfaces" - "libstatistics_collector" - "rcl_interfaces" - "rcutils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "statistics_msgs" - "test_msgs") - target_link_libraries(test_subscription_topic_statistics - ${PROJECT_NAME} - ${cpp_typesupport_target}) -endif() - -ament_add_gtest(test_subscription_options test_subscription_options.cpp) -if(TARGET test_subscription_options) - ament_target_dependencies(test_subscription_options "rcl") - target_link_libraries(test_subscription_options ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp) -if(TARGET test_dynamic_storage) - ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") - target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) -if(TARGET test_storage_policy_common) - ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") - target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp) -if(TARGET test_static_storage) - ament_target_dependencies(test_static_storage "rcl" "test_msgs") - target_link_libraries(test_static_storage ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp) -if(TARGET test_thread_safe_synchronization) - ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") - target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) -if(TARGET test_rosout_qos) - ament_target_dependencies(test_rosout_qos "rcl") - target_link_libraries(test_rosout_qos ${PROJECT_NAME}) -endif() - -ament_add_gtest(test_executor test_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" - TIMEOUT 120) -if(TARGET test_executor) - ament_target_dependencies(test_executor "rcl") - target_link_libraries(test_executor ${PROJECT_NAME} mimick) -endif() - -ament_add_gtest(test_graph_listener test_graph_listener.cpp) -if(TARGET test_graph_listener) - target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) -endif() diff --git a/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp deleted file mode 100644 index 84699b3816..0000000000 --- a/rclcpp/test/test_subscription_publisher_with_same_type_adapter.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2021 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 -#include -#include - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/loaned_message.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" -#include "rclcpp/msg/string.hpp" - - -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - -using namespace std::chrono_literals; - -static const int g_max_loops = 200; -static const std::chrono::milliseconds g_sleep_per_loop(10); - - -class TestSubscriptionPublisher : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } -}; - -namespace rclcpp -{ - -template<> -struct TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = std::string; - using ros_message_type = rclcpp::msg::String; - - static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) - { - (void) source; - (void) destination; - throw std::runtime_error("This should not happen"); - } - - static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) - { - (void) source; - (void) destination; - throw std::runtime_error("This should not happen"); - } -}; - -} // namespace rclcpp - -void wait_for_message_to_be_received( - bool & is_received, - const std::shared_ptr & node) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - executor.spin_once(std::chrono::milliseconds(0)); - int i = 0; - while (!is_received && i < g_max_loops) { - printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); - executor.spin_once(g_sleep_per_loop); - } -} - -/* - * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. - */ -TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), - check_type_adapted_messages_are_received_by_intra_process_subscription) { - using StringTypeAdapter = rclcpp::TypeAdapter; - const std::string message_data = "Message Data"; - const std::string topic_name = "topic_name"; - - RCLCPP_WARN(rclcpp::get_node_logger(nullptr), "BLASSS"); - - auto node = rclcpp::Node::make_shared( - "test_intra_process", - rclcpp::NodeOptions().use_intra_process_comms(true)); - auto pub = node->create_publisher(topic_name, 1); - - { - { // std::unique_ptr - bool is_received = false; - auto callback = - [message_data, &is_received]( - std::unique_ptr msg) -> void { - is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - }; - auto sub = node->create_subscription(topic_name, 1, callback); - - - RCLCPP_WARN(rclcpp::get_node_logger(nullptr), "BLASSS"); - - auto pu_message = std::make_unique(message_data); - pub->publish(std::move(pu_message)); - - ASSERT_FALSE(is_received); - wait_for_message_to_be_received(is_received, node); - ASSERT_TRUE(is_received); - } - { // std::unique_ptr with message info - bool is_received = false; - auto callback = - [message_data, &is_received]( - std::unique_ptr msg, - const rclcpp::MessageInfo & message_info) -> void { - is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); - }; - auto sub = node->create_subscription(topic_name, 1, callback); - - auto pu_message = std::make_unique(message_data); - pub->publish(std::move(pu_message)); - - ASSERT_FALSE(is_received); - wait_for_message_to_be_received(is_received, node); - ASSERT_TRUE(is_received); - } - } -} From 6dc5e4eb32a34320da71ef8098b7572084baaf65 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Dec 2021 15:25:41 -0500 Subject: [PATCH 04/20] Add in more of the necessary logic on the publish side. (#2) That is, make sure to deal with inter-process publishing properly. This requires us to introduce two copies of do_intra_process_and_return_shared(), one of which deals with the ROS Message type and the other that deals with the PublishedType. This is kind of wasteful, and we may get rid of this later on, but this works for now. Signed-off-by: Chris Lalancette Make sure to add the appropriate library directory. (#3) Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/publisher.hpp | 106 ++++++++++++++++++++-------- rclcpp/test/rclcpp/CMakeLists.txt | 5 ++ 2 files changed, 83 insertions(+), 28 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 73d76e46dc..0f9a77e5bd 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -263,7 +263,7 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); this->do_inter_process_publish(*shared_msg); } else { this->do_intra_process_publish(std::move(msg)); @@ -319,15 +319,29 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - std::cout << "Publisher LALA" << std::endl; - // auto unique_ros_msg = this->create_ros_message_unique_ptr(); - // rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - //this->publish(std::move(unique_ros_msg)); - this->do_intra_process_publish(std::move(msg)); + if (!intra_process_is_enabled_) { + // If we aren't using intraprocess at all, do the type conversion + // immediately and publish interprocess. + auto unique_ros_msg = this->create_ros_message_unique_ptr(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); + this->do_inter_process_publish(*unique_ros_msg); + return; + } + // If an interprocess subscription exists, then the unique_ptr is promoted + // to a shared_ptr and published. + // This allows doing the intraprocess publish first and then doing the + // interprocess publish, resulting in lower publish-to-subscribe latency. + // It's not possible to do that with an unique_ptr, + // as do_intra_process_publish takes the ownership of the message. + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + // this->do_inter_process_publish(*shared_msg); + } else { + this->do_intra_process_publish(std::move(msg)); + } } /// Publish a message on the topic. @@ -348,26 +362,18 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - - // Avoid allocating when not using intra process. + // Avoid double allocation when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. ROSMessageType ros_msg; rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); // In this case we're not using intra process. - return this->do_inter_process_publish(ros_msg); + this->do_inter_process_publish(ros_msg); + return; } - // Otherwise we have to allocate memory in a unique_ptr, convert it, - // and pass it along. - // As the message is not const, a copy should be made. - // A shared_ptr could also be constructed here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + // Otherwise we have to allocate memory in a unique_ptr and pass it along. + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } void @@ -492,8 +498,8 @@ class Publisher : public PublisherBase } } -void -do_intra_process_publish(std::unique_ptr msg) + void + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -511,7 +517,12 @@ do_intra_process_publish(std::unique_ptr ms } - std::shared_ptr + template + typename + std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, std::shared_ptr + > do_intra_process_publish_and_return_shared( std::unique_ptr msg) { @@ -531,6 +542,36 @@ do_intra_process_publish(std::unique_ptr ms published_type_allocator_); } + template + typename + std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, std::shared_ptr + > + do_intra_process_publish_and_return_shared( + std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); + + auto unique_ros_msg = this->create_ros_message_unique_ptr(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); + + return unique_ros_msg; + } + /// Return a new unique_ptr using the ROSMessageType of the publisher. std::unique_ptr create_ros_message_unique_ptr() @@ -540,7 +581,7 @@ do_intra_process_publish(std::unique_ptr ms return std::unique_ptr(ptr, ros_message_type_deleter_); } - /// Duplicate a given ros message as a unique_ptr. + /// Duplicate a given ROS message as a unique_ptr. std::unique_ptr duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg) { @@ -549,6 +590,15 @@ do_intra_process_publish(std::unique_ptr ms return std::unique_ptr(ptr, ros_message_type_deleter_); } + /// Duplicate a given type adapted message as a unique_ptr. + std::unique_ptr + duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) + { + auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); + PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, published_type_deleter_); + } + /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 4762bc5d27..4ec155e136 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -15,6 +15,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs # Need the target name to depend on generated interface libraries rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") +set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") +if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") +endif() + ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) From 1334d338ca9b16e534bc1040b391fdd18262bf49 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Dec 2021 15:28:03 -0500 Subject: [PATCH 05/20] Re-enable most of the rclcpp tests. (#4) We comment out the ones that are currently giving us trouble. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 4 - rclcpp/include/rclcpp/publisher.hpp | 5 +- rclcpp/test/rclcpp/CMakeLists.txt | 720 ++++++++++++++++++ 3 files changed, 724 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 5cc0b83db7..7264ddd4a2 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -379,10 +379,6 @@ class IntraProcessManager using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; - // auto ros_message_subscription = std::dynamic_pointer_cast< - // rclcpp::experimental::ROSMessageIntraProcessBuffer - // >(subscription_base); - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageType ros_msg; rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 0f9a77e5bd..46b50eda53 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -338,7 +338,7 @@ class Publisher : public PublisherBase if (inter_process_publish_needed) { auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); - // this->do_inter_process_publish(*shared_msg); + this->do_inter_process_publish(*shared_msg); } else { this->do_intra_process_publish(std::move(msg)); } @@ -566,6 +566,9 @@ class Publisher : public PublisherBase std::move(msg), published_type_allocator_); + // TODO(clalancette): We are doing the conversion at least twice; inside of + // IntraProcessManager::do_intra_process_publish_and_return_shared(), and here. + // We should just do it in the IntraProcessManager and return it here. auto unique_ros_msg = this->create_ros_message_unique_ptr(); rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 4ec155e136..29d7b92901 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -15,11 +15,391 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs # Need the target name to depend on generated interface libraries rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") +ament_add_gtest( + test_allocator_common + allocator/test_allocator_common.cpp) +if(TARGET test_allocator_common) + target_link_libraries(test_allocator_common ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_allocator_deleter + allocator/test_allocator_deleter.cpp) +if(TARGET test_allocator_deleter) + target_link_libraries(test_allocator_deleter ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_exceptions + exceptions/test_exceptions.cpp) +if(TARGET test_exceptions) + target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) +endif() + +# Increasing timeout because connext can take a long time to destroy nodes +# TODO(brawner) remove when destroying Node for Connext is resolved. See: +# https://github.com/ros2/rclcpp/issues/1250 +ament_add_gtest( + test_allocator_memory_strategy + strategies/test_allocator_memory_strategy.cpp + TIMEOUT 360) +if(TARGET test_allocator_memory_strategy) + ament_target_dependencies(test_allocator_memory_strategy + "rcl" + "test_msgs" + ) + target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) +endif() +ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) +if(TARGET test_message_pool_memory_strategy) + ament_target_dependencies(test_message_pool_memory_strategy + "rcl" + "test_msgs" + ) + target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) +endif() +ament_add_gtest(test_any_service_callback test_any_service_callback.cpp) +if(TARGET test_any_service_callback) + ament_target_dependencies(test_any_service_callback + "test_msgs" + ) + target_link_libraries(test_any_service_callback ${PROJECT_NAME}) +endif() +# ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) +# if(TARGET test_any_subscription_callback) +# ament_target_dependencies(test_any_subscription_callback +# "test_msgs" +# ) +# target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) +# endif() +ament_add_gtest(test_client test_client.cpp) +if(TARGET test_client) + ament_target_dependencies(test_client + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_client ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_create_timer test_create_timer.cpp) +if(TARGET test_create_timer) + ament_target_dependencies(test_create_timer + "rcl_interfaces" + "rmw" + "rcl" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_create_timer ${PROJECT_NAME}) + target_include_directories(test_create_timer PRIVATE ./) +endif() +ament_add_gtest(test_create_subscription test_create_subscription.cpp) +if(TARGET test_create_subscription) + target_link_libraries(test_create_subscription ${PROJECT_NAME}) + ament_target_dependencies(test_create_subscription + "test_msgs" + ) +endif() +ament_add_gtest(test_add_callback_groups_to_executor + test_add_callback_groups_to_executor.cpp + TIMEOUT 120) +if(TARGET test_add_callback_groups_to_executor) + target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME}) + ament_target_dependencies(test_add_callback_groups_to_executor + "test_msgs" + ) +endif() +ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) +if(TARGET test_expand_topic_or_service_name) + ament_target_dependencies(test_expand_topic_or_service_name + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_function_traits test_function_traits.cpp) +if(TARGET test_function_traits) + target_include_directories(test_function_traits PUBLIC ../../include) + ament_target_dependencies(test_function_traits + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) +endif() +ament_add_gtest( + test_future_return_code + test_future_return_code.cpp) +if(TARGET test_future_return_code) + target_link_libraries(test_future_return_code ${PROJECT_NAME}) +endif() +# ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) +# if(TARGET test_intra_process_manager) +# ament_target_dependencies(test_intra_process_manager +# "rcl" +# "rcl_interfaces" +# "rmw" +# "rosidl_runtime_cpp" +# "rosidl_typesupport_cpp" +# ) +# target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) +# endif() +# ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) +# if(TARGET test_intra_process_manager_with_allocators) +# ament_target_dependencies(test_intra_process_manager_with_allocators +# "test_msgs" +# ) +# target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) +# endif() +ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) +if(TARGET test_ring_buffer_implementation) + ament_target_dependencies(test_ring_buffer_implementation + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) +endif() +ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp) +if(TARGET test_intra_process_buffer) + ament_target_dependencies(test_intra_process_buffer + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_loaned_message test_loaned_message.cpp) +ament_target_dependencies(test_loaned_message + "test_msgs" +) +target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) + +ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) +ament_target_dependencies(test_memory_strategy + "test_msgs" +) +target_link_libraries(test_memory_strategy ${PROJECT_NAME}) + +ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) +ament_target_dependencies(test_message_memory_strategy + "test_msgs" +) +target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) + +ament_add_gtest(test_node test_node.cpp TIMEOUT 240) +if(TARGET test_node) + ament_target_dependencies(test_node + "rcl_interfaces" + "rcpputils" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_node ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_node_interfaces__get_node_interfaces + node_interfaces/test_get_node_interfaces.cpp) +if(TARGET test_node_interfaces__get_node_interfaces) + target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_base + node_interfaces/test_node_base.cpp) +if(TARGET test_node_interfaces__node_base) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_clock + node_interfaces/test_node_clock.cpp) +if(TARGET test_node_interfaces__node_clock) + target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_graph + node_interfaces/test_node_graph.cpp + TIMEOUT 120) +if(TARGET test_node_interfaces__node_graph) + ament_target_dependencies( + test_node_interfaces__node_graph + "test_msgs") + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_parameters + node_interfaces/test_node_parameters.cpp) +if(TARGET test_node_interfaces__node_parameters) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_services + node_interfaces/test_node_services.cpp) +if(TARGET test_node_interfaces__node_services) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_timers + node_interfaces/test_node_timers.cpp) +if(TARGET test_node_interfaces__node_timers) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_topics + node_interfaces/test_node_topics.cpp) +if(TARGET test_node_interfaces__node_topics) + ament_target_dependencies( + test_node_interfaces__node_topics + "test_msgs") + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_node_interfaces__node_waitables + node_interfaces/test_node_waitables.cpp) +if(TARGET test_node_interfaces__node_waitables) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) +endif() + +# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +ament_add_gtest(test_node_global_args test_node_global_args.cpp) +if(TARGET test_node_global_args) + ament_target_dependencies(test_node_global_args + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_node_global_args ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_options test_node_options.cpp) +if(TARGET test_node_options) + ament_target_dependencies(test_node_options "rcl") + target_link_libraries(test_node_options ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_init_options test_init_options.cpp) +if(TARGET test_init_options) + ament_target_dependencies(test_init_options "rcl") + target_link_libraries(test_init_options ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_parameter_client test_parameter_client.cpp) +if(TARGET test_parameter_client) + ament_target_dependencies(test_parameter_client + "rcl_interfaces" + ) + target_link_libraries(test_parameter_client ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_service test_parameter_service.cpp) +if(TARGET test_parameter_service) + ament_target_dependencies(test_parameter_service + "rcl_interfaces" + ) + target_link_libraries(test_parameter_service ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp) +if(TARGET test_parameter_events_filter) + ament_target_dependencies(test_parameter_events_filter + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter test_parameter.cpp) +if(TARGET test_parameter) + ament_target_dependencies(test_parameter + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) +if(TARGET test_parameter_event_handler) + ament_target_dependencies(test_parameter_event_handler + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_map test_parameter_map.cpp) +if(TARGET test_parameter_map) + target_link_libraries(test_parameter_map ${PROJECT_NAME}) +endif() +ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) +if(TARGET test_publisher) + ament_target_dependencies(test_publisher + "rcl" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick) +endif() + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") if(WIN32) set(append_library_dirs "${append_library_dirs}/$") endif() +# ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp +# APPEND_LIBRARY_DIRS "${append_library_dirs}" +# ) +# if(TARGET test_publisher_with_type_adapter) +# ament_target_dependencies(test_publisher_with_type_adapter +# "rcutils" +# "rcl_interfaces" +# "rmw" +# "rosidl_runtime_cpp" +# "rosidl_typesupport_cpp" +# "test_msgs" +# ) +# target_link_libraries(test_publisher_with_type_adapter +# ${PROJECT_NAME} +# mimick +# ${cpp_typesupport_target}) +# endif() + +# ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp +# APPEND_LIBRARY_DIRS "${append_library_dirs}" +# ) +# if(TARGET test_subscription_with_type_adapter) +# ament_target_dependencies(test_subscription_with_type_adapter +# "rcutils" +# "rcl_interfaces" +# "rmw" +# "rosidl_runtime_cpp" +# "rosidl_typesupport_cpp" +# "test_msgs" +# ) +# target_link_libraries(test_subscription_with_type_adapter +# ${PROJECT_NAME} +# mimick +# ${cpp_typesupport_target}) +# endif() + ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) @@ -37,3 +417,343 @@ if(TARGET test_subscription_publisher_with_same_type_adapter) mimick ${cpp_typesupport_target}) endif() + +ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) +if(TARGET test_publisher_subscription_count_api) + ament_target_dependencies(test_publisher_subscription_count_api + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) +endif() +ament_add_gtest(test_qos test_qos.cpp) +if(TARGET test_qos) + ament_target_dependencies(test_qos + "rmw" + ) + target_link_libraries(test_qos + ${PROJECT_NAME} + ) +endif() +function(test_generic_pubsub_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp + ENV ${rmw_implementation_env_var} + ) + if(TARGET test_generic_pubsub${target_suffix}) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) + ament_target_dependencies(test_generic_pubsub${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) +ament_add_gtest(test_qos_event test_qos_event.cpp) +if(TARGET test_qos_event) + ament_target_dependencies(test_qos_event + "rmw" + "test_msgs" + ) + target_link_libraries(test_qos_event + ${PROJECT_NAME} + mimick + ) +endif() +ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) +if(TARGET test_qos_overriding_options) + target_link_libraries(test_qos_overriding_options + ${PROJECT_NAME} + ) +endif() +ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) +if(TARGET test_qos_parameters) + target_link_libraries(test_qos_parameters + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_rate test_rate.cpp) +if(TARGET test_rate) + ament_target_dependencies(test_rate + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_rate + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp) +if(TARGET test_serialized_message_allocator) + ament_target_dependencies(test_serialized_message_allocator + test_msgs + ) + target_link_libraries(test_serialized_message_allocator + ${PROJECT_NAME} + ) +endif() +ament_add_gtest(test_serialized_message 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_service.cpp) +if(TARGET test_service) + ament_target_dependencies(test_service + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_service ${PROJECT_NAME} mimick) +endif() +# Creating and destroying nodes is slow with Connext, so this needs larger timeout. +ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) +if(TARGET test_subscription) + ament_target_dependencies(test_subscription + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick) +endif() +ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp) +if(TARGET test_subscription_publisher_count_api) + ament_target_dependencies(test_subscription_publisher_count_api + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) +endif() +ament_add_gtest(test_subscription_traits test_subscription_traits.cpp) +if(TARGET test_subscription_traits) + ament_target_dependencies(test_subscription_traits + "rcl" + "test_msgs" + ) + target_link_libraries(test_subscription_traits ${PROJECT_NAME}) +endif() +ament_add_gtest(test_type_support test_type_support.cpp) +if(TARGET test_type_support) + ament_target_dependencies(test_type_support + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_type_support ${PROJECT_NAME}) +endif() +ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) +if(TARGET test_typesupport_helpers) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) +endif() +ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) +if(TARGET test_find_weak_nodes) + ament_target_dependencies(test_find_weak_nodes + "rcl" + ) + target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) +ament_target_dependencies(test_externally_defined_services + "rcl" + "test_msgs" +) +target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) + +ament_add_gtest(test_duration test_duration.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_duration) + ament_target_dependencies(test_duration + "rcl") + target_link_libraries(test_duration ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_logger test_logger.cpp) +target_link_libraries(test_logger ${PROJECT_NAME}) + +ament_add_gmock(test_logging test_logging.cpp) +target_link_libraries(test_logging ${PROJECT_NAME}) + +ament_add_gtest(test_time test_time.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time) + ament_target_dependencies(test_time + "rcl") + target_link_libraries(test_time ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_timer test_timer.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_timer) + ament_target_dependencies(test_timer + "rcl") + target_link_libraries(test_timer ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_time_source test_time_source.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time_source) + ament_target_dependencies(test_time_source + "rcl") + target_link_libraries(test_time_source ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_utilities test_utilities.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_utilities) + ament_target_dependencies(test_utilities + "rcl") + target_link_libraries(test_utilities ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_wait_for_message test_wait_for_message.cpp) +if(TARGET test_wait_for_message) + ament_target_dependencies(test_wait_for_message + "test_msgs") + target_link_libraries(test_wait_for_message ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_interface_traits test_interface_traits.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_interface_traits) + ament_target_dependencies(test_interface_traits + "rcl") + target_link_libraries(test_interface_traits ${PROJECT_NAME}) +endif() + +# TODO(brawner) remove when destroying Node for Connext is resolved. See: +# https://github.com/ros2/rclcpp/issues/1250 +ament_add_gtest( + test_executors + executors/test_executors.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + ament_target_dependencies(test_executors + "rcl" + "test_msgs") + target_link_libraries(test_executors ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_static_single_threaded_executor) + ament_target_dependencies(test_static_single_threaded_executor + "test_msgs") + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_multi_threaded_executor) + ament_target_dependencies(test_multi_threaded_executor + "rcl") + target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_static_executor_entities_collector) + ament_target_dependencies(test_static_executor_entities_collector + "rcl" + "test_msgs") + target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_guard_condition test_guard_condition.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_guard_condition) + target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_wait_set test_wait_set.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_wait_set) + ament_target_dependencies(test_wait_set "test_msgs") + target_link_libraries(test_wait_set ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_topic_statistics) + ament_target_dependencies(test_subscription_topic_statistics + "builtin_interfaces" + "libstatistics_collector" + "rcl_interfaces" + "rcutils" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "statistics_msgs" + "test_msgs") + target_link_libraries(test_subscription_topic_statistics + ${PROJECT_NAME} + ${cpp_typesupport_target}) +endif() + +ament_add_gtest(test_subscription_options test_subscription_options.cpp) +if(TARGET test_subscription_options) + ament_target_dependencies(test_subscription_options "rcl") + target_link_libraries(test_subscription_options ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp) +if(TARGET test_dynamic_storage) + ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") + target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) +if(TARGET test_storage_policy_common) + ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") + target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp) +if(TARGET test_static_storage) + ament_target_dependencies(test_static_storage "rcl" "test_msgs") + target_link_libraries(test_static_storage ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp) +if(TARGET test_thread_safe_synchronization) + ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") + target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) +if(TARGET test_rosout_qos) + ament_target_dependencies(test_rosout_qos "rcl") + target_link_libraries(test_rosout_qos ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_executor test_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 120) +if(TARGET test_executor) + ament_target_dependencies(test_executor "rcl") + target_link_libraries(test_executor ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_graph_listener test_graph_listener.cpp) +if(TARGET test_graph_listener) + target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) +endif() From df5f52e6ba16506d0f092a33ffd7dc6384cc752a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 9 Nov 2021 15:58:51 -0500 Subject: [PATCH 06/20] Revamp the IntraProcessManager publish logic. There is no change here, the new way is just easier to read. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 63 +++++++++---------- 1 file changed, 30 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 7264ddd4a2..6eb15437f6 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -210,38 +210,36 @@ class IntraProcessManager this->template add_shared_msg_to_buffers( msg, sub_ids.take_shared_subscriptions); - } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT - sub_ids.take_shared_subscriptions.size() <= 1) - { + } else { + if (sub_ids.take_shared_subscriptions.size() <= 1) { + // There is at maximum 1 buffer that does not require ownership. + // So this case is equivalent to all the buffers requiring ownership - std::cout << " to all the buffers requiring ownership --- " << std::endl; - // There is at maximum 1 buffer that does not require ownership. - // So this case is equivalent to all the buffers requiring ownership + std::cout << " to all the buffers requiring ownership --- " << std::endl; - std::cout << "message has type : " << typeid(message).name() << std::endl; + std::cout << "message has type : " << typeid(message).name() << std::endl; - // Merge the two vector of ids into a unique one - std::vector concatenated_vector(sub_ids.take_shared_subscriptions); - concatenated_vector.insert( - concatenated_vector.end(), - sub_ids.take_ownership_subscriptions.begin(), - sub_ids.take_ownership_subscriptions.end()); + // Merge the two vector of ids into a unique one + std::vector concatenated_vector(sub_ids.take_shared_subscriptions); + concatenated_vector.insert( + concatenated_vector.end(), + sub_ids.take_ownership_subscriptions.begin(), + sub_ids.take_ownership_subscriptions.end()); - this->template add_owned_msg_to_buffers( - std::move(message), - concatenated_vector, - allocator); - } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT - sub_ids.take_shared_subscriptions.size() > 1) - { - // Construct a new shared pointer from the message - // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(allocator, *message); + this->template add_owned_msg_to_buffers( + std::move(message), + concatenated_vector, + allocator); + } else { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( - std::move(message), sub_ids.take_ownership_subscriptions, allocator); + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), sub_ids.take_ownership_subscriptions, allocator); + } } } @@ -288,12 +286,11 @@ class IntraProcessManager shared_msg, sub_ids.take_shared_subscriptions); } - if (!sub_ids.take_ownership_subscriptions.empty()) { - this->template add_owned_msg_to_buffers( - std::move(message), - sub_ids.take_ownership_subscriptions, - allocator); - } + + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); return shared_msg; } From 54166ee1149d677df2437118c38f9ea6681b43d2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Dec 2021 15:26:03 -0500 Subject: [PATCH 07/20] Push the publish conversion down to the intra-process manager (#6) * Remove template defaults that we never use. Signed-off-by: Chris Lalancette * Rename MessageT to PublishedType in the IntraProcessManager. It makes it easier to determine which type we are talking about. Signed-off-by: Chris Lalancette * Get all the types setup to push the conversion down to IPM. There really is no functional change here, just preparing to move the conversion down to the intra-process manager. Signed-off-by: Chris Lalancette * Move conversion down to the intra-process manager. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 145 ++++++++++++++---- rclcpp/include/rclcpp/publisher.hpp | 47 +----- 2 files changed, 125 insertions(+), 67 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 6eb15437f6..cebff15ac6 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -177,19 +177,20 @@ class IntraProcessManager * \param message the message that is being stored. */ template< - typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename PublishedType, + typename Alloc, + typename Deleter = std::default_delete + > void do_intra_process_publish( uint64_t intra_process_publisher_id, - std::unique_ptr message, - typename allocator::AllocRebind::allocator_type & allocator) + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator) { std::cout << "do_intra_process_publish --- " << std::endl; - using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; std::shared_lock lock(mutex_); @@ -206,9 +207,9 @@ class IntraProcessManager if (sub_ids.take_ownership_subscriptions.empty()) { // None of the buffers require ownership, so we promote the pointer - std::shared_ptr msg = std::move(message); + std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( msg, sub_ids.take_shared_subscriptions); } else { if (sub_ids.take_shared_subscriptions.size() <= 1) { @@ -226,18 +227,18 @@ class IntraProcessManager sub_ids.take_ownership_subscriptions.begin(), sub_ids.take_ownership_subscriptions.end()); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, allocator); } else { // Construct a new shared pointer from the message // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(allocator, *message); + auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } @@ -245,15 +246,31 @@ class IntraProcessManager template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> - std::shared_ptr + typename T, + typename PublishedType, + typename ROSMessageType, + typename Alloc, + typename ROSMessageTypeAllocatorTraits, + typename ROSMessageTypeAllocator, + typename ROSMessageTypeDeleter, + typename Deleter = std::default_delete + > + typename + std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, std::shared_ptr + > do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, - std::unique_ptr message, - typename allocator::AllocRebind::allocator_type & allocator) + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator, + ROSMessageTypeAllocator & ros_message_type_allocator, + ROSMessageTypeDeleter & ros_message_type_deleter) { - using MessageAllocTraits = allocator::AllocRebind; + (void)ros_message_type_allocator; + (void)ros_message_type_deleter; + + using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; std::shared_lock lock(mutex_); @@ -270,24 +287,24 @@ class IntraProcessManager if (sub_ids.take_ownership_subscriptions.empty()) { // If there are no owning, just convert to shared. - std::shared_ptr shared_msg = std::move(message); + std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(allocator, *message); + auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); @@ -296,6 +313,80 @@ class IntraProcessManager } } + template< + typename MessageT, + typename T, + typename PublishedType, + typename ROSMessageType, + typename Alloc, + typename ROSMessageTypeAllocatorTraits, + typename ROSMessageTypeAllocator, + typename ROSMessageTypeDeleter, + typename Deleter = std::default_delete + > + typename + std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, std::shared_ptr + > + do_intra_process_publish_and_return_shared( + uint64_t intra_process_publisher_id, + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator, + ROSMessageTypeAllocator & ros_message_type_allocator, + ROSMessageTypeDeleter & ros_message_type_deleter) + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); + auto unique_ros_msg = std::unique_ptr(ptr, ros_message_type_deleter); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *unique_ros_msg); + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling do_intra_process_publish for invalid or no longer existing publisher id"); + return nullptr; + } + const auto & sub_ids = publisher_it->second; + + if (sub_ids.take_ownership_subscriptions.empty()) { + // If there are no owning, just convert to shared. + std::shared_ptr shared_msg = std::move(message); + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); + } + } else { + // Construct a new shared pointer from the message for the buffers that + // do not require ownership and to return. + auto shared_msg = std::allocate_shared(allocator, *message); + + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions); + } + + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + + // TODO(clalancette): depending on how the publisher and subscriber are setup, we may end + // up doing a conversion more than once; in this function and down in + // add_{shared,owned}_msg_to_buffers(). We should probably push this down further to avoid + // that double conversion. + return unique_ros_msg; + } + /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC bool @@ -344,7 +435,8 @@ class IntraProcessManager template< typename MessageT, typename Alloc, - typename Deleter> + typename Deleter + > void add_shared_msg_to_buffers( std::shared_ptr message, @@ -398,8 +490,9 @@ class IntraProcessManager template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Alloc, + typename Deleter + > void add_owned_msg_to_buffers( std::unique_ptr message, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 46b50eda53..21e56a9ae5 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -518,11 +518,7 @@ class Publisher : public PublisherBase template - typename - std::enable_if_t< - rosidl_generator_traits::is_message::value && - std::is_same::value, std::shared_ptr - > + std::shared_ptr do_intra_process_publish_and_return_shared( std::unique_ptr msg) { @@ -535,44 +531,13 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_shared( + return ipm->template do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), - published_type_allocator_); - } - - template - typename - std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value, std::shared_ptr - > - do_intra_process_publish_and_return_shared( - std::unique_ptr msg) - { - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publish msg which is a null pointer"); - } - - ipm->template do_intra_process_publish_and_return_shared( - intra_process_publisher_id_, - std::move(msg), - published_type_allocator_); - - // TODO(clalancette): We are doing the conversion at least twice; inside of - // IntraProcessManager::do_intra_process_publish_and_return_shared(), and here. - // We should just do it in the IntraProcessManager and return it here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - - return unique_ros_msg; + published_type_allocator_, + ros_message_type_allocator_, + ros_message_type_deleter_); } /// Return a new unique_ptr using the ROSMessageType of the publisher. From 6fb025421add2d1be49ef03f5a3f0c2216eae19e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 20 Dec 2021 15:27:32 -0500 Subject: [PATCH 08/20] Clarify the template types (#7) * Rename the types so they are much more clear. Signed-off-by: Chris Lalancette * More renaming of types in the subscription side. Signed-off-by: Chris Lalancette * More clarification of the types. Signed-off-by: Chris Lalancette * More clarification of the types. Signed-off-by: Chris Lalancette * Cleanup subscription_intra_process_* classes. Remove virtual methods that are redundant (since they come from the underlying Waitable class), and mark methods as override as appropriate. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 185 +++++++++++------- .../subscription_intra_process.hpp | 24 +-- .../subscription_intra_process_base.hpp | 16 +- .../subscription_intra_process_buffer.hpp | 27 ++- rclcpp/include/rclcpp/publisher.hpp | 8 +- rclcpp/include/rclcpp/subscription.hpp | 4 +- 6 files changed, 145 insertions(+), 119 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index cebff15ac6..0ed7e11924 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -177,22 +177,26 @@ class IntraProcessManager * \param message the message that is being stored. */ template< + typename MessageT, typename PublishedType, + typename ROSMessageType, typename Alloc, - typename Deleter = std::default_delete + typename Deleter, + typename ROSMessageTypeAllocatorTraits, + typename ROSMessageTypeAllocator, + typename ROSMessageTypeDeleter, + typename PublishedTypeAllocator > void do_intra_process_publish( uint64_t intra_process_publisher_id, std::unique_ptr message, - typename allocator::AllocRebind::allocator_type & allocator) + PublishedTypeAllocator & published_type_allocator, + ROSMessageTypeAllocator & ros_message_type_allocator, + ROSMessageTypeDeleter & ros_message_type_deleter) { - std::cout << "do_intra_process_publish --- " << std::endl; - using MessageAllocTraits = allocator::AllocRebind; - using MessageAllocatorT = typename MessageAllocTraits::allocator_type; - std::shared_lock lock(mutex_); auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); @@ -209,8 +213,11 @@ class IntraProcessManager // None of the buffers require ownership, so we promote the pointer std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers( - msg, sub_ids.take_shared_subscriptions); + this->template add_shared_msg_to_buffers( + msg, + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); } else { if (sub_ids.take_shared_subscriptions.size() <= 1) { // There is at maximum 1 buffer that does not require ownership. @@ -227,19 +234,32 @@ class IntraProcessManager sub_ids.take_ownership_subscriptions.begin(), sub_ids.take_ownership_subscriptions.end()); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, - allocator); + published_type_allocator, + ros_message_type_allocator, + ros_message_type_deleter); } else { // Construct a new shared pointer from the message // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(allocator, *message); + auto shared_msg = std::allocate_shared(published_type_allocator, *message); - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( - std::move(message), sub_ids.take_ownership_subscriptions, allocator); + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + published_type_allocator, + ros_message_type_allocator, + ros_message_type_deleter); } } } @@ -250,10 +270,11 @@ class IntraProcessManager typename PublishedType, typename ROSMessageType, typename Alloc, + typename Deleter, typename ROSMessageTypeAllocatorTraits, typename ROSMessageTypeAllocator, typename ROSMessageTypeDeleter, - typename Deleter = std::default_delete + typename PublishedTypeAllocator > typename std::enable_if_t< @@ -263,16 +284,10 @@ class IntraProcessManager do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, std::unique_ptr message, - typename allocator::AllocRebind::allocator_type & allocator, + PublishedTypeAllocator & published_type_allocator, ROSMessageTypeAllocator & ros_message_type_allocator, ROSMessageTypeDeleter & ros_message_type_deleter) { - (void)ros_message_type_allocator; - (void)ros_message_type_deleter; - - using MessageAllocTraits = allocator::AllocRebind; - using MessageAllocatorT = typename MessageAllocTraits::allocator_type; - std::shared_lock lock(mutex_); auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); @@ -289,25 +304,34 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); } return shared_msg; } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(allocator, *message); + auto shared_msg = std::allocate_shared(published_type_allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, - sub_ids.take_shared_subscriptions); + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); } - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, - allocator); + published_type_allocator, + ros_message_type_allocator, + ros_message_type_deleter); return shared_msg; } @@ -319,10 +343,11 @@ class IntraProcessManager typename PublishedType, typename ROSMessageType, typename Alloc, + typename Deleter, typename ROSMessageTypeAllocatorTraits, typename ROSMessageTypeAllocator, typename ROSMessageTypeDeleter, - typename Deleter = std::default_delete + typename PublishedTypeAllocator > typename std::enable_if_t< @@ -332,7 +357,7 @@ class IntraProcessManager do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, std::unique_ptr message, - typename allocator::AllocRebind::allocator_type & allocator, + PublishedTypeAllocator & published_type_allocator, ROSMessageTypeAllocator & ros_message_type_allocator, ROSMessageTypeDeleter & ros_message_type_deleter) { @@ -341,9 +366,6 @@ class IntraProcessManager auto unique_ros_msg = std::unique_ptr(ptr, ros_message_type_deleter); rclcpp::TypeAdapter::convert_to_ros_message(*message, *unique_ros_msg); - using MessageAllocTraits = allocator::AllocRebind; - using MessageAllocatorT = typename MessageAllocTraits::allocator_type; - std::shared_lock lock(mutex_); auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); @@ -360,24 +382,33 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, sub_ids.take_shared_subscriptions); + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); } } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(allocator, *message); + auto shared_msg = std::allocate_shared(published_type_allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, - sub_ids.take_shared_subscriptions); + sub_ids.take_shared_subscriptions, + ros_message_type_allocator); } - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, - allocator); + published_type_allocator, + ros_message_type_allocator, + ros_message_type_deleter); } // TODO(clalancette): depending on how the publisher and subscriber are setup, we may end @@ -434,13 +465,18 @@ class IntraProcessManager template< typename MessageT, + typename PublishedType, + typename ROSMessageType, typename Alloc, - typename Deleter + typename Deleter, + typename ROSMessageTypeAllocatorTraits, + typename ROSMessageTypeAllocator > void add_shared_msg_to_buffers( - std::shared_ptr message, - std::vector subscription_ids) + std::shared_ptr message, + std::vector subscription_ids, + ROSMessageTypeAllocator & ros_message_type_allocator) { for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); @@ -450,27 +486,26 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer + rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { - - using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); + auto ros_msg = std::shared_ptr(ptr); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ros_msg); ros_message_subscription->provide_intra_process_message(ros_msg); } else { ros_message_subscription->provide_intra_process_message(message); @@ -490,22 +525,30 @@ class IntraProcessManager template< typename MessageT, + typename PublishedType, + typename ROSMessageType, typename Alloc, - typename Deleter + typename Deleter, + typename ROSMessageTypeAllocatorTraits, + typename ROSMessageTypeAllocator, + typename ROSMessageTypeDeleter, + typename PublishedTypeAllocator > void add_owned_msg_to_buffers( - std::unique_ptr message, + std::unique_ptr message, std::vector subscription_ids, - typename allocator::AllocRebind::allocator_type & allocator) + PublishedTypeAllocator & published_type_allocator, + ROSMessageTypeAllocator & ros_message_type_allocator, + ROSMessageTypeDeleter & ros_message_type_deleter) { std::cout << "add owned msg to buffers" << std::endl; std::cout << "message has type : " << typeid(message).name() << std::endl; - using MessageAllocTraits = allocator::AllocRebind; - using MessageUniquePtr = std::unique_ptr; + using MessageAllocTraits = allocator::AllocRebind; + using MessageUniquePtr = std::unique_ptr; for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); @@ -519,35 +562,31 @@ class IntraProcessManager std::cout << "Typed Subscription" << std::endl; auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { std::cout << "ROSMessage Subscription" << std::endl; auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer + rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { std::cout << "ROSMessage TypeAdapted Subscription" << std::endl; - using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; - auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1); - ROSMessageTypeAllocatorTraits::construct(allocator, ptr); - Deleter deleter = message.get_deleter(); - auto ros_msg = std::unique_ptr(ptr, deleter); - rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); + auto ros_msg = std::unique_ptr(ptr, ros_message_type_deleter); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ros_msg); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { if (std::next(it) == subscription_ids.end()) { @@ -558,8 +597,8 @@ class IntraProcessManager // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(allocator, 1); - MessageAllocTraits::construct(allocator, ptr, *message); + auto ptr = MessageAllocTraits::allocate(published_type_allocator, 1); + MessageAllocTraits::construct(published_type_allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); ros_message_subscription->provide_intra_process_message(std::move(copy_message)); @@ -576,8 +615,8 @@ class IntraProcessManager // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(allocator, 1); - MessageAllocTraits::construct(allocator, ptr, *message); + auto ptr = MessageAllocTraits::allocate(published_type_allocator, 1); + MessageAllocTraits::construct(published_type_allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); subscription->provide_intra_process_data(std::move(copy_message)); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 679aff1644..fcdb8c76da 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -42,24 +42,24 @@ namespace experimental { template< -/// MessageT::custom_type if MessageT is a TypeAdapter, -/// otherwise just MessageT. + typename MessageT, typename SubscribedType, typename SubscribedTypeAlloc = std::allocator, - typename SubscribedTypeDeleter = std::default_delete, - typename CallbackMessageT = SubscribedType + typename SubscribedTypeDeleter = std::default_delete > class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< + MessageT, SubscribedType, SubscribedTypeAlloc, SubscribedTypeDeleter > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - SubscribedType, - SubscribedTypeAlloc, - SubscribedTypeDeleter + MessageT, + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter >; public: @@ -72,13 +72,13 @@ class SubscriptionIntraProcess using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( - AnySubscriptionCallback callback, + AnySubscriptionCallback callback, std::shared_ptr allocator, rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( + : SubscriptionIntraProcessBuffer( allocator, context, topic_name, @@ -101,7 +101,7 @@ class SubscriptionIntraProcess virtual ~SubscriptionIntraProcess() = default; std::shared_ptr - take_data() + take_data() override { ConstMessageSharedPtr shared_msg; MessageUniquePtr unique_msg; @@ -118,7 +118,7 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) + void execute(std::shared_ptr & data) override { execute_impl(data); } @@ -157,7 +157,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ba575c763c..f0ee39ce47 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -52,21 +52,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC size_t - get_number_of_ready_guard_conditions() {return 1;} + get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); - - virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; - - virtual - std::shared_ptr - take_data() = 0; - - virtual void - execute(std::shared_ptr & data) = 0; + add_to_wait_set(rcl_wait_set_t * wait_set) override; virtual bool use_take_shared_method() const = 0; @@ -83,10 +73,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; -private: virtual void trigger_guard_condition() = 0; +private: std::string topic_name_; QoS qos_profile_; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 2874c321b8..98b2379698 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -44,23 +44,18 @@ namespace experimental template< typename MessageT, + typename SubscribedType, typename Alloc = std::allocator, - typename Deleter = std::default_delete, - /// MessageT::custom_type if MessageT is a TypeAdapter, - /// otherwise just MessageT. - typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + typename Deleter = std::default_delete, /// MessageT::ros_message_type if MessageT is a TypeAdapter, /// otherwise just MessageT. - typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type + typename ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type > -class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer +class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - using SubscribedType = SubscribedT; - using ROSMessageType = ROSMessageT; - using SubscribedTypeAllocatorTraits = allocator::AllocRebind; using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; using SubscribedTypeDeleter = allocator::Deleter; @@ -87,7 +82,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer(context, topic_name, qos_profile) + : ROSMessageIntraProcessBuffer(context, topic_name, qos_profile) { // Create the intra-process buffer. buffer_ = rclcpp::experimental::create_intra_process_buffer( @@ -97,7 +92,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferhas_data(); @@ -109,7 +104,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::is_specialized::value) { + if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); } else { @@ -124,13 +119,13 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::is_specialized::value) { + if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); } else { // auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); // SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); - // rclcpp::TypeAdapter::convert_to_custom(*message, *ptr); + // rclcpp::TypeAdapter::convert_to_custom(*message, *ptr); // buffer_->add_unique(std::unique_ptr(ptr, subscribed_type_deleter_)); // trigger_guard_condition(); } @@ -154,14 +149,14 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferuse_take_shared_method(); } protected: void - trigger_guard_condition() + trigger_guard_condition() override { this->gc_.trigger(); } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 21e56a9ae5..9da5e84aa6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -510,10 +510,12 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, PublishedTypeAllocator>( intra_process_publisher_id_, std::move(msg), - published_type_allocator_); + published_type_allocator_, + ros_message_type_allocator_, + ros_message_type_deleter_); } @@ -532,7 +534,7 @@ class Publisher : public PublisherBase } return ipm->template do_intra_process_publish_and_return_shared( + ROSMessageType, AllocatorT, std::default_delete, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, PublishedTypeAllocator>( intra_process_publisher_id_, std::move(msg), published_type_allocator_, diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 3e6ca1599d..cf8732bf3d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -406,10 +406,10 @@ class Subscription : public SubscriptionBase SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + MessageT, SubscribedType, AllocatorT, - SubscribedTypeDeleter, - MessageT>; + SubscribedTypeDeleter>; std::shared_ptr subscription_intra_process_; }; From 7925aee6dd7d7435656ea4ee355f8130263ff882 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Mon, 20 Dec 2021 15:28:42 -0500 Subject: [PATCH 09/20] Add required conversions to any subscription callback (#8) * Add required conversions to any subscription callback * Style fixes for the linters. (#9) Signed-off-by: Chris Lalancette Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 137 ++++++++++++------ .../experimental/intra_process_manager.hpp | 74 +++++----- .../ros_message_intra_process_buffer.hpp | 1 - .../subscription_intra_process.hpp | 18 ++- .../subscription_intra_process_buffer.hpp | 17 ++- rclcpp/include/rclcpp/publisher.hpp | 8 +- rclcpp/include/rclcpp/subscription.hpp | 1 - ...ption_publisher_with_same_type_adapter.cpp | 3 - 8 files changed, 155 insertions(+), 104 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 061ded9c4b..191abd2228 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -16,7 +16,6 @@ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #include -#include //TODO remove when removed std::cout s #include #include #include @@ -477,6 +476,21 @@ class AnySubscriptionCallback } } + std::unique_ptr + convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } else { + throw std::runtime_error( + "convert_custom_type_to_ros_message_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + // Dispatch when input is a ros message and the output could be anything. void dispatch( @@ -687,21 +701,21 @@ class AnySubscriptionCallback std::is_same_v )) { - // callback(message); //TODO Convert + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - // callback(message, message_info); //TODO Convert + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - // callback(message); //TODO Convert + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| @@ -711,30 +725,56 @@ class AnySubscriptionCallback callback(message, message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - //callback(*message); //TODO convert to rosMessage - } else if constexpr (std::is_same_v) { - //callback(*message, message_info); //TODO convert to rosMessage + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(message); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(message); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(message, message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(message, message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] @@ -761,14 +801,11 @@ class AnySubscriptionCallback TRACEPOINT(callback_end, static_cast(this)); } - void + void dispatch_intra_process( std::unique_ptr message, const rclcpp::MessageInfo & message_info) { - - std::cout << "ASC dispatch_intra_process" << std::endl; - TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { @@ -783,80 +820,90 @@ class AnySubscriptionCallback using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; - std::cout << "IS TA: " << is_ta << std::endl; - // conditions for custom type if constexpr (is_ta && std::is_same_v) { - //We won't need to convert - //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - std::cout << "ASC crcb" << std::endl; callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - //auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - std::cout << "ASC crcb+info" << std::endl; callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| - std::is_same_v - )) + std::is_same_v)) { - std::cout << "ASC unorshcb" << std::endl; - //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); - callback(std::move(message)); //TODO convert + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - std::cout << "ASC unorshcb+info" << std::endl; - //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); - callback(std::move(message), message_info); //TODO convert + callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - std::cout << "ASC shconsrref" << std::endl; - //callback(convert_ros_message_to_custom_type_unique_ptr(*message)); - // callback(message); //TODO Convert + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - std::cout << "ASC shconsrref+info" << std::endl; - //callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); - //callback(message, message_info); //TODO Convert + callback(std::move(message), message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - //callback(*message); - } else if constexpr (std::is_same_v) { - // callback(*message, message_info); + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - // callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 0ed7e11924..a566a51504 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -214,7 +214,7 @@ class IntraProcessManager std::shared_ptr msg = std::move(message); this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); @@ -235,8 +235,8 @@ class IntraProcessManager sub_ids.take_ownership_subscriptions.end()); this->template add_owned_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, + PublishedTypeAllocator>( std::move(message), concatenated_vector, published_type_allocator, @@ -245,16 +245,18 @@ class IntraProcessManager } else { // Construct a new shared pointer from the message // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(published_type_allocator, *message); + auto shared_msg = std::allocate_shared( + published_type_allocator, + *message); this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( shared_msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, published_type_allocator, @@ -305,7 +307,7 @@ class IntraProcessManager std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( shared_msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); @@ -314,19 +316,21 @@ class IntraProcessManager } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(published_type_allocator, *message); + auto shared_msg = std::allocate_shared( + published_type_allocator, + *message); if (!sub_ids.take_shared_subscriptions.empty()) { this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( shared_msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); } this->template add_owned_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, + PublishedTypeAllocator>( std::move(message), sub_ids.take_ownership_subscriptions, published_type_allocator, @@ -363,7 +367,9 @@ class IntraProcessManager { auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); - auto unique_ros_msg = std::unique_ptr(ptr, ros_message_type_deleter); + auto unique_ros_msg = std::unique_ptr( + ptr, + ros_message_type_deleter); rclcpp::TypeAdapter::convert_to_ros_message(*message, *unique_ros_msg); std::shared_lock lock(mutex_); @@ -383,7 +389,7 @@ class IntraProcessManager std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( shared_msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); @@ -391,19 +397,21 @@ class IntraProcessManager } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(published_type_allocator, *message); + auto shared_msg = std::allocate_shared( + published_type_allocator, + *message); if (!sub_ids.take_shared_subscriptions.empty()) { this->template add_shared_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator>( shared_msg, sub_ids.take_shared_subscriptions, ros_message_type_allocator); } this->template add_owned_msg_to_buffers( + Deleter, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, + PublishedTypeAllocator>( std::move(message), sub_ids.take_ownership_subscriptions, published_type_allocator, @@ -486,10 +494,9 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); + rclcpp::experimental::SubscriptionIntraProcessBuffer>(subscription_base); if (nullptr == subscription) { - auto ros_message_subscription = std::dynamic_pointer_cast< rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); @@ -497,7 +504,7 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { @@ -510,13 +517,10 @@ class IntraProcessManager } else { ros_message_subscription->provide_intra_process_message(message); } - } } else { subscription->provide_intra_process_data(message); } - - } else { subscriptions_.erase(id); } @@ -542,7 +546,6 @@ class IntraProcessManager ROSMessageTypeAllocator & ros_message_type_allocator, ROSMessageTypeDeleter & ros_message_type_deleter) { - std::cout << "add owned msg to buffers" << std::endl; std::cout << "message has type : " << typeid(message).name() << std::endl; @@ -558,15 +561,13 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); std::cout << "Subscription Base?" << std::endl; if (subscription_base) { - std::cout << "Typed Subscription" << std::endl; auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); + rclcpp::experimental::SubscriptionIntraProcessBuffer>(subscription_base); if (nullptr == subscription) { - - std::cout << "ROSMessage Subscription" << std::endl; + std::cout << "ROSMessage Subscription" << std::endl; auto ros_message_subscription = std::dynamic_pointer_cast< rclcpp::experimental::ROSMessageIntraProcessBuffer @@ -575,17 +576,18 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { - std::cout << "ROSMessage TypeAdapted Subscription" << std::endl; if constexpr (rclcpp::TypeAdapter::is_specialized::value) { auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); - auto ros_msg = std::unique_ptr(ptr, ros_message_type_deleter); + auto ros_msg = std::unique_ptr( + ptr, + ros_message_type_deleter); rclcpp::TypeAdapter::convert_to_ros_message(*message, *ros_msg); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { @@ -604,7 +606,6 @@ class IntraProcessManager ros_message_subscription->provide_intra_process_message(std::move(copy_message)); } } - } } else { std::cout << "Typed Subscription" << std::endl; @@ -622,7 +623,6 @@ class IntraProcessManager subscription->provide_intra_process_data(std::move(copy_message)); } } - } else { std::cout << "Erasing subscription" << std::endl; subscriptions_.erase(subscription_it); diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp index 30f9d08efd..3a3b1ed81b 100644 --- a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -61,7 +61,6 @@ class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase virtual void provide_intra_process_message(MessageUniquePtr message) = 0; - }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index fcdb8c76da..dd30c1972f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -46,14 +46,14 @@ template< typename SubscribedType, typename SubscribedTypeAlloc = std::allocator, typename SubscribedTypeDeleter = std::default_delete - > +> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, - SubscribedType, - SubscribedTypeAlloc, - SubscribedTypeDeleter - > + MessageT, + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter + > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< MessageT, @@ -65,7 +65,8 @@ class SubscriptionIntraProcess public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; + using MessageAllocTraits = + typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; @@ -78,7 +79,8 @@ class SubscriptionIntraProcess const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( + : SubscriptionIntraProcessBuffer( allocator, context, topic_name, diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 98b2379698..413b6f72d5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -51,7 +51,8 @@ template< /// otherwise just MessageT. typename ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type > -class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer +class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) @@ -82,10 +83,12 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer(context, topic_name, qos_profile) + : ROSMessageIntraProcessBuffer(context, topic_name, + qos_profile) { // Create the intra-process buffer. - buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, allocator); @@ -101,7 +104,6 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::is_specialized::value) { @@ -110,7 +112,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferadd_shared(std::unique_ptr(ptr, subscribed_type_deleter_)); + // buffer_->add_shared(std::unique_ptr( + // ptr, subscribed_type_deleter_)); // trigger_guard_condition(); } } @@ -126,10 +129,10 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::convert_to_custom(*message, *ptr); - // buffer_->add_unique(std::unique_ptr(ptr, subscribed_type_deleter_)); + // buffer_->add_unique(std::unique_ptr( + // ptr, subscribed_type_deleter_)); // trigger_guard_condition(); } - } void diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 9da5e84aa6..9ceb200596 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -510,7 +510,9 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, PublishedTypeAllocator>( + ipm->template do_intra_process_publish, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, + ROSMessageTypeDeleter, PublishedTypeAllocator>( intra_process_publisher_id_, std::move(msg), published_type_allocator_, @@ -534,7 +536,9 @@ class Publisher : public PublisherBase } return ipm->template do_intra_process_publish_and_return_shared, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, PublishedTypeAllocator>( + ROSMessageType, AllocatorT, std::default_delete, + ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, + PublishedTypeAllocator>( intra_process_publisher_id_, std::move(msg), published_type_allocator_, diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index cf8732bf3d..ad23d60882 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -411,7 +411,6 @@ class Subscription : public SubscriptionBase AllocatorT, SubscribedTypeDeleter>; std::shared_ptr subscription_intra_process_; - }; } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp index 9ae90e61d6..036fa11359 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -143,15 +143,12 @@ TEST_F( const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; - auto node = rclcpp::Node::make_shared( "test_intra_process", rclcpp::NodeOptions().use_intra_process_comms(true)); auto pub = node->create_publisher(topic_name, 1); - - { { // std::unique_ptr bool is_received = false; From a5c550459cab267e7e68a3aadd88fd6eb87b1777 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Thu, 9 Dec 2021 21:32:04 -0300 Subject: [PATCH 10/20] Fixed bugs, fixed linters Fixed image tools issues Signed-off-by: Gonzalo de Pedro Fix linting. (#12) Signed-off-by: Chris Lalancette Fix the test for any_subscription_callback. (#13) In particular, change it so that the tests for TypeAdaptation with any_subscription_callback always pass the custom type to dispatch_intra_process(). That means that when using TypeAdaptation, it is not possible to call dispatch_intra_process() with the ROS message type. But since this is a low-level interface, this should be fine; the public-facing APIs handle this case for us. This also requires us to remove the test for inter-process publishing with type adaptation. That's because we didn't change the signature of AnySubscriptionCallback::dispath() to take in the custom type, so it always expects the ROS message type. Signed-off-by: Chris Lalancette Reenable the test_intra_process* tests. (#14) Signed-off-by: Chris Lalancette Remove debugging print statements. (#11) Signed-off-by: Chris Lalancette Signed-off-by: Gonzalo de Pedro --- .../experimental/intra_process_manager.hpp | 341 +++++------------- .../ros_message_intra_process_buffer.hpp | 13 +- .../subscription_intra_process.hpp | 8 +- .../subscription_intra_process_buffer.hpp | 54 +-- rclcpp/include/rclcpp/publisher.hpp | 124 ++++--- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/test/rclcpp/CMakeLists.txt | 50 +-- .../rclcpp/test_any_subscription_callback.cpp | 114 +++--- .../rclcpp/test_intra_process_manager.cpp | 12 + 9 files changed, 305 insertions(+), 413 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index a566a51504..c663989044 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -32,14 +32,15 @@ #include #include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -178,24 +179,16 @@ class IntraProcessManager */ template< typename MessageT, - typename PublishedType, - typename ROSMessageType, - typename Alloc, - typename Deleter, - typename ROSMessageTypeAllocatorTraits, - typename ROSMessageTypeAllocator, - typename ROSMessageTypeDeleter, - typename PublishedTypeAllocator - > + typename Alloc = std::allocator, + typename Deleter = std::default_delete> void do_intra_process_publish( uint64_t intra_process_publisher_id, - std::unique_ptr message, - PublishedTypeAllocator & published_type_allocator, - ROSMessageTypeAllocator & ros_message_type_allocator, - ROSMessageTypeDeleter & ros_message_type_deleter) + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator) { - std::cout << "do_intra_process_publish --- " << std::endl; + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; std::shared_lock lock(mutex_); @@ -211,166 +204,50 @@ class IntraProcessManager if (sub_ids.take_ownership_subscriptions.empty()) { // None of the buffers require ownership, so we promote the pointer - std::shared_ptr msg = std::move(message); - - this->template add_shared_msg_to_buffers( - msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); - } else { - if (sub_ids.take_shared_subscriptions.size() <= 1) { - // There is at maximum 1 buffer that does not require ownership. - // So this case is equivalent to all the buffers requiring ownership - - std::cout << " to all the buffers requiring ownership --- " << std::endl; - - std::cout << "message has type : " << typeid(message).name() << std::endl; - - // Merge the two vector of ids into a unique one - std::vector concatenated_vector(sub_ids.take_shared_subscriptions); - concatenated_vector.insert( - concatenated_vector.end(), - sub_ids.take_ownership_subscriptions.begin(), - sub_ids.take_ownership_subscriptions.end()); - - this->template add_owned_msg_to_buffers( - std::move(message), - concatenated_vector, - published_type_allocator, - ros_message_type_allocator, - ros_message_type_deleter); - } else { - // Construct a new shared pointer from the message - // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared( - published_type_allocator, - *message); - - this->template add_shared_msg_to_buffers( - shared_msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); - this->template add_owned_msg_to_buffers( - std::move(message), - sub_ids.take_ownership_subscriptions, - published_type_allocator, - ros_message_type_allocator, - ros_message_type_deleter); - } - } - } - - template< - typename MessageT, - typename T, - typename PublishedType, - typename ROSMessageType, - typename Alloc, - typename Deleter, - typename ROSMessageTypeAllocatorTraits, - typename ROSMessageTypeAllocator, - typename ROSMessageTypeDeleter, - typename PublishedTypeAllocator - > - typename - std::enable_if_t< - rosidl_generator_traits::is_message::value && - std::is_same::value, std::shared_ptr - > - do_intra_process_publish_and_return_shared( - uint64_t intra_process_publisher_id, - std::unique_ptr message, - PublishedTypeAllocator & published_type_allocator, - ROSMessageTypeAllocator & ros_message_type_allocator, - ROSMessageTypeDeleter & ros_message_type_deleter) - { - std::shared_lock lock(mutex_); - - auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); - if (publisher_it == pub_to_subs_.end()) { - // Publisher is either invalid or no longer exists. - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "Calling do_intra_process_publish for invalid or no longer existing publisher id"); - return nullptr; - } - const auto & sub_ids = publisher_it->second; - - if (sub_ids.take_ownership_subscriptions.empty()) { - // If there are no owning, just convert to shared. - std::shared_ptr shared_msg = std::move(message); - if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); - } - return shared_msg; - } else { - // Construct a new shared pointer from the message for the buffers that - // do not require ownership and to return. - auto shared_msg = std::allocate_shared( - published_type_allocator, - *message); - - if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); - } - - this->template add_owned_msg_to_buffers( + std::shared_ptr msg = std::move(message); + + this->template add_shared_msg_to_buffers( + msg, sub_ids.take_shared_subscriptions); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() <= 1) + { + // Merge the two vector of ids into a unique one + std::vector concatenated_vector(sub_ids.take_shared_subscriptions); + concatenated_vector.insert( + concatenated_vector.end(), + sub_ids.take_ownership_subscriptions.begin(), + sub_ids.take_ownership_subscriptions.end()); + + this->template add_owned_msg_to_buffers( std::move(message), - sub_ids.take_ownership_subscriptions, - published_type_allocator, - ros_message_type_allocator, - ros_message_type_deleter); - - return shared_msg; + concatenated_vector, + allocator); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() > 1) + { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = std::allocate_shared(allocator, *message); + + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } template< typename MessageT, - typename T, - typename PublishedType, - typename ROSMessageType, - typename Alloc, - typename Deleter, - typename ROSMessageTypeAllocatorTraits, - typename ROSMessageTypeAllocator, - typename ROSMessageTypeDeleter, - typename PublishedTypeAllocator - > - typename - std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value, std::shared_ptr - > + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + std::shared_ptr do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, - std::unique_ptr message, - PublishedTypeAllocator & published_type_allocator, - ROSMessageTypeAllocator & ros_message_type_allocator, - ROSMessageTypeDeleter & ros_message_type_deleter) + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator) { - auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); - ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); - auto unique_ros_msg = std::unique_ptr( - ptr, - ros_message_type_deleter); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *unique_ros_msg); + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; std::shared_lock lock(mutex_); @@ -386,44 +263,30 @@ class IntraProcessManager if (sub_ids.take_ownership_subscriptions.empty()) { // If there are no owning, just convert to shared. - std::shared_ptr shared_msg = std::move(message); + std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( - shared_msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); } + return shared_msg; } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared( - published_type_allocator, - *message); + auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, - sub_ids.take_shared_subscriptions, - ros_message_type_allocator); + sub_ids.take_shared_subscriptions); } - - this->template add_owned_msg_to_buffers( - std::move(message), - sub_ids.take_ownership_subscriptions, - published_type_allocator, - ros_message_type_allocator, - ros_message_type_deleter); + if (!sub_ids.take_ownership_subscriptions.empty()) { + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + return shared_msg; } - - // TODO(clalancette): depending on how the publisher and subscriber are setup, we may end - // up doing a conversion more than once; in this function and down in - // add_{shared,owned}_msg_to_buffers(). We should probably push this down further to avoid - // that double conversion. - return unique_ros_msg; } /// Return true if the given rmw_gid_t matches any stored Publishers. @@ -473,19 +336,16 @@ class IntraProcessManager template< typename MessageT, - typename PublishedType, - typename ROSMessageType, typename Alloc, - typename Deleter, - typename ROSMessageTypeAllocatorTraits, - typename ROSMessageTypeAllocator - > + typename Deleter> void add_shared_msg_to_buffers( - std::shared_ptr message, - std::vector subscription_ids, - ROSMessageTypeAllocator & ros_message_type_allocator) + std::shared_ptr message, + std::vector subscription_ids) { + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { @@ -494,8 +354,8 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer>(subscription_base); + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); if (nullptr == subscription) { auto ros_message_subscription = std::dynamic_pointer_cast< rclcpp::experimental::ROSMessageIntraProcessBuffer @@ -504,15 +364,13 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); - ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); - auto ros_msg = std::shared_ptr(ptr); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *ros_msg); + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); ros_message_subscription->provide_intra_process_message(ros_msg); } else { ros_message_subscription->provide_intra_process_message(message); @@ -529,29 +387,18 @@ class IntraProcessManager template< typename MessageT, - typename PublishedType, - typename ROSMessageType, - typename Alloc, - typename Deleter, - typename ROSMessageTypeAllocatorTraits, - typename ROSMessageTypeAllocator, - typename ROSMessageTypeDeleter, - typename PublishedTypeAllocator - > + typename Alloc = std::allocator, + typename Deleter = std::default_delete> void add_owned_msg_to_buffers( - std::unique_ptr message, + std::unique_ptr message, std::vector subscription_ids, - PublishedTypeAllocator & published_type_allocator, - ROSMessageTypeAllocator & ros_message_type_allocator, - ROSMessageTypeDeleter & ros_message_type_deleter) + typename allocator::AllocRebind::allocator_type & allocator) { - std::cout << "add owned msg to buffers" << std::endl; - - std::cout << "message has type : " << typeid(message).name() << std::endl; - - using MessageAllocTraits = allocator::AllocRebind; - using MessageUniquePtr = std::unique_ptr; + using MessageAllocTraits = allocator::AllocRebind; + using MessageUniquePtr = std::unique_ptr; + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); @@ -559,16 +406,11 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - std::cout << "Subscription Base?" << std::endl; if (subscription_base) { - std::cout << "Typed Subscription" << std::endl; - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer>(subscription_base); + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); if (nullptr == subscription) { - std::cout << "ROSMessage Subscription" << std::endl; - auto ros_message_subscription = std::dynamic_pointer_cast< rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); @@ -576,31 +418,28 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { - std::cout << "ROSMessage TypeAdapted Subscription" << std::endl; - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator, 1); - ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator, ptr); - auto ros_msg = std::unique_ptr( - ptr, - ros_message_type_deleter); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *ros_msg); + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1); + ROSMessageTypeAllocatorTraits::construct(allocator, ptr); + Deleter deleter = message.get_deleter(); + auto ros_msg = std::unique_ptr(ptr, deleter); + rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership - ros_message_subscription->provide_intra_process_message(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(published_type_allocator, 1); - MessageAllocTraits::construct(published_type_allocator, ptr, *message); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); ros_message_subscription->provide_intra_process_message(std::move(copy_message)); @@ -608,7 +447,6 @@ class IntraProcessManager } } } else { - std::cout << "Typed Subscription" << std::endl; if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership subscription->provide_intra_process_data(std::move(message)); @@ -616,15 +454,14 @@ class IntraProcessManager // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(published_type_allocator, 1); - MessageAllocTraits::construct(published_type_allocator, ptr, *message); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); subscription->provide_intra_process_data(std::move(copy_message)); } } } else { - std::cout << "Erasing subscription" << std::endl; subscriptions_.erase(subscription_it); } } diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp index 3a3b1ed81b..82088fb39a 100644 --- a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -15,16 +15,13 @@ #ifndef RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ -#include -#include #include -#include #include -#include #include "rcl/error_handling.h" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "tracetools/tracetools.h" @@ -36,15 +33,19 @@ namespace experimental template< typename RosMessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete + typename Deleter = std::default_delete > class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase { public: RCLCPP_SMART_PTR_DEFINITIONS(ROSMessageIntraProcessBuffer) + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + using MessageUniquePtr = std::unique_ptr; ROSMessageIntraProcessBuffer( rclcpp::Context::SharedPtr context, diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index dd30c1972f..117737957b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -49,14 +49,12 @@ template< > class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, SubscribedType, SubscribedTypeAlloc, SubscribedTypeDeleter > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - MessageT, SubscribedType, SubscribedTypeAlloc, SubscribedTypeDeleter @@ -68,8 +66,8 @@ class SubscriptionIntraProcess using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; - using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; - using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; + using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr; + using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr; using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( @@ -79,7 +77,7 @@ class SubscriptionIntraProcess const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( allocator, context, diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 413b6f72d5..5b8aa39581 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -18,7 +18,6 @@ #include #include -#include //TODO remove #include #include #include @@ -43,7 +42,6 @@ namespace experimental { template< - typename MessageT, typename SubscribedType, typename Alloc = std::allocator, typename Deleter = std::default_delete, @@ -69,7 +67,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer; using ConstDataSharedPtr = std::shared_ptr; - using DataUniquePtr = std::unique_ptr; + using SubscribedTypeUniquePtr = std::unique_ptr; using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< SubscribedType, @@ -83,9 +81,12 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer(context, topic_name, - qos_profile) + : ROSMessageIntraProcessBuffer(context, topic_name, + qos_profile), + subscribed_type_allocator_(*allocator) { + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + // Create the intra-process buffer. buffer_ = rclcpp::experimental::create_intra_process_buffer( @@ -101,52 +102,55 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBufferhas_data(); } + SubscribedTypeUniquePtr + convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (!std::is_same::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_subscribed_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + void provide_intra_process_message(ConstMessageSharedPtr message) { - std::cout << "--------------Provide Intra Process Message (ConstMessageSharedPtr)" << std::endl; - - if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { + if constexpr (std::is_same::value) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); } else { - // auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); - // SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); - // buffer_->add_shared(std::unique_ptr( - // ptr, subscribed_type_deleter_)); - // trigger_guard_condition(); + buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); } } void provide_intra_process_message(MessageUniquePtr message) { - std::cout << "--------------Provide Intra Process Message (MessageUniquePtr)" << std::endl; - if constexpr (!rclcpp::TypeAdapter::is_specialized::value) { + if constexpr (std::is_same::value) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); } else { - // auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); - // SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); - // rclcpp::TypeAdapter::convert_to_custom(*message, *ptr); - // buffer_->add_unique(std::unique_ptr( - // ptr, subscribed_type_deleter_)); - // trigger_guard_condition(); + buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); } } void provide_intra_process_data(ConstDataSharedPtr message) { - std::cout << "--------------Provide Intra Process DATA (ConstDataSharedPtr)" << std::endl; buffer_->add_shared(std::move(message)); trigger_guard_condition(); } void - provide_intra_process_data(DataUniquePtr message) + provide_intra_process_data(SubscribedTypeUniquePtr message) { - std::cout << "--------------Provide Intra Process DATA (DataUniquePtr)" << std::endl; buffer_->add_unique(std::move(message)); trigger_guard_condition(); } @@ -165,6 +169,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); this->do_inter_process_publish(*shared_msg); } else { - this->do_intra_process_publish(std::move(msg)); + this->do_intra_process_ros_message_publish(std::move(msg)); } } @@ -319,29 +320,11 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - if (!intra_process_is_enabled_) { - // If we aren't using intraprocess at all, do the type conversion - // immediately and publish interprocess. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - this->do_inter_process_publish(*unique_ros_msg); - return; - } - // If an interprocess subscription exists, then the unique_ptr is promoted - // to a shared_ptr and published. - // This allows doing the intraprocess publish first and then doing the - // interprocess publish, resulting in lower publish-to-subscribe latency. - // It's not possible to do that with an unique_ptr, - // as do_intra_process_publish takes the ownership of the message. - bool inter_process_publish_needed = - get_subscription_count() > get_intra_process_subscription_count(); - - if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); - this->do_inter_process_publish(*shared_msg); - } else { - this->do_intra_process_publish(std::move(msg)); - } + // TODO(wjwwood): later update this to give the unique_ptr to the intra + // process manager and let it decide if it needs to be converted or not. + // For now, convert it unconditionally and pass it the ROSMessageType + // publish function specialization. + this->do_intra_process_publish(std::move(msg)); } /// Publish a message on the topic. @@ -362,18 +345,26 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // Avoid double allocation when not using intra process. + // TODO(wjwwood): later update this to give the unique_ptr to the intra + // process manager and let it decide if it needs to be converted or not. + // For now, convert it unconditionally and pass it the ROSMessageType + // publish function specialization. + + // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. ROSMessageType ros_msg; rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); // In this case we're not using intra process. - this->do_inter_process_publish(ros_msg); - return; + return this->do_inter_process_publish(ros_msg); } - // Otherwise we have to allocate memory in a unique_ptr and pass it along. - auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); - this->publish(std::move(unique_msg)); + // Otherwise we have to allocate memory in a unique_ptr, convert it, + // and pass it along. + // As the message is not const, a copy should be made. + // A shared_ptr could also be constructed here. + auto unique_ros_msg = this->create_ros_message_unique_ptr(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); + this->publish(std::move(unique_ros_msg)); } void @@ -510,19 +501,32 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish, ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, - ROSMessageTypeDeleter, PublishedTypeAllocator>( + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); + } + + void + do_intra_process_ros_message_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), - published_type_allocator_, - ros_message_type_allocator_, - ros_message_type_deleter_); + ros_message_type_allocator_); } - template - std::shared_ptr + std::shared_ptr do_intra_process_publish_and_return_shared( std::unique_ptr msg) { @@ -535,17 +539,34 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_shared, - ROSMessageTypeAllocatorTraits, ROSMessageTypeAllocator, ROSMessageTypeDeleter, - PublishedTypeAllocator>( + return ipm->template do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), - published_type_allocator_, - ros_message_type_allocator_, - ros_message_type_deleter_); + published_type_allocator_); } + std::shared_ptr + do_intra_process_ros_message_publish_and_return_shared( + std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + return ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + ros_message_type_allocator_); + } + + /// Return a new unique_ptr using the ROSMessageType of the publisher. std::unique_ptr create_ros_message_unique_ptr() @@ -555,7 +576,7 @@ class Publisher : public PublisherBase return std::unique_ptr(ptr, ros_message_type_deleter_); } - /// Duplicate a given ROS message as a unique_ptr. + /// Duplicate a given ros message as a unique_ptr. std::unique_ptr duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg) { @@ -564,15 +585,6 @@ class Publisher : public PublisherBase return std::unique_ptr(ptr, ros_message_type_deleter_); } - /// Duplicate a given type adapted message as a unique_ptr. - std::unique_ptr - duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) - { - auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); - PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); - return std::unique_ptr(ptr, published_type_deleter_); - } - /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index ad23d60882..c238c9581f 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -276,7 +276,7 @@ class Subscription : public SubscriptionBase /// Take the next message from the inter-process subscription. /** - * This verison takes a SubscribedType which is different frmo the + * This version takes a SubscribedType which is different from the * ROSMessageType when a rclcpp::TypeAdapter is in used. * * \sa take(ROSMessageType &, rclcpp::MessageInfo &) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 29d7b92901..e60b1636b8 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -63,13 +63,13 @@ if(TARGET test_any_service_callback) ) target_link_libraries(test_any_service_callback ${PROJECT_NAME}) endif() -# ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) -# if(TARGET test_any_subscription_callback) -# ament_target_dependencies(test_any_subscription_callback -# "test_msgs" -# ) -# target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) -# endif() +ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) +if(TARGET test_any_subscription_callback) + ament_target_dependencies(test_any_subscription_callback + "test_msgs" + ) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) +endif() ament_add_gtest(test_client test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -135,24 +135,24 @@ ament_add_gtest( if(TARGET test_future_return_code) target_link_libraries(test_future_return_code ${PROJECT_NAME}) endif() -# ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) -# if(TARGET test_intra_process_manager) -# ament_target_dependencies(test_intra_process_manager -# "rcl" -# "rcl_interfaces" -# "rmw" -# "rosidl_runtime_cpp" -# "rosidl_typesupport_cpp" -# ) -# target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) -# endif() -# ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) -# if(TARGET test_intra_process_manager_with_allocators) -# ament_target_dependencies(test_intra_process_manager_with_allocators -# "test_msgs" -# ) -# target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) -# endif() +ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) +if(TARGET test_intra_process_manager) + ament_target_dependencies(test_intra_process_manager + "rcl" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) +endif() +ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) +if(TARGET test_intra_process_manager_with_allocators) + ament_target_dependencies(test_intra_process_manager_with_allocators + "test_msgs" + ) + target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) +endif() ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) if(TARGET test_ring_buffer_implementation) ament_target_dependencies(test_ring_buffer_implementation diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 32abdf2934..02daee010b 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -25,6 +25,29 @@ #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" +// Type adapter to be used in tests. +struct MyEmpty {}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; + +using MyTA = rclcpp::TypeAdapter; + class TestAnySubscriptionCallback : public ::testing::Test { public: @@ -43,6 +66,24 @@ class TestAnySubscriptionCallback : public ::testing::Test rclcpp::MessageInfo message_info_; }; +class TestAnySubscriptionCallbackTA : public ::testing::Test +{ +public: + TestAnySubscriptionCallbackTA() {} + + static + std::unique_ptr + get_unique_ptr_msg() + { + return std::make_unique(); + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; + rclcpp::MessageInfo message_info_; +}; + void construct_with_null_allocator() { // suppress deprecated function warning @@ -97,29 +138,6 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { // Parameterized test to test across all callback types and dispatch types. // -// Type adapter to be used in tests. -struct MyEmpty {}; - -template<> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = MyEmpty; - using ros_message_type = test_msgs::msg::Empty; - - static - void - convert_to_ros_message(const custom_type &, ros_message_type &) - {} - - static - void - convert_to_custom(const ros_message_type &, custom_type &) - {} -}; - -using MyTA = rclcpp::TypeAdapter; - template class InstanceContextImpl { @@ -177,7 +195,7 @@ class DispatchTests {}; class DispatchTestsWithTA - : public TestAnySubscriptionCallback, + : public TestAnySubscriptionCallbackTA, public ::testing::WithParamInterface> {}; @@ -193,27 +211,35 @@ format_parameter_with_ta(const ::testing::TestParamInfo as input */ \ - TEST_P(DispatchTests_name, test_inter_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with shared_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with unique_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); \ - } +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_inter_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTests, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} -PARAMETERIZED_TESTS(DispatchTests) -PARAMETERIZED_TESTS(DispatchTestsWithTA) +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} // Generic classes for testing callbacks using std::bind to class methods. template diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 59437aa560..45b781995d 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -246,6 +246,18 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase buffer->add(std::move(msg)); } + void + provide_intra_process_data(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_data(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + std::uintptr_t pop() { From 89c70d14933fd88e6920e39c82cd88c8f79cdb24 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Tue, 14 Dec 2021 17:22:33 -0300 Subject: [PATCH 11/20] Tests fix (#15) * Tests fix Signed-off-by: Gonzalo de Pedro * Tests fix Signed-off-by: Gonzalo de Pedro * Fixed some tests Signed-off-by: Gonzalo de Pedro * Fix test_intra_process_manager test. Signed-off-by: Chris Lalancette * Style fixups. Signed-off-by: Chris Lalancette * One more small fix. Signed-off-by: Chris Lalancette * Fixed issues with allocators Signed-off-by: Gonzalo de Pedro * Small fix for lint_cmake. Signed-off-by: Chris Lalancette Co-authored-by: Chris Lalancette Add additional tests for the TypeAdapt conversions (#16) * Implement skipping ROS conversion when using the const ref publish. Signed-off-by: Chris Lalancette * Add tests to ensure that type adaptation doesn't unnecessarily convert. Signed-off-by: Chris Lalancette fixed allocator allocate usage on intra process manager (#17) Signed-off-by: Gonzalo de Pedro Removed TestPublisher.conversion_exception_is_passed_up test Signed-off-by: Gonzalo de Pedro --- .../experimental/intra_process_manager.hpp | 118 +++-- .../subscription_intra_process.hpp | 22 +- .../subscription_intra_process_buffer.hpp | 16 +- rclcpp/include/rclcpp/publisher.hpp | 61 ++- rclcpp/include/rclcpp/subscription.hpp | 6 +- rclcpp/test/rclcpp/CMakeLists.txt | 70 +-- .../rclcpp/test_intra_process_manager.cpp | 8 +- .../test_publisher_with_type_adapter.cpp | 22 +- ...ption_publisher_with_same_type_adapter.cpp | 497 +++++++++++++++--- 9 files changed, 607 insertions(+), 213 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index c663989044..cb3390338f 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -179,8 +179,10 @@ class IntraProcessManager */ template< typename MessageT, + typename ROSMessageType, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete + > void do_intra_process_publish( uint64_t intra_process_publisher_id, @@ -206,7 +208,7 @@ class IntraProcessManager // None of the buffers require ownership, so we promote the pointer std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( msg, sub_ids.take_shared_subscriptions); } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) @@ -217,8 +219,7 @@ class IntraProcessManager concatenated_vector.end(), sub_ids.take_ownership_subscriptions.begin(), sub_ids.take_ownership_subscriptions.end()); - - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, allocator); @@ -229,17 +230,19 @@ class IntraProcessManager // for the buffers that do not require ownership auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } template< typename MessageT, + typename ROSMessageType, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete + > std::shared_ptr do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, @@ -265,7 +268,7 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; @@ -275,12 +278,12 @@ class IntraProcessManager auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } if (!sub_ids.take_ownership_subscriptions.empty()) { - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); @@ -337,14 +340,22 @@ class IntraProcessManager template< typename MessageT, typename Alloc, - typename Deleter> + typename Deleter, + typename ROSMessageType> void add_shared_msg_to_buffers( std::shared_ptr message, std::vector subscription_ids) { using PublishedType = typename rclcpp::TypeAdapter::custom_type; - using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); @@ -354,11 +365,13 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer + rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { @@ -371,9 +384,22 @@ class IntraProcessManager if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageType ros_msg; rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); - ros_message_subscription->provide_intra_process_message(ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); } else { - ros_message_subscription->provide_intra_process_message(message); + if constexpr (std::is_same::value) { + ros_message_subscription->provide_intra_process_message(message); + } else { + if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) + { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message( + *message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } + } } } } else { @@ -388,7 +414,8 @@ class IntraProcessManager template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT> void add_owned_msg_to_buffers( std::unique_ptr message, @@ -398,7 +425,14 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; using PublishedType = typename rclcpp::TypeAdapter::custom_type; - using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); @@ -408,41 +442,47 @@ class IntraProcessManager auto subscription_base = subscription_it->second.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer + rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " + "--failed to dynamic cast SubscriptionIntraProcessBase to " "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } else { if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; - auto ptr = ROSMessageTypeAllocatorTraits::allocate(allocator, 1); - ROSMessageTypeAllocatorTraits::construct(allocator, ptr); - Deleter deleter = message.get_deleter(); - auto ros_msg = std::unique_ptr(ptr, deleter); - rclcpp::TypeAdapter::convert_to_ros_message(message, ros_msg); + ROSMessageTypeAllocator ros_message_alloc(allocator); + auto ptr = ros_message_alloc.allocate(1); + ros_message_alloc.construct(ptr); + ROSMessageTypeDeleter deleter; + allocator::set_allocator_for_deleter(&deleter, &allocator); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + auto ros_msg = std::unique_ptr(ptr, deleter); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { - if (std::next(it) == subscription_ids.end()) { - // If this is the last subscription, give up ownership - ros_message_subscription->provide_intra_process_message(std::move(message)); - } else { - // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; - Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(allocator, 1); - MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - - ros_message_subscription->provide_intra_process_message(std::move(copy_message)); + if constexpr (std::is_same::value) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + ros_message_subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + allocator::set_allocator_for_deleter(&deleter, &allocator); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + ros_message_subscription->provide_intra_process_message(std::move(copy_message)); + } } } } diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 117737957b..54857562d0 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -44,20 +44,24 @@ namespace experimental template< typename MessageT, typename SubscribedType, - typename SubscribedTypeAlloc = std::allocator, - typename SubscribedTypeDeleter = std::default_delete + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename ROSMessageType = SubscribedType, + typename Alloc = std::allocator > class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< SubscribedType, SubscribedTypeAlloc, - SubscribedTypeDeleter + SubscribedTypeDeleter, + ROSMessageType > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< SubscribedType, SubscribedTypeAlloc, - SubscribedTypeDeleter + SubscribedTypeDeleter, + ROSMessageType >; public: @@ -71,15 +75,15 @@ class SubscriptionIntraProcess using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( - AnySubscriptionCallback callback, - std::shared_ptr allocator, + AnySubscriptionCallback callback, + std::shared_ptr allocator, rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) : SubscriptionIntraProcessBuffer( - allocator, + SubscribedTypeDeleter, ROSMessageType>( + std::make_shared(*allocator), context, topic_name, qos_profile, @@ -157,7 +161,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 5b8aa39581..0ef98bd70f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -43,14 +43,16 @@ namespace experimental template< typename SubscribedType, - typename Alloc = std::allocator, + typename Alloc = std::allocator, typename Deleter = std::default_delete, /// MessageT::ros_message_type if MessageT is a TypeAdapter, /// otherwise just MessageT. - typename ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type + typename ROSMessageType = SubscribedType > -class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer +class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::allocator_type, + allocator::Deleter::allocator_type, + ROSMessageType>> { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) @@ -81,8 +83,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer(context, topic_name, - qos_profile), + : ROSMessageIntraProcessBuffer( + context, topic_name, qos_profile), subscribed_type_allocator_(*allocator) { allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); @@ -92,7 +94,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer( buffer_type, qos_profile, - allocator); + std::make_shared(subscribed_type_allocator_)); } bool diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index aaeedf3768..e1a409976e 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -81,7 +81,6 @@ class Publisher : public PublisherBase /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. using PublishedType = typename rclcpp::TypeAdapter::custom_type; - /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; using PublishedTypeAllocatorTraits = allocator::AllocRebind; @@ -320,11 +319,28 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - this->do_intra_process_publish(std::move(msg)); + // Avoid allocating when not using intra process. + if (!intra_process_is_enabled_) { + // In this case we're not using intra process. + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + return this->do_inter_process_publish(ros_msg); + } + + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + // TODO(clalancette): This is unnecessarily doing an additional conversion + // that may have already been done in do_intra_process_publish_and_return_shared(). + // We should just reuse that effort. + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + this->do_inter_process_publish(ros_msg); + } else { + this->do_intra_process_publish(std::move(msg)); + } } /// Publish a message on the topic. @@ -345,12 +361,7 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - - // Avoid allocating when not using intra process. + // Avoid double allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. ROSMessageType ros_msg; @@ -358,13 +369,12 @@ class Publisher : public PublisherBase // In this case we're not using intra process. return this->do_inter_process_publish(ros_msg); } - // Otherwise we have to allocate memory in a unique_ptr, convert it, - // and pass it along. + + // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. // A shared_ptr could also be constructed here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } void @@ -501,7 +511,7 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), published_type_allocator_); @@ -519,7 +529,7 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), ros_message_type_allocator_); @@ -540,7 +550,7 @@ class Publisher : public PublisherBase } return ipm->template do_intra_process_publish_and_return_shared( + ROSMessageType, AllocatorT>( intra_process_publisher_id_, std::move(msg), published_type_allocator_); @@ -559,7 +569,7 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_sharedtemplate do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), @@ -585,6 +595,15 @@ class Publisher : public PublisherBase return std::unique_ptr(ptr, ros_message_type_deleter_); } + /// Duplicate a given type adapted message as a unique_ptr. + std::unique_ptr + duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) + { + auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); + PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, published_type_deleter_); + } + /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index c238c9581f..2d52388e3b 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -408,8 +408,10 @@ class Subscription : public SubscriptionBase using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< MessageT, SubscribedType, - AllocatorT, - SubscribedTypeDeleter>; + SubscribedTypeAllocator, + SubscribedTypeDeleter, + ROSMessageT, + AllocatorT>; std::shared_ptr subscription_intra_process_; }; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index e60b1636b8..c8292a530b 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -364,41 +364,41 @@ if(WIN32) set(append_library_dirs "${append_library_dirs}/$") endif() -# ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp -# APPEND_LIBRARY_DIRS "${append_library_dirs}" -# ) -# if(TARGET test_publisher_with_type_adapter) -# ament_target_dependencies(test_publisher_with_type_adapter -# "rcutils" -# "rcl_interfaces" -# "rmw" -# "rosidl_runtime_cpp" -# "rosidl_typesupport_cpp" -# "test_msgs" -# ) -# target_link_libraries(test_publisher_with_type_adapter -# ${PROJECT_NAME} -# mimick -# ${cpp_typesupport_target}) -# endif() - -# ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp -# APPEND_LIBRARY_DIRS "${append_library_dirs}" -# ) -# if(TARGET test_subscription_with_type_adapter) -# ament_target_dependencies(test_subscription_with_type_adapter -# "rcutils" -# "rcl_interfaces" -# "rmw" -# "rosidl_runtime_cpp" -# "rosidl_typesupport_cpp" -# "test_msgs" -# ) -# target_link_libraries(test_subscription_with_type_adapter -# ${PROJECT_NAME} -# mimick -# ${cpp_typesupport_target}) -# endif() +ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_publisher_with_type_adapter) + ament_target_dependencies(test_publisher_with_type_adapter + "rcutils" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_publisher_with_type_adapter + ${PROJECT_NAME} + mimick + ${cpp_typesupport_target}) +endif() + +ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_with_type_adapter) + ament_target_dependencies(test_subscription_with_type_adapter + "rcutils" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription_with_type_adapter + ${PROJECT_NAME} + mimick + ${cpp_typesupport_target}) +endif() ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 45b781995d..47fc0aa83b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -222,7 +222,9 @@ class SubscriptionIntraProcessBase template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT +> class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase { public: @@ -279,7 +281,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase template< typename MessageT, - typename Alloc = std::allocator, + typename Alloc = std::allocator, typename Deleter = std::default_delete> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< MessageT, @@ -340,7 +342,7 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), *message_allocator_); diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index f5ab43cb7c..8b9d25aef3 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -166,17 +166,19 @@ TEST_F(TestPublisher, various_creation_signatures) { /* * Testing that conversion errors are passed up. + * TODO. This test was removed when implementing non convering intra-proccess + * publishing. It may be fixed and added again later. */ -TEST_F(TestPublisher, conversion_exception_is_passed_up) { - using BadStringTypeAdapter = rclcpp::TypeAdapter; - for (auto is_intra_process : {true, false}) { - rclcpp::NodeOptions options; - options.use_intra_process_comms(is_intra_process); - initialize(options); - auto pub = node->create_publisher("topic_name", 1); - EXPECT_THROW(pub->publish(1), std::runtime_error); - } -} +// TEST_F(TestPublisher, conversion_exception_is_passed_up) { +// using BadStringTypeAdapter = rclcpp::TypeAdapter; +// for (auto is_intra_process : {true, false}) { +// rclcpp::NodeOptions options; +// options.use_intra_process_comms(is_intra_process); +// initialize(options); +// auto pub = node->create_publisher("topic_name", 1); +// EXPECT_THROW(pub->publish(1), std::runtime_error); +// } +// } /* * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp index 036fa11359..77c4207b43 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -16,63 +16,21 @@ #include #include -#include #include +#include #include -#include +#include #include -#include #include "rclcpp/exceptions.hpp" -#include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" #include "rclcpp/msg/string.hpp" - -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - -using namespace std::chrono_literals; - static const int g_max_loops = 200; static const std::chrono::milliseconds g_sleep_per_loop(10); - -class TestSubscriptionPublisher : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test +class test_intra_process_within_one_node : public ::testing::Test { public: static void SetUpTestCase() @@ -137,8 +95,9 @@ void wait_for_message_to_be_received( * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), - check_type_adapted_messages_are_received_by_intra_process_subscription) { + test_intra_process_within_one_node, + type_adapted_messages_unique_pub_received_by_intra_process_subscription) +{ using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; @@ -149,45 +108,409 @@ TEST_F( auto pub = node->create_publisher(topic_name, 1); - { - { // std::unique_ptr - bool is_received = false; - auto callback = - [message_data, &is_received]( - std::unique_ptr msg) -> void { - is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - }; - auto sub = node->create_subscription(topic_name, 1, callback); - - std::cout << "LALA" << std::endl; - - auto pu_message = std::make_unique(message_data); - pub->publish(std::move(pu_message)); - - ASSERT_FALSE(is_received); - std::cout << "LALA TO BE RECEIVED" << std::endl; - wait_for_message_to_be_received(is_received, node); - ASSERT_TRUE(is_received); - } - { // std::unique_ptr with message info - bool is_received = false; - auto callback = - [message_data, &is_received]( - std::unique_ptr msg, - const rclcpp::MessageInfo & message_info) -> void { - is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); - }; - auto sub = node->create_subscription(topic_name, 1, callback); - - auto pu_message = std::make_unique(message_data); - pub->publish(std::move(pu_message)); - - ASSERT_FALSE(is_received); - wait_for_message_to_be_received(is_received, node); - ASSERT_TRUE(is_received); - } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_const_ref_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); } } From b76af8edd0aac8e9481972880674d90133192338 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 21 Dec 2021 12:36:05 -0500 Subject: [PATCH 12/20] Reenable disabled test. (#18) We just had to make sure there was a subscriber so that the conversion would be attempted, and then things started work. Signed-off-by: Chris Lalancette --- .../test_publisher_with_type_adapter.cpp | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 8b9d25aef3..01fc38dfd9 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -166,19 +166,25 @@ TEST_F(TestPublisher, various_creation_signatures) { /* * Testing that conversion errors are passed up. - * TODO. This test was removed when implementing non convering intra-proccess - * publishing. It may be fixed and added again later. */ -// TEST_F(TestPublisher, conversion_exception_is_passed_up) { -// using BadStringTypeAdapter = rclcpp::TypeAdapter; -// for (auto is_intra_process : {true, false}) { -// rclcpp::NodeOptions options; -// options.use_intra_process_comms(is_intra_process); -// initialize(options); -// auto pub = node->create_publisher("topic_name", 1); -// EXPECT_THROW(pub->publish(1), std::runtime_error); -// } -// } +TEST_F(TestPublisher, conversion_exception_is_passed_up) { + using BadStringTypeAdapter = rclcpp::TypeAdapter; + for (auto is_intra_process : {true, false}) { + rclcpp::NodeOptions options; + options.use_intra_process_comms(is_intra_process); + + auto callback = + [](const rclcpp::msg::String::ConstSharedPtr msg) -> void + { + (void)msg; + }; + + initialize(options); + auto pub = node->create_publisher("topic_name", 1); + auto sub = node->create_subscription("topic_name", 1, callback); + EXPECT_THROW(pub->publish(1), std::runtime_error); + } +} /* * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. From 42cad32fdc2d266bc5694b9944f582885d9566db Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 22 Dec 2021 11:32:45 -0300 Subject: [PATCH 13/20] Fix intra process publisher message move bug (#19) Signed-off-by: Gonzalo de Pedro --- rclcpp/include/rclcpp/publisher.hpp | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index e1a409976e..6afb397177 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -331,12 +331,12 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + ROSMessageType ros_msg; // TODO(clalancette): This is unnecessarily doing an additional conversion // that may have already been done in do_intra_process_publish_and_return_shared(). // We should just reuse that effort. - ROSMessageType ros_msg; rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + this->do_intra_process_publish(std::move(msg)); this->do_inter_process_publish(ros_msg); } else { this->do_intra_process_publish(std::move(msg)); @@ -535,27 +535,6 @@ class Publisher : public PublisherBase ros_message_type_allocator_); } - - std::shared_ptr - do_intra_process_publish_and_return_shared( - std::unique_ptr msg) - { - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publish msg which is a null pointer"); - } - - return ipm->template do_intra_process_publish_and_return_shared( - intra_process_publisher_id_, - std::move(msg), - published_type_allocator_); - } - std::shared_ptr do_intra_process_ros_message_publish_and_return_shared( std::unique_ptr msg) From 71c32391157d5061d8a7b28e5674305f66331983 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 22 Dec 2021 16:02:28 -0500 Subject: [PATCH 14/20] Fixes that came out of macOS. (#20) Signed-off-by: Chris Lalancette --- .../rclcpp/any_subscription_callback.hpp | 5 +++++ .../ros_message_intra_process_buffer.hpp | 2 -- .../subscription_intra_process_buffer.hpp | 4 ++-- .../test/rclcpp/test_intra_process_manager.cpp | 18 ++++++++++++------ 4 files changed, 19 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 191abd2228..4e5f0a4c6e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -817,6 +817,11 @@ class AnySubscriptionCallback // Dispatch. std::visit( [&message, &message_info, this](auto && callback) { + // clang complains that 'this' lambda capture is unused, which is true + // in *some* specializations of this template, but not others. Just + // quiet it down. + (void)this; + using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp index 82088fb39a..9cab1c52ab 100644 --- a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -38,8 +38,6 @@ template< class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase { public: - RCLCPP_SMART_PTR_DEFINITIONS(ROSMessageIntraProcessBuffer) - using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 0ef98bd70f..bd0927927f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -120,7 +120,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::value) { buffer_->add_shared(std::move(message)); @@ -132,7 +132,7 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::value) { buffer_->add_unique(std::move(message)); diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 47fc0aa83b..c78900ac1d 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -22,6 +22,7 @@ #define RCLCPP_BUILDING_LIBRARY 1 #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rmw/types.h" @@ -194,9 +195,14 @@ class SubscriptionIntraProcessBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - explicit SubscriptionIntraProcessBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), topic_name("topic") - {} + explicit SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic = "topic", + rclcpp::QoS qos = rclcpp::QoS(10)) + : qos_profile(qos), topic_name(topic) + { + (void)context; + } virtual ~SubscriptionIntraProcessBase() {} @@ -212,11 +218,11 @@ class SubscriptionIntraProcessBase const char * get_topic_name() { - return topic_name; + return topic_name.c_str(); } rclcpp::QoS qos_profile; - const char * topic_name; + std::string topic_name; }; template< @@ -231,7 +237,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos) - : SubscriptionIntraProcessBase(qos), take_shared_method(false) + : SubscriptionIntraProcessBase(nullptr, "topic", qos), take_shared_method(false) { buffer = std::make_unique>(); } From da5f12e657675aad8e6759e6d85506fc4e348ea6 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Thu, 30 Dec 2021 19:12:58 -0300 Subject: [PATCH 15/20] Addressed review comments Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 7 ++- .../experimental/intra_process_manager.hpp | 22 +++---- .../test_publisher_with_type_adapter.cpp | 2 + ...ption_publisher_with_same_type_adapter.cpp | 60 +++++++++---------- 4 files changed, 48 insertions(+), 43 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 4e5f0a4c6e..f6d9bf639b 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -485,9 +485,10 @@ class AnySubscriptionCallback rclcpp::TypeAdapter::convert_to_ros_message(msg, *ptr); return std::unique_ptr(ptr, ros_message_type_deleter_); } else { - throw std::runtime_error( - "convert_custom_type_to_ros_message_unique_ptr " - "unexpectedly called without TypeAdapter"); + static_assert( + !sizeof(MessageT*), + "convert_custom_type_to_ros_message_unique_ptr() " + "unexpectedly called without specialized TypeAdapter"); } } diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index cb3390338f..dba31ec0b1 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -213,6 +213,7 @@ class IntraProcessManager } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) { + // There is at maximum 1 buffer that does not require ownership. // Merge the two vector of ids into a unique one std::vector concatenated_vector(sub_ids.take_shared_subscriptions); concatenated_vector.insert( @@ -347,16 +348,15 @@ class IntraProcessManager std::shared_ptr message, std::vector subscription_ids) { - using PublishedType = typename rclcpp::TypeAdapter::custom_type; using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; + using PublishedType = typename rclcpp::TypeAdapter::custom_type; using PublishedTypeAllocatorTraits = allocator::AllocRebind; using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; using PublishedTypeDeleter = allocator::Deleter; - for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { @@ -377,9 +377,10 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); + "SubscriptionIntraProcessBuffer, and " + "to ROSMessageIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); } else { if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageType ros_msg; @@ -424,12 +425,12 @@ class IntraProcessManager { using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; - using PublishedType = typename rclcpp::TypeAdapter::custom_type; using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; + using PublishedType = typename rclcpp::TypeAdapter::custom_type; using PublishedTypeAllocatorTraits = allocator::AllocRebind; using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; using PublishedTypeDeleter = allocator::Deleter; @@ -453,10 +454,11 @@ class IntraProcessManager if (nullptr == ros_message_subscription) { throw std::runtime_error( - "--failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, and " + "to ROSMessageIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); } else { if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageTypeAllocator ros_message_alloc(allocator); diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 01fc38dfd9..e49eb42a37 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -181,6 +181,8 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { initialize(options); auto pub = node->create_publisher("topic_name", 1); + //A subscription is created to ensure the existence of a buffer in the intra proccess + //manager which will trigger the faulty conversion. auto sub = node->create_subscription("topic_name", 1, callback); EXPECT_THROW(pub->publish(1), std::runtime_error); } diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp index 77c4207b43..117e8652e0 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -115,7 +115,7 @@ TEST_F( [message_data, &is_received]( const std::string & msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_STREQ(message_data.c_str(), msg.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -133,8 +133,8 @@ TEST_F( [message_data, &is_received]( const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), msg.c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -153,7 +153,7 @@ TEST_F( [message_data, &is_received]( std::unique_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -172,8 +172,8 @@ TEST_F( std::unique_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -192,7 +192,7 @@ TEST_F( [message_data, &is_received]( std::shared_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -211,8 +211,8 @@ TEST_F( std::shared_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -231,7 +231,7 @@ TEST_F( [message_data, &is_received]( std::shared_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -250,8 +250,8 @@ TEST_F( std::shared_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -270,7 +270,7 @@ TEST_F( [message_data, &is_received]( const std::shared_ptr & msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -290,8 +290,8 @@ TEST_F( const std::shared_ptr & msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -325,7 +325,7 @@ TEST_F( [message_data, &is_received]( const std::string & msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_STREQ(message_data.c_str(), msg.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -343,8 +343,8 @@ TEST_F( [message_data, &is_received]( const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), msg.c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -363,7 +363,7 @@ TEST_F( [message_data, &is_received]( std::unique_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -382,8 +382,8 @@ TEST_F( std::unique_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -402,7 +402,7 @@ TEST_F( [message_data, &is_received]( std::shared_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -421,8 +421,8 @@ TEST_F( std::shared_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -441,7 +441,7 @@ TEST_F( [message_data, &is_received]( std::shared_ptr msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -461,8 +461,8 @@ TEST_F( std::shared_ptr msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -481,7 +481,7 @@ TEST_F( [message_data, &is_received]( const std::shared_ptr & msg) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); @@ -501,8 +501,8 @@ TEST_F( const std::shared_ptr & msg, const rclcpp::MessageInfo & message_info) -> void { is_received = true; - ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); From f3b761677eeb64510cee688d5e8bdc94dc1267e6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 4 Jan 2022 16:16:02 +0000 Subject: [PATCH 16/20] Fix style issues. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/any_subscription_callback.hpp | 2 +- rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index f6d9bf639b..56af35c518 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -486,7 +486,7 @@ class AnySubscriptionCallback return std::unique_ptr(ptr, ros_message_type_deleter_); } else { static_assert( - !sizeof(MessageT*), + !sizeof(MessageT *), "convert_custom_type_to_ros_message_unique_ptr() " "unexpectedly called without specialized TypeAdapter"); } diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index e49eb42a37..8c6ebaee57 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -181,8 +181,8 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { initialize(options); auto pub = node->create_publisher("topic_name", 1); - //A subscription is created to ensure the existence of a buffer in the intra proccess - //manager which will trigger the faulty conversion. + // A subscription is created to ensure the existence of a buffer in the intra proccess + // manager which will trigger the faulty conversion. auto sub = node->create_subscription("topic_name", 1, callback); EXPECT_THROW(pub->publish(1), std::runtime_error); } From 3c29a572f72b6274a486f72116ca5667f0cfdba8 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Thu, 30 Dec 2021 17:10:27 -0300 Subject: [PATCH 17/20] Added tests for non transform intra process Signed-off-by: Gonzalo de Pedro --- rclcpp/test/rclcpp/CMakeLists.txt | 8 +- ...ption_publisher_with_same_type_adapter.cpp | 475 ++++++++++++++++++ 2 files changed, 476 insertions(+), 7 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c8292a530b..34bdc0daed 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -405,16 +405,10 @@ ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscrip ) if(TARGET test_subscription_publisher_with_same_type_adapter) ament_target_dependencies(test_subscription_publisher_with_same_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" + "statistics_msgs" ) target_link_libraries(test_subscription_publisher_with_same_type_adapter ${PROJECT_NAME} - mimick ${cpp_typesupport_target}) endif() diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp index 117e8652e0..5cee82c3ef 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -26,6 +26,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/msg/string.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" static const int g_max_loops = 200; static const std::chrono::milliseconds g_sleep_per_loop(10); @@ -75,6 +76,31 @@ struct TypeAdapter } }; +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = double; + using ros_message_type = statistics_msgs::msg::StatisticDataPoint; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data_type = 0; + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + } // namespace rclcpp void wait_for_message_to_be_received( @@ -514,3 +540,452 @@ TEST_F( ASSERT_TRUE(is_received); } } + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_ros_message_ref_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.894; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_ptr_ros_message_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.7508; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} From 1779cd820a78763c867ad8183166c7025e5285fa Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 4 Jan 2022 19:28:30 +0000 Subject: [PATCH 18/20] Refactor the intra-process-manager to be more readable. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 191 +++++++++--------- 1 file changed, 97 insertions(+), 94 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index dba31ec0b1..3f8e0ed8a4 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -363,51 +363,52 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer - >(subscription_base); - - if (nullptr == ros_message_subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, and " - "to ROSMessageIntraProcessBuffer which can happen when the publisher and " - "subscription use different allocator types, which is not supported"); - } else { - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); - ros_message_subscription->provide_intra_process_message( - std::make_shared(ros_msg)); - } else { - if constexpr (std::is_same::value) { - ros_message_subscription->provide_intra_process_message(message); - } else { - if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) - { - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message( - *message, ros_msg); - ros_message_subscription->provide_intra_process_message( - std::make_shared(ros_msg)); - } - } - } - } + if (subscription_base == nullptr) { + subscriptions_.erase(id); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + subscription->provide_intra_process_data(message); + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::ROSMessageIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "ROSMessageIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } else { + if constexpr (std::is_same::value) { + ros_message_subscription->provide_intra_process_message(message); } else { - subscription->provide_intra_process_data(message); + if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) + { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message( + *message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } } - } else { - subscriptions_.erase(id); } } } @@ -441,70 +442,72 @@ class IntraProcessManager throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer - >(subscription_base); - - if (nullptr == ros_message_subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, and " - "to ROSMessageIntraProcessBuffer which can happen when the publisher and " - "subscription use different allocator types, which is not supported"); - } else { - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { - ROSMessageTypeAllocator ros_message_alloc(allocator); - auto ptr = ros_message_alloc.allocate(1); - ros_message_alloc.construct(ptr); - ROSMessageTypeDeleter deleter; - allocator::set_allocator_for_deleter(&deleter, &allocator); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); - auto ros_msg = std::unique_ptr(ptr, deleter); - ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); - } else { - if constexpr (std::is_same::value) { - if (std::next(it) == subscription_ids.end()) { - // If this is the last subscription, give up ownership - ros_message_subscription->provide_intra_process_message(std::move(message)); - } else { - // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; - Deleter deleter = message.get_deleter(); - allocator::set_allocator_for_deleter(&deleter, &allocator); - auto ptr = MessageAllocTraits::allocate(allocator, 1); - MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - - ros_message_subscription->provide_intra_process_message(std::move(copy_message)); - } - } - } - } + if (subscription_base == nullptr) { + subscriptions_.erase(subscription_it); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_data(std::move(message)); } else { + // Copy the message since we have additional subscriptions to serve + MessageUniquePtr copy_message; + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_data(std::move(copy_message)); + } + + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::ROSMessageIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "ROSMessageIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageTypeAllocator ros_message_alloc(allocator); + auto ptr = ros_message_alloc.allocate(1); + ros_message_alloc.construct(ptr); + ROSMessageTypeDeleter deleter; + allocator::set_allocator_for_deleter(&deleter, &allocator); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + auto ros_msg = std::unique_ptr(ptr, deleter); + ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); + } else { + if constexpr (std::is_same::value) { if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership - subscription->provide_intra_process_data(std::move(message)); + ros_message_subscription->provide_intra_process_message(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); + allocator::set_allocator_for_deleter(&deleter, &allocator); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); - subscription->provide_intra_process_data(std::move(copy_message)); + ros_message_subscription->provide_intra_process_message(std::move(copy_message)); } } - } else { - subscriptions_.erase(subscription_it); } } } From c8c4851b53de49cc1fe2f3fc3d36ef3e2df8ffee Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 4 Jan 2022 17:17:45 -0500 Subject: [PATCH 19/20] Respond to more review comments. Signed-off-by: Chris Lalancette --- .../experimental/intra_process_manager.hpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 3f8e0ed8a4..aca8f38ce0 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -180,7 +180,7 @@ class IntraProcessManager template< typename MessageT, typename ROSMessageType, - typename Alloc = std::allocator, + typename Alloc, typename Deleter = std::default_delete > void @@ -214,6 +214,8 @@ class IntraProcessManager sub_ids.take_shared_subscriptions.size() <= 1) { // There is at maximum 1 buffer that does not require ownership. + // So this case is equivalent to all the buffers requiring ownership + // Merge the two vector of ids into a unique one std::vector concatenated_vector(sub_ids.take_shared_subscriptions); concatenated_vector.insert( @@ -241,7 +243,7 @@ class IntraProcessManager template< typename MessageT, typename ROSMessageType, - typename Alloc = std::allocator, + typename Alloc, typename Deleter = std::default_delete > std::shared_ptr @@ -415,9 +417,9 @@ class IntraProcessManager template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete, - typename ROSMessageType = MessageT> + typename Alloc, + typename Deleter, + typename ROSMessageType> void add_owned_msg_to_buffers( std::unique_ptr message, @@ -457,13 +459,11 @@ class IntraProcessManager subscription->provide_intra_process_data(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - subscription->provide_intra_process_data(std::move(copy_message)); + subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); } continue; @@ -498,14 +498,13 @@ class IntraProcessManager ros_message_subscription->provide_intra_process_message(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); allocator::set_allocator_for_deleter(&deleter, &allocator); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - ros_message_subscription->provide_intra_process_message(std::move(copy_message)); + ros_message_subscription->provide_intra_process_message( + std::move(MessageUniquePtr(ptr, deleter))); } } } From 6d47368b2e46f61735c99756499d4d0354fa586d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 6 Jan 2022 16:37:41 -0500 Subject: [PATCH 20/20] Rename ROSMessageIntraProcessBuffer() To SubscriptionROSMsgIntraProcessBuffer() Signed-off-by: Chris Lalancette --- .../include/rclcpp/experimental/intra_process_manager.hpp | 8 ++++---- .../experimental/ros_message_intra_process_buffer.hpp | 6 +++--- .../experimental/subscription_intra_process_buffer.hpp | 5 +++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index aca8f38ce0..178a6026e3 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -380,14 +380,14 @@ class IntraProcessManager } auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " "SubscriptionIntraProcessBuffer, or to " - "ROSMessageIntraProcessBuffer which can happen when the publisher and " "subscription use different allocator types, which is not supported"); } @@ -470,14 +470,14 @@ class IntraProcessManager } auto ros_message_subscription = std::dynamic_pointer_cast< - rclcpp::experimental::ROSMessageIntraProcessBuffer >(subscription_base); if (nullptr == ros_message_subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " "SubscriptionIntraProcessBuffer, or to " - "ROSMessageIntraProcessBuffer which can happen when the publisher and " "subscription use different allocator types, which is not supported"); } diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp index 9cab1c52ab..7b8b3b833d 100644 --- a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -35,7 +35,7 @@ template< typename Alloc = std::allocator, typename Deleter = std::default_delete > -class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase +class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase { public: using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; @@ -45,14 +45,14 @@ class ROSMessageIntraProcessBuffer : public SubscriptionIntraProcessBase using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; - ROSMessageIntraProcessBuffer( + SubscriptionROSMsgIntraProcessBuffer( rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile) : SubscriptionIntraProcessBase(context, topic_name, qos_profile) {} - virtual ~ROSMessageIntraProcessBuffer() + virtual ~SubscriptionROSMsgIntraProcessBuffer() {} virtual void diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index bd0927927f..2e20c3de9b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -49,7 +49,7 @@ template< /// otherwise just MessageT. typename ROSMessageType = SubscribedType > -class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer::allocator_type, allocator::Deleter::allocator_type, ROSMessageType>> @@ -83,7 +83,8 @@ class SubscriptionIntraProcessBuffer : public ROSMessageIntraProcessBuffer( + : SubscriptionROSMsgIntraProcessBuffer( context, topic_name, qos_profile), subscribed_type_allocator_(*allocator) {