From 60deefe7aa0349a2fc163becac780e4b1f3ef6b1 Mon Sep 17 00:00:00 2001 From: alberto Date: Wed, 3 Jul 2019 19:59:06 +0100 Subject: [PATCH 01/18] basic ipc implementation from alsora/new_ipc_proposal Signed-off-by: alberto better use of node_topic create subscription Signed-off-by: alberto added intra process manager test Signed-off-by: alberto fixed ring buffer and added test Signed-off-by: alberto added intra process buffer test Signed-off-by: alberto added intra process buffer test Signed-off-by: alberto Signed-off-by: alberto removed intra-process methods from subscription base Signed-off-by: alberto using lock_guard instead of unique_lock, renamed var without camel case Signed-off-by: alberto using unordered set and references in intra process manager Signed-off-by: alberto subscription intra-process does not depend anymore on subscription, but has a copy of the callback Signed-off-by: alberto changed buffer API to use rvo Signed-off-by: Alberto avoid copying shared_ptr Signed-off-by: alberto revert not needed changes to create_subscription Signed-off-by: alberto updated tests according to new buffer APIs Signed-off-by: alberto updated types in ring buffer implementation avoid using uint32_t Signed-off-by: alberto using unique ptr for buffers in subscription_intra_process Signed-off-by: alberto added missing std::move in subscription_intra_process constructor Signed-off-by: alberto use consisting names for ring_buffer_implementation members Signed-off-by: alberto addressing typos, one-liners and similar from ivanpauno review Signed-off-by: alberto moved subscription_intra_process_base to its own files and moved non templated method from derived class Signed-off-by: alberto removed forward declarations, fixed include subscription_intra_process_base Signed-off-by: alberto removed member variable from do_intra_process_publish signature Signed-off-by: alberto declare public before private in intra_process_manager_impl Signed-off-by: alberto made matches_any_intra_process_publishers const Signed-off-by: alberto using const reference in get_all_matching_publishers Signed-off-by: alberto added deleter and alloc templates in intra_process_buffer Signed-off-by: alberto added RCLCPP_WARN to intra_process_manager_impl Signed-off-by: alberto passing context from node to subscription_intra_process Signed-off-by: alberto using allocators in intra_process_manager Signed-off-by: alberto use size_t instead of int in ring buffer indices Signed-off-by: alberto creating buffer inside subscription_intra_process constructor Signed-off-by: alberto fix lint errors Signed-off-by: alberto throw error if trying to dequeue when buffer empty; remove duplicated methods in intra_process_buffer Signed-off-by: alberto added todo for creating an rmw function for checking qos compatibility Signed-off-by: alberto test fixes Signed-off-by: alberto refactored intra_process_manager, removed ipm impl Signed-off-by: alberto added mutex in intra_process_manager add_* methods Signed-off-by: Soragna, Alberto added allocator to intra_process_buffer Signed-off-by: Soragna, Alberto added invalid intra_process qos test for subscription Signed-off-by: Soragna, Alberto throw error if history size is 0 with keep last and ipc Signed-off-by: Soragna, Alberto using allocator when creating unique_ptr from shared_ptr Signed-off-by: Soragna, Alberto adding deleter template argument to intra_process buffer Signed-off-by: Soragna, Alberto fix linter Signed-off-by: Soragna, Alberto throw error with callbackT different from messageT Signed-off-by: Soragna, Alberto updated deleter template argument in subscription factory Signed-off-by: Soragna, Alberto Fix typo in test fixture tear down method name (#787) Signed-off-by: Jacob Perron Add free function for creating service clients (#788) Equivalent to the free function for creating a service. Resolves #768 Signed-off-by: Jacob Perron Cmake infrastructure for creating components (#784) *cmake macro to create components for libraries with multiple nodes Signed-off-by: Siddharth Kucheria Allow registering multiple on_parameters_set_callback (#772) Signed-off-by: ivanpauno fix for multiple nodes not being recognized (#790) Signed-off-by: Siddharth Kucheria Remove non-package from ament_target_dependencies() (#793) Signed-off-by: Shane Loretz fix linter issue (#795) Signed-off-by: Siddharth Kucheria Make TimeSource ignore use_sim_time events coming from other nodes. (#799) Signed-off-by: Michel Hidalgo passing deleter template parameter Signed-off-by: Soragna, Alberto small fixes for failing tests Signed-off-by: Soragna, Alberto fixed imports in test_intra_process_manager Signed-off-by: Soragna, Alberto using RCLCPP_SMART_PTR_ALIASES_ONLY and RCLCPP_PUBLIC macros Signed-off-by: Soragna, Alberto added RCLCPP_PUBLIC macros and virtual destructor to sub intra_process base Signed-off-by: Soragna, Alberto added unique_ptr alias to macros Signed-off-by: Soragna, Alberto updated test_intra_process_manager.cpp Signed-off-by: Soragna, Alberto remove mock msgs from rclcpp (#800) Signed-off-by: Karsten Knese Add line break after first open paren in multiline function call (#785) * Add line break after first open paren in multiline function call as per developer guide: https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#open-versus-cuddled-braces see https://github.com/ament/ament_lint/pull/148 Signed-off-by: Dan Rose Fix dedent when first function argument starts with a brace Signed-off-by: Dan Rose Line break with multiline if condition Remove line breaks where allowed. Signed-off-by: Dan Rose Fixup after rebase Signed-off-by: Dan Rose Fixup again after reverting indent_paren_open_brace Signed-off-by: Dan Rose * Revert comment spacing change, condense some lines Signed-off-by: Dan Rose Adapt to '--ros-args ... [--]'-based ROS args extraction (#816) * Use --ros-args to deal with node arguments in rclcpp. Signed-off-by: Michel Hidalgo * Document implicit --ros-args flag in NodeOptions::arguments(). Signed-off-by: Michel Hidalgo * Add missing size_t to int cast. Signed-off-by: Michel Hidalgo * Only add implicit --ros-args flag if not present already. Signed-off-by: Michel Hidalgo * Add some rclcpp::NodeOptions test coverage. Signed-off-by: Michel Hidalgo * Address peer review comments. Signed-off-by: Michel Hidalgo * Please cpplint and uncrustify. Signed-off-by: Michel Hidalgo Guard against making multiple result requests for a goal handle (#808) This fixes a runtime error caused by a race condition when making consecutive requests for the result. Specifically, this happens if the user provides a result callback when sending a goal and then calls async_get_result shortly after. Resolves #783 Signed-off-by: Jacob Perron Explain return value of spin_until_future_complete (#792) Signed-off-by: Dan Rose Allow passing logger by const ref (#820) Signed-off-by: Karsten Knese Delete unnecessary call for get_node_by_group (#823) Signed-off-by: Tomoya.Fujita Fix get_node_interfaces functions taking a pointer (#821) Signed-off-by: ivanpauno add callback group as member variable and constructor arg (#811) Signed-off-by: bpwilcox remove callback group as member variable Wrap documentation examples in code blocks (#830) This makes the code examples easier to read in the generated documentation. Signed-off-by: Jacob Perron Crash in callback group pointer vector iterator (#814) Signed-off-by: Guillaume Autran add mutex in add/remove_node and wait_for_work to protect concurrent use/change of memory_strategy_ (#837) Signed-off-by: Dirk Thomas Fix hang with timers in MultiThreadedExecutor (#835) (#836) Signed-off-by: Todd Malsbary Use of -r/--remap flags where appropriate. (#834) Signed-off-by: Michel Hidalgo Force explicit --ros-args in NodeOptions::arguments(). (#845) Signed-off-by: Michel Hidalgo Fail on invalid and unknown ROS specific arguments (#842) * Fail on invalid and unknown ROS specific arguments. Signed-off-by: Michel Hidalgo * Revert changes to utilities.hpp in rclcpp Signed-off-by: Michel Hidalgo * Fully revert change to utilities.hpp Signed-off-by: Michel Hidalgo Fix typo in deprecated warning. (#848) "it's" instead of its Signed-off-by: Luca Della Vedova Add throwing parameter name if parameter is not set (#833) * added throwing parameter name if parameter is not set Signed-off-by: Alex Signed-off-by: ivanpauno check valid timer handler 1st to reduce the time window for scan. (#841) Signed-off-by: Tomoya.Fujita remove features and related code which were deprecated in dashing (#852) Signed-off-by: William Woodall reset error message before setting a new one, embed the original one (#854) Signed-off-by: Dirk Thomas restored virtual destructor in publisher_base Signed-off-by: Soragna, Alberto --- rclcpp/CMakeLists.txt | 27 +- .../rclcpp/any_subscription_callback.hpp | 2 +- .../buffers/buffer_implementation_base.hpp} | 28 +- .../rclcpp/buffers/intra_process_buffer.hpp | 230 +++ .../buffers/ring_buffer_implementation.hpp | 120 ++ .../rclcpp/create_intra_process_buffer.hpp | 95 ++ rclcpp/include/rclcpp/create_subscription.hpp | 1 + rclcpp/include/rclcpp/executor.hpp | 5 - .../rclcpp/intra_process_buffer_type.hpp | 35 + .../include/rclcpp/intra_process_manager.hpp | 526 +++---- .../rclcpp/intra_process_manager_impl.hpp | 358 ----- rclcpp/include/rclcpp/macros.hpp | 1 + rclcpp/include/rclcpp/mapped_ring_buffer.hpp | 319 ----- .../rclcpp/node_interfaces/node_topics.hpp | 3 +- .../node_interfaces/node_topics_interface.hpp | 4 +- rclcpp/include/rclcpp/publisher.hpp | 89 +- rclcpp/include/rclcpp/publisher_base.hpp | 18 +- .../strategies/allocator_memory_strategy.hpp | 20 +- rclcpp/include/rclcpp/subscription.hpp | 113 +- rclcpp/include/rclcpp/subscription_base.hpp | 24 +- .../include/rclcpp/subscription_factory.hpp | 1 + .../rclcpp/subscription_intra_process.hpp | 157 ++ .../subscription_intra_process_base.hpp | 84 ++ .../include/rclcpp/subscription_options.hpp | 4 + rclcpp/src/rclcpp/executor.cpp | 29 +- rclcpp/src/rclcpp/intra_process_manager.cpp | 152 +- rclcpp/src/rclcpp/memory_strategy.cpp | 4 +- .../rclcpp/node_interfaces/node_topics.cpp | 17 +- rclcpp/src/rclcpp/publisher_base.cpp | 82 +- rclcpp/src/rclcpp/subscription_base.cpp | 55 +- .../subscription_intra_process_base.cpp | 38 + rclcpp/src/rclcpp/time_source.cpp | 7 + rclcpp/test/test_intra_process_buffer.cpp | 246 ++++ rclcpp/test/test_intra_process_manager.cpp | 1266 +++++++---------- rclcpp/test/test_mapped_ring_buffer.cpp | 334 ----- rclcpp/test/test_publisher.cpp | 6 +- .../test/test_ring_buffer_implementation.cpp | 80 ++ rclcpp/test/test_subscription.cpp | 77 +- 38 files changed, 2256 insertions(+), 2401 deletions(-) rename rclcpp/{src/rclcpp/intra_process_manager_impl.cpp => include/rclcpp/buffers/buffer_implementation_base.hpp} (50%) create mode 100644 rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp create mode 100644 rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp create mode 100644 rclcpp/include/rclcpp/create_intra_process_buffer.hpp create mode 100644 rclcpp/include/rclcpp/intra_process_buffer_type.hpp delete mode 100644 rclcpp/include/rclcpp/intra_process_manager_impl.hpp delete mode 100644 rclcpp/include/rclcpp/mapped_ring_buffer.hpp create mode 100644 rclcpp/include/rclcpp/subscription_intra_process.hpp create mode 100644 rclcpp/include/rclcpp/subscription_intra_process_base.hpp create mode 100644 rclcpp/src/rclcpp/subscription_intra_process_base.cpp create mode 100644 rclcpp/test/test_intra_process_buffer.cpp delete mode 100644 rclcpp/test/test_mapped_ring_buffer.cpp create mode 100644 rclcpp/test/test_ring_buffer_implementation.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 16a67f0e00..275b9c6c45 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -47,7 +47,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/graph_listener.cpp src/rclcpp/init_options.cpp src/rclcpp/intra_process_manager.cpp - src/rclcpp/intra_process_manager_impl.cpp src/rclcpp/logger.cpp src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp @@ -75,6 +74,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp src/rclcpp/subscription_base.cpp + src/rclcpp/subscription_intra_process_base.cpp src/rclcpp/time.cpp src/rclcpp/time_source.cpp src/rclcpp/timer.cpp @@ -200,25 +200,36 @@ if(BUILD_TESTING) "rosidl_typesupport_cpp" ) endif() - ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp) - if(TARGET test_mapped_ring_buffer) - ament_target_dependencies(test_mapped_ring_buffer + ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp) + if(TARGET test_intra_process_manager) + ament_target_dependencies(test_intra_process_manager "rcl" "rcl_interfaces" "rmw" "rosidl_generator_cpp" "rosidl_typesupport_cpp" ) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) endif() - ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) - if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" + ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp) + if(TARGET test_ring_buffer_implementation) + ament_target_dependencies(test_ring_buffer_implementation + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" + "rosidl_typesupport_cpp" + ) + target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) + endif() + ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp) + if(TARGET test_intra_process_buffer) + ament_target_dependencies(test_intra_process_buffer "rcl_interfaces" "rmw" "rosidl_generator_cpp" "rosidl_typesupport_cpp" ) + target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) endif() ament_add_gtest(test_loaned_message test/test_loaned_message.cpp) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 26aeaba98e..f6b669fedb 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -227,7 +227,7 @@ class AnySubscriptionCallback TRACEPOINT(callback_end, (const void *)this); } - bool use_take_shared_method() + bool use_take_shared_method() const { return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_; } diff --git a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp b/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp similarity index 50% rename from rclcpp/src/rclcpp/intra_process_manager_impl.cpp rename to rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp index 2fa1d2f969..5121dbc095 100644 --- a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp +++ b/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2019 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/intra_process_manager_impl.hpp" +#ifndef RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#define RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ -#include +namespace rclcpp +{ +namespace intra_process_buffer +{ -rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr -rclcpp::intra_process_manager::create_default_impl() +template +class BufferImplementationBase { - return std::make_shared>(); -} +public: + virtual BufferT dequeue() = 0; + virtual void enqueue(BufferT request) = 0; + + virtual void clear() = 0; + virtual bool has_data() const = 0; +}; + +} // namespace intra_process_buffer +} // namespace rclcpp + +#endif // RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp new file mode 100644 index 0000000000..c266f7183d --- /dev/null +++ b/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp @@ -0,0 +1,230 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/buffers/buffer_implementation_base.hpp" + +namespace rclcpp +{ +namespace intra_process_buffer +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual void clear() = 0; + + virtual bool has_data() const = 0; + virtual bool use_take_shared_method() const = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer) + + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + virtual void add_shared(MessageSharedPtr msg) = 0; + virtual void add_unique(MessageUniquePtr msg) = 0; + + virtual MessageSharedPtr consume_shared() = 0; + virtual MessageUniquePtr consume_unique() = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete, + typename BufferT = std::unique_ptr> +class TypedIntraProcessBuffer : public IntraProcessBuffer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + TypedIntraProcessBuffer( + std::unique_ptr> buffer_impl, + std::shared_ptr allocator = nullptr) + { + bool valid_type = (std::is_same::value || + std::is_same::value); + if (!valid_type) { + throw std::runtime_error("Creating TypedIntraProcessBuffer with not valid BufferT"); + } + + buffer_ = std::move(buffer_impl); + + if (!allocator) { + message_allocator_ = std::make_shared(); + } else { + message_allocator_ = std::make_shared(*allocator.get()); + } + } + + void add_shared(MessageSharedPtr msg) + { + add_shared_impl(std::move(msg)); + } + + void add_unique(MessageUniquePtr msg) + { + buffer_->enqueue(std::move(msg)); + } + + MessageSharedPtr consume_shared() + { + return consume_shared_impl(); + } + + MessageUniquePtr consume_unique() + { + return consume_unique_impl(); + } + + bool has_data() const + { + return buffer_->has_data(); + } + + void clear() + { + buffer_->clear(); + } + + bool use_take_shared_method() const + { + return std::is_same::value; + } + +private: + std::unique_ptr> buffer_; + + std::shared_ptr message_allocator_; + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + buffer_->enqueue(std::move(shared_msg)); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + // This should not happen: here a copy is unconditionally made, while the intra-process manager + // can decide whether a copy is needed depending on the number and the type of buffers + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(shared_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + buffer_->enqueue(std::move(unique_msg)); + } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + (std::is_same::value), + MessageSharedPtr + >::type + consume_shared_impl() + { + return buffer_->dequeue(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + (std::is_same::value), + MessageSharedPtr + >::type + consume_shared_impl() + { + // automatic cast from unique ptr to shared ptr + return buffer_->dequeue(); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + MessageSharedPtr buffer_msg = buffer_->dequeue(); + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(buffer_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *buffer_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + return unique_msg; + } + + // MessageUniquePtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + return buffer_->dequeue(); + } +}; + +} // namespace intra_process_buffer +} // namespace rclcpp + + +#endif // RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp new file mode 100644 index 0000000000..a8d94e457d --- /dev/null +++ b/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp @@ -0,0 +1,120 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#define RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "iostream" + +namespace rclcpp +{ +namespace intra_process_buffer +{ + +template +class RingBufferImplementation : public BufferImplementationBase +{ +public: + explicit RingBufferImplementation(size_t capacity) + : ring_buffer_(capacity) + { + capacity_ = capacity; + write_index_ = capacity_ - 1; + read_index_ = 0; + size_ = 0; + + if (capacity == 0) { + throw std::invalid_argument("capacity must be a positive, non-zero value"); + } + } + + virtual ~RingBufferImplementation() {} + + void enqueue(BufferT request) + { + std::lock_guard lock(mutex_); + + write_index_ = next(write_index_); + ring_buffer_[write_index_] = std::move(request); + + if (is_full()) { + read_index_ = next(read_index_); + } else { + size_++; + } + } + + BufferT dequeue() + { + if (!has_data()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); + throw std::runtime_error("Calling dequeue on empty intra-process buffer"); + } + + std::lock_guard lock(mutex_); + + auto request = std::move(ring_buffer_[read_index_]); + read_index_ = next(read_index_); + + size_--; + + return request; + } + + inline uint32_t next(uint32_t val) + { + return (val + 1) % capacity_; + } + + inline bool has_data() const + { + return size_ != 0; + } + + inline bool is_full() + { + return size_ == capacity_; + } + + void clear() {} + +private: + std::vector ring_buffer_; + + size_t write_index_; + size_t read_index_; + size_t size_; + size_t capacity_; + + std::mutex mutex_; +}; + +} // namespace intra_process_buffer +} // namespace rclcpp + +#endif // RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ diff --git a/rclcpp/include/rclcpp/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/create_intra_process_buffer.hpp new file mode 100644 index 0000000000..c8c069bc7e --- /dev/null +++ b/rclcpp/include/rclcpp/create_intra_process_buffer.hpp @@ -0,0 +1,95 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/buffers/ring_buffer_implementation.hpp" +#include "rclcpp/buffers/intra_process_buffer.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" + +namespace rclcpp +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +typename intra_process_buffer::IntraProcessBuffer::UniquePtr +create_intra_process_buffer( + IntraProcessBufferType buffer_type, + rmw_qos_profile_t qos, + std::shared_ptr allocator) +{ + using MessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + size_t buffer_size = qos.depth; + + typename intra_process_buffer::IntraProcessBuffer::UniquePtr buffer; + + switch (buffer_type) { + case IntraProcessBufferType::SharedPtr: + { + using BufferT = MessageSharedPtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + case IntraProcessBufferType::UniquePtr: + { + using BufferT = MessageUniquePtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + default: + { + throw std::runtime_error("Unrecognized IntraProcessBufferType value"); + break; + } + } + + return buffer; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 0f267cb94d..9248254047 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -72,6 +72,7 @@ create_subscription( auto sub = node_topics->create_subscription(topic_name, factory, qos); node_topics->add_subscription(sub, options.callback_group); + return std::dynamic_pointer_cast(sub); } diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 095a765e86..cbeda82a24 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -305,11 +305,6 @@ class Executor execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription); - RCLCPP_PUBLIC - static void - execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); - RCLCPP_PUBLIC static void execute_timer(rclcpp::TimerBase::SharedPtr timer); diff --git a/rclcpp/include/rclcpp/intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/intra_process_buffer_type.hpp new file mode 100644 index 0000000000..828a96c765 --- /dev/null +++ b/rclcpp/include/rclcpp/intra_process_buffer_type.hpp @@ -0,0 +1,35 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ + +namespace rclcpp +{ + +/// Used as argument in create_publisher and create_subscriber +/// when intra-process communication is enabled +enum class IntraProcessBufferType +{ + /// Set the data type used in the intra-process buffer as std::shared_ptr + SharedPtr, + /// Set the data type used in the intra-process buffer as std::unique_ptr + UniquePtr, + /// Set the data type used in the intra-process buffer as the same used in the callback + CallbackDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp index 5d3abed3f6..73d0eb0e45 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2019 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,31 +17,35 @@ #include +#include + #include #include #include #include #include #include -#include +#include #include #include -#include +#include #include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/intra_process_manager_impl.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" -#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_intra_process.hpp" +#include "rclcpp/subscription_intra_process_base.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { + namespace intra_process_manager { -/// This class facilitates intra process communication between nodes. +/// This class performs intra process communication between nodes. /** * This class is used in the creation of publishers and subscriptions. * A singleton instance of this class is owned by a rclcpp::Context and a @@ -49,74 +53,34 @@ namespace intra_process_manager * Nodes which do not have a common Context will not exchange intra process * messages because they will not share access to an instance of this class. * - * When a Node creates a publisher or subscription, it will register them - * with this class. - * The node will also hook into the publisher's publish call - * in order to do intra process related work. - * - * When a publisher is created, it advertises on the topic the user provided, - * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage. - * For instance, if the user specified the topic '/namespace/chatter', then the - * corresponding intra process topic might be '/namespace/chatter/_intra'. - * The publisher is also allocated an id which is unique among all publishers - * and subscriptions in this process. - * Additionally, when registered with this class a ring buffer is created and - * owned by this class as a temporary place to hold messages destined for intra - * process subscriptions. - * - * When a subscription is created, it subscribes to the topic provided by the - * user as well as to the corresponding intra process topic. - * It is also gets a unique id from the singleton instance of this class which - * is unique among publishers and subscriptions. - * - * When the user publishes a message, the message is stored by calling - * store_intra_process_message on this class. - * The instance of that message is uniquely identified by a publisher id and a - * message sequence number. - * The publisher id, message sequence pair is unique with in the process. - * At that point a list of the id's of intra process subscriptions which have - * been registered with the singleton instance of this class are stored with - * the message instance so that delivery is only made to those subscriptions. - * Then an instance of rcl_interfaces/IntraProcessMessage is published to the - * intra process topic which is specific to the topic specified by the user. + * When a Node creates a subscription, it can also create an additional + * wrapper meant to receive intra process messages. + * This structure can be registered with this class. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the subscription. * - * When an instance of rcl_interfaces/IntraProcessMessage is received by a - * subscription, then it is handled by calling take_intra_process_message - * on a singleton of this class. - * The subscription passes a publisher id, message sequence pair which - * uniquely identifies the message instance it was suppose to receive as well - * as the subscriptions unique id. - * If the message is still being held by this class and the subscription's id - * is in the list of intended subscriptions then the message is returned. - * If either of those predicates are not satisfied then the message is not - * returned and the subscription does not call the users callback. + * When a Node creates a publisher, as before this can be registered with this class. + * This is required in order to publish intra-process messages. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the publisher. * - * Since the publisher builds a list of destined subscriptions on publish, and - * other requests are ignored, this class knows how many times a message - * instance should be requested. - * The final time a message is requested, the ownership is passed out of this - * class and passed to the final subscription, effectively freeing space in - * this class's internal storage. + * When a publisher or a subscription are registered with this class, an internal + * structure is updated in order to store which of them can communicate. + * i.e. they have the same topic and compatible QoS. * - * Since a topic is being used to ferry notifications about new intra process - * messages between publishers and subscriptions, it is possible for that - * notification to be lost. - * It is also possible that a subscription which was available when publish was - * called will no longer exist once the notification gets posted. - * In both cases this might result in a message instance getting requested - * fewer times than expected. - * This is why the internal storage of this class is a ring buffer. - * That way if a message is orphaned it will eventually be dropped from storage - * when a new message instance is stored and will not result in a memory leak. + * When the user publishes a message, if intra-process communication is enabled + * on the publisher, the message is handed to this class. + * Using the publisher id, a list of recipients for the message is selected. + * For each item in the list, this class stores its intra-process wrapper. * - * However, since the storage system is finite, this also means that a message - * instance might get displaced by an incoming message instance before all - * interested parties have called take_intra_process_message. - * Because of this the size of the internal storage should be carefully - * considered. + * The wrapper contains a buffer where published intra-process messages are stored + * until the subscription picks them up. + * Depending on the data type stored in the buffer, the subscription intra process + * can request ownership on the inserted messages. * - * /TODO(wjwwood): update to include information about handling latching. - * /TODO(wjwwood): consider thread safety of the class. + * Thus, when an intra-process message is published, this class knows how many + * intra-process subscriptions needs it and how many require ownership. + * This information allows to efficiently perform a minimum number of copies of the message. * * This class is neither CopyConstructable nor CopyAssignable. */ @@ -129,28 +93,25 @@ class IntraProcessManager RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) RCLCPP_PUBLIC - explicit IntraProcessManager( - IntraProcessManagerImplBase::SharedPtr state = create_default_impl()); + IntraProcessManager(); RCLCPP_PUBLIC virtual ~IntraProcessManager(); /// Register a subscription with the manager, returns subscriptions unique id. /** - * In addition to generating a unique intra process id for the subscription, - * this method also stores the topic name of the subscription. + * This method stores the subscription intra process object, together with + * the information of its wrapped subscription (i.e. topic name and QoS). * - * This method is normally called during the creation of a subscription, - * but after it creates the internal intra process rmw_subscription_t. + * In addition this generates a unique intra process id for the subscription. * - * This method will allocate memory. - * - * \param subscription the Subscription to register. + * \param subscription the SubscriptionIntraProcess to register. * \return an unsigned 64-bit integer which is the subscription's unique id. */ RCLCPP_PUBLIC uint64_t - add_subscription(SubscriptionBase::SharedPtr subscription); + add_subscription( + SubscriptionIntraProcessBase::SharedPtr subscription); /// Unregister a subscription using the subscription's unique id. /** @@ -164,32 +125,18 @@ class IntraProcessManager /// Register a publisher with the manager, returns the publisher unique id. /** - * In addition to generating and returning a unique id for the publisher, - * this method creates internal ring buffer storage for "in-flight" intra - * process messages which are stored when store_intra_process_message is - * called with this publisher's unique id. - * - * The buffer_size must be less than or equal to the max uint64_t value. - * If the buffer_size is 0 then a buffer size is calculated using the - * publisher's QoS settings. - * The default is to use the depth field of the publisher's QoS. - * TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar. - * TODO(wjwwood): Consider what to do for keep all. - * - * This method is templated on the publisher's message type so that internal - * storage of the same type can be allocated. + * This method stores the publisher intra process object, together with + * the information of its wrapped publisher (i.e. topic name and QoS). * - * This method will allocate memory. + * In addition this generates a unique intra process id for the publisher. * * \param publisher publisher to be registered with the manager. - * \param buffer_size if 0 (default) a size is calculated based on the QoS. * \return an unsigned 64-bit integer which is the publisher's unique id. */ RCLCPP_PUBLIC uint64_t add_publisher( - rclcpp::PublisherBase::SharedPtr publisher, - size_t buffer_size = 0); + PublisherBase::SharedPtr publisher); /// Unregister a publisher using the publisher's unique id. /** @@ -201,27 +148,20 @@ class IntraProcessManager void remove_publisher(uint64_t intra_process_publisher_id); - /// Store a message in the manager, and return the message sequence number. + /// Publishes an intra-process message, passed as a shared pointer. /** - * The given message is stored in internal storage using the given publisher - * id and the newly generated message sequence, which is also returned. - * The combination of publisher id and message sequence number can later - * be used with a subscription id to retrieve the message by calling - * take_intra_process_message. - * The number of times take_intra_process_message can be called with this - * unique pair of id's is determined by the number of subscriptions currently - * subscribed to the same topic and which share the same Context, i.e. once - * for each subscription which should receive the intra process message. + * This is one of the two methods for publishing intra-process. + * + * Using the intra-process publisher id, a list of recipients is obtained. + * This list is split in half, depending whether they require ownership or not. + * + * This particular method takes a shared pointer as input. + * This can be safely passed to all the subscriptions that do not require ownership. + * No copies are needed in this case. + * For every subscription requiring ownership, a copy has to be made. * - * The ownership of the incoming message is transfered to the internal - * storage in order to avoid copying the message data. - * Therefore, the message parameter will no longer contain the original - * message after calling this method. - * Instead it will either be a nullptr or it will contain the ownership of - * the message instance which was displaced. - * If the message parameter is not equal to nullptr after calling this method - * then a message was prematurely displaced, i.e. take_intra_process_message - * had not been called on it as many times as was expected. + * The total number of copies is always equal to the number + * of subscriptions requiring ownership. * * This method can throw an exception if the publisher id is not found or * if the publisher shared_ptr given to add_publisher has gone out of scope. @@ -230,168 +170,138 @@ class IntraProcessManager * * \param intra_process_publisher_id the id of the publisher of this message. * \param message the message that is being stored. - * \return the message sequence number. */ template< - typename MessageT, typename Alloc = std::allocator> - uint64_t - store_intra_process_message( + typename MessageT, + typename Alloc = std::allocator> + void + do_intra_process_publish( uint64_t intra_process_publisher_id, - std::shared_ptr message) + std::shared_ptr message, + std::shared_ptr::allocator_type> allocator) { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; - uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - throw std::runtime_error("Typecast failed due to incorrect message type"); - } - - // Insert the message into the ring buffer using the message_seq to identify it. - bool did_replace = typed_buffer->push_and_replace(message_seq, message); - // TODO(wjwwood): do something when a message was displaced. log debug? - (void)did_replace; // Avoid unused variable warning. - - impl_->store_intra_process_message(intra_process_publisher_id, message_seq); + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; - // Return the message sequence which is sent to the subscription. - return message_seq; - } + std::shared_lock lock(mutex_); - template< - typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::unique_ptr message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; - uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - throw std::runtime_error("Typecast failed due to incorrect message type"); + 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; } + const auto & sub_ids = publisher_it->second; - // Insert the message into the ring buffer using the message_seq to identify it. - bool did_replace = typed_buffer->push_and_replace(message_seq, std::move(message)); - // TODO(wjwwood): do something when a message was displaced. log debug? - (void)did_replace; // Avoid unused variable warning. + this->template add_shared_msg_to_buffers(message, sub_ids.take_shared_subscriptions); - impl_->store_intra_process_message(intra_process_publisher_id, message_seq); + if (sub_ids.take_ownership_subscriptions.size() > 0) { + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(message); + auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } - // Return the message sequence which is sent to the subscription. - return message_seq; + this->template add_owned_msg_to_buffers( + std::move(unique_msg), + sub_ids.take_ownership_subscriptions, + allocator); + } } - /// Take an intra process message. + /// Publishes an intra-process message, passed as a unique pointer. /** - * The intra_process_publisher_id and message_sequence_number parameters - * uniquely identify a message instance, which should be taken. + * This is one of the two methods for publishing intra-process. * - * The requesting_subscriptions_intra_process_id parameter is used to make - * sure the requesting subscription was intended to receive this message - * instance. - * This check is made because it could happen that the requester - * comes up after the publish event, so it still receives the notification of - * a new intra process message, but it wasn't registered with the manager at - * the time of publishing, causing it to take when it wasn't intended. - * This should be avioded unless latching-like behavior is involved. + * Using the intra-process publisher id, a list of recipients is obtained. + * This list is split in half, depending whether they require ownership or not. * - * The message parameter is used to store the taken message. - * On the last expected call to this method, the ownership is transfered out - * of internal storage and into the message parameter. - * On all previous calls a copy of the internally stored message is made and - * the ownership of the copy is transfered to the message parameter. - * TODO(wjwwood): update this documentation when latching is supported. + * This particular method takes a unique pointer as input. + * The pointer can be promoted to a shared pointer and passed to all the subscriptions + * that do not require ownership. + * In case of subscriptions requiring ownership, the message will be copied for all of + * them except the last one, when ownership can be transferred. * - * The message parameter can be set to nullptr if: + * This method can save an additional copy compared to the shared pointer one. * - * - The publisher id is not found. - * - The message sequence is not found for the given publisher id. - * - The requesting subscription's id is not in the list of intended takers. - * - The requesting subscription's id has been used before with this message. + * This method can throw an exception if the publisher id is not found or + * if the publisher shared_ptr given to add_publisher has gone out of scope. * - * This method may allocate memory to copy the stored message. + * This method does allocate memory. * - * \param intra_process_publisher_id the id of the message's publisher. - * \param message_sequence_number the sequence number of the message. - * \param requesting_subscriptions_intra_process_id the subscription's id. - * \param message the message typed unique_ptr used to return the message. + * \param intra_process_publisher_id the id of the publisher of this message. + * \param message the message that is being stored. */ template< - typename MessageT, typename Alloc = std::allocator, + typename MessageT, + typename Alloc = std::allocator, typename Deleter = std::default_delete> void - take_intra_process_message( + do_intra_process_publish( uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - std::unique_ptr & message) + std::unique_ptr message, + std::shared_ptr::allocator_type> allocator) { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = mapped_ring_buffer::MappedRingBuffer; - message = nullptr; - - size_t target_subs_size = 0; - std::lock_guard lock(take_mutex_); - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message( - intra_process_publisher_id, - message_sequence_number, - requesting_subscriptions_intra_process_id, - target_subs_size - ); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - return; - } - // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left. - if (target_subs_size) { - // There are more subscriptions to serve, return a copy. - typed_buffer->get(message_sequence_number, message); - } else { - // This is the last one to be returned, transfer ownership. - typed_buffer->pop(message_sequence_number, message); - } - } + using MessageAllocTraits = allocator::AllocRebind; - template< - typename MessageT, typename Alloc = std::allocator> - void - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - std::shared_ptr & message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = mapped_ring_buffer::MappedRingBuffer; - message = nullptr; - - size_t target_subs_size = 0; - std::lock_guard lock(take_mutex_); - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message( - intra_process_publisher_id, - message_sequence_number, - requesting_subscriptions_intra_process_id, - target_subs_size - ); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { + 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; } - // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left. - if (target_subs_size) { - // There are more subscriptions to serve, return a copy. - typed_buffer->get(message_sequence_number, message); - } else { - // This is the last one to be returned, transfer ownership. - typed_buffer->pop(message_sequence_number, message); + const auto & sub_ids = publisher_it->second; + + 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); + } 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. + // So we 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( + 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 + std::shared_ptr shared_msg; + auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + shared_msg = std::shared_ptr(ptr); + + 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); } } @@ -400,18 +310,126 @@ class IntraProcessManager bool matches_any_publishers(const rmw_gid_t * id) const; - /// Return the number of intraprocess subscriptions to a topic, given the publisher id. + /// Return the number of intraprocess subscriptions that are matched with a given publisher id. RCLCPP_PUBLIC size_t get_subscription_count(uint64_t intra_process_publisher_id) const; + RCLCPP_PUBLIC + SubscriptionIntraProcessBase::SharedPtr + get_subscription_intra_process(uint64_t intra_process_subscription_id); + private: + struct SubscriptionInfo + { + SubscriptionInfo() = default; + + SubscriptionIntraProcessBase::SharedPtr subscription; + rmw_qos_profile_t qos; + const char * topic_name; + bool use_take_shared_method; + }; + + struct PublisherInfo + { + PublisherInfo() = default; + + PublisherBase::WeakPtr publisher; + rmw_qos_profile_t qos; + const char * topic_name; + }; + + struct SplittedSubscriptions + { + std::vector take_shared_subscriptions; + std::vector take_ownership_subscriptions; + }; + + using SubscriptionMap = std::unordered_map< + uint64_t, SubscriptionInfo>; + + using PublisherMap = std::unordered_map< + uint64_t, PublisherInfo>; + + using PublisherToSubscriptionIdsMap = std::unordered_map< + uint64_t, SplittedSubscriptions>; + RCLCPP_PUBLIC static uint64_t get_next_unique_id(); - IntraProcessManagerImplBase::SharedPtr impl_; - std::mutex take_mutex_; + RCLCPP_PUBLIC + void + insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method); + + RCLCPP_PUBLIC + bool + can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const; + + template + void + add_shared_msg_to_buffers( + std::shared_ptr message, + std::vector subscription_ids) + { + for (auto id : subscription_ids) { + auto subscription_it = subscriptions_.find(id); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + auto subscription = + std::static_pointer_cast>(subscription_base); + + subscription->provide_intra_process_message(message); + } + } + + template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> + void + add_owned_msg_to_buffers( + std::unique_ptr message, + std::vector subscription_ids, + std::shared_ptr::allocator_type> allocator) + { + 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); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.subscription; + + auto subscription = + std::static_pointer_cast>(subscription_base); + + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + 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.get(), 1); + MessageAllocTraits::construct(*allocator.get(), ptr, *message); + copy_message = MessageUniquePtr(ptr, deleter); + + subscription->provide_intra_process_message(std::move(copy_message)); + } + } + } + + PublisherToSubscriptionIdsMap pub_to_subs_; + SubscriptionMap subscriptions_; + PublisherMap publishers_; + + mutable std::shared_timed_mutex mutex_; }; } // namespace intra_process_manager diff --git a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp deleted file mode 100644 index ab29af7b92..0000000000 --- a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp +++ /dev/null @@ -1,358 +0,0 @@ -// Copyright 2015 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__INTRA_PROCESS_MANAGER_IMPL_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rmw/validate_full_topic_name.h" - -#include "rclcpp/macros.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" -#include "rclcpp/publisher_base.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace intra_process_manager -{ - -class IntraProcessManagerImplBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase) - - IntraProcessManagerImplBase() = default; - virtual ~IntraProcessManagerImplBase() = default; - - virtual void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0; - - virtual void - remove_subscription(uint64_t intra_process_subscription_id) = 0; - - virtual void add_publisher( - uint64_t id, - PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) = 0; - - virtual void - remove_publisher(uint64_t intra_process_publisher_id) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) = 0; - - virtual void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size) = 0; - - virtual bool - matches_any_publishers(const rmw_gid_t * id) const = 0; - - virtual size_t - get_subscription_count(uint64_t intra_process_publisher_id) const = 0; - -private: - RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase) -}; - -template> -class IntraProcessManagerImpl : public IntraProcessManagerImplBase -{ -public: - IntraProcessManagerImpl() = default; - ~IntraProcessManagerImpl() = default; - - void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) - { - subscriptions_[id] = subscription; - subscription_ids_by_topic_[fixed_size_string(subscription->get_topic_name())].insert(id); - } - - void - remove_subscription(uint64_t intra_process_subscription_id) - { - subscriptions_.erase(intra_process_subscription_id); - for (auto & pair : subscription_ids_by_topic_) { - pair.second.erase(intra_process_subscription_id); - } - // Iterate over all publisher infos and all stored subscription id's and - // remove references to this subscription's id. - for (auto & publisher_pair : publishers_) { - for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) { - sub_pair.second.erase(intra_process_subscription_id); - } - } - } - - void add_publisher( - uint64_t id, - PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) - { - publishers_[id].publisher = publisher; - // As long as the size of the ring buffer is less than the max sequence number, we're safe. - if (size > std::numeric_limits::max()) { - throw std::invalid_argument("the calculated buffer size is too large"); - } - publishers_[id].sequence_number.store(0); - - publishers_[id].buffer = mrb; - publishers_[id].target_subscriptions_by_message_sequence.reserve(size); - } - - void - remove_publisher(uint64_t intra_process_publisher_id) - { - publishers_.erase(intra_process_publisher_id); - } - - // return message_seq and mrb - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) - { - std::lock_guard lock(runtime_mutex_); - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id"); - } - PublisherInfo & info = it->second; - // Calculate the next message sequence number. - message_seq = info.sequence_number.fetch_add(1); - - return info.buffer; - } - - void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) - { - std::lock_guard lock(runtime_mutex_); - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("store_intra_process_message called with invalid publisher id"); - } - PublisherInfo & info = it->second; - auto publisher = info.publisher.lock(); - if (!publisher) { - throw std::runtime_error("publisher has unexpectedly gone out of scope"); - } - - // Figure out what subscriptions should receive the message. - auto & destined_subscriptions = - subscription_ids_by_topic_[fixed_size_string(publisher->get_topic_name())]; - // Store the list for later comparison. - if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) { - info.target_subscriptions_by_message_sequence.emplace( - message_seq, AllocSet(std::less(), uint64_allocator)); - } else { - info.target_subscriptions_by_message_sequence[message_seq].clear(); - } - std::copy( - destined_subscriptions.begin(), destined_subscriptions.end(), - // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq] - std::inserter( - info.target_subscriptions_by_message_sequence[message_seq], - // This ends up only being a hint to std::set, could also be .begin(). - info.target_subscriptions_by_message_sequence[message_seq].end() - ) - ); - } - - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size - ) - { - std::lock_guard lock(runtime_mutex_); - PublisherInfo * info; - { - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - // Publisher is either invalid or no longer exists. - return 0; - } - info = &it->second; - } - // Figure out how many subscriptions are left. - AllocSet * target_subs; - { - auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number); - if (it == info->target_subscriptions_by_message_sequence.end()) { - // Message is no longer being stored by this publisher. - return 0; - } - target_subs = &it->second; - } - { - auto it = std::find( - target_subs->begin(), target_subs->end(), - requesting_subscriptions_intra_process_id); - if (it == target_subs->end()) { - // This publisher id/message seq pair was not intended for this subscription. - return 0; - } - target_subs->erase(it); - } - size = target_subs->size(); - return info->buffer; - } - - bool - matches_any_publishers(const rmw_gid_t * id) const - { - for (auto & publisher_pair : publishers_) { - auto publisher = publisher_pair.second.publisher.lock(); - if (!publisher) { - continue; - } - if (*publisher.get() == id) { - return true; - } - } - return false; - } - - size_t - get_subscription_count(uint64_t intra_process_publisher_id) const - { - auto publisher_it = publishers_.find(intra_process_publisher_id); - if (publisher_it == publishers_.end()) { - // Publisher is either invalid or no longer exists. - return 0; - } - auto publisher = publisher_it->second.publisher.lock(); - if (!publisher) { - throw std::runtime_error("publisher has unexpectedly gone out of scope"); - } - auto sub_map_it = - subscription_ids_by_topic_.find(fixed_size_string(publisher->get_topic_name())); - if (sub_map_it == subscription_ids_by_topic_.end()) { - // No intraprocess subscribers - return 0; - } - return sub_map_it->second.size(); - } - -private: - RCLCPP_DISABLE_COPY(IntraProcessManagerImpl) - - using FixedSizeString = std::array; - - FixedSizeString - fixed_size_string(const char * str) const - { - FixedSizeString ret; - size_t size = std::strlen(str) + 1; - if (size > ret.size()) { - throw std::runtime_error("failed to copy topic name"); - } - std::memcpy(ret.data(), str, size); - return ret; - } - struct strcmp_wrapper - { - bool - operator()(const FixedSizeString lhs, const FixedSizeString rhs) const - { - return std::strcmp(lhs.data(), rhs.data()) < 0; - } - }; - - template - using RebindAlloc = typename std::allocator_traits::template rebind_alloc; - - RebindAlloc uint64_allocator; - - using AllocSet = std::set, RebindAlloc>; - using SubscriptionMap = std::unordered_map< - uint64_t, SubscriptionBase::WeakPtr, - std::hash, std::equal_to, - RebindAlloc>>; - - using IDTopicMap = std::map< - FixedSizeString, - AllocSet, - strcmp_wrapper, - RebindAlloc>>; - - SubscriptionMap subscriptions_; - - IDTopicMap subscription_ids_by_topic_; - - struct PublisherInfo - { - RCLCPP_DISABLE_COPY(PublisherInfo) - - PublisherInfo() = default; - - PublisherBase::WeakPtr publisher; - std::atomic sequence_number; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer; - - using TargetSubscriptionsMap = std::unordered_map< - uint64_t, AllocSet, - std::hash, std::equal_to, - RebindAlloc>>; - TargetSubscriptionsMap target_subscriptions_by_message_sequence; - }; - - using PublisherMap = std::unordered_map< - uint64_t, PublisherInfo, - std::hash, std::equal_to, - RebindAlloc>>; - - PublisherMap publishers_; - - std::mutex runtime_mutex_; -}; - -RCLCPP_PUBLIC -IntraProcessManagerImplBase::SharedPtr -create_default_impl(); - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index f224abe60a..d13c5d21b9 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -66,6 +66,7 @@ #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \ __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) #define __RCLCPP_SHARED_PTR_ALIAS(...) \ diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp deleted file mode 100644 index c8fdf6493d..0000000000 --- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp +++ /dev/null @@ -1,319 +0,0 @@ -// Copyright 2015 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__MAPPED_RING_BUFFER_HPP_ -#define RCLCPP__MAPPED_RING_BUFFER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/allocator/allocator_common.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace mapped_ring_buffer -{ - -class RCLCPP_PUBLIC MappedRingBufferBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase) -}; - -/// Ring buffer container of shared_ptr's or unique_ptr's of T, which can be accessed by a key. -/** - * T must be a CopyConstructable and CopyAssignable. - * This class can be used in a container by using the base class MappedRingBufferBase. - * This class must have a positive, non-zero size. - * This class cannot be resized nor can it reserve additional space after construction. - * This class is not CopyConstructable nor CopyAssignable. - * - * The key's are not guaranteed to be unique because push_and_replace does not - * check for colliding keys. - * It is up to the user to only use unique keys. - * A side effect of this is that when get_copy_at_key or pop_at_key are called, - * they return the first encountered instance of the key. - * But iteration does not begin with the ring buffer's head, and therefore - * there is no guarantee on which value is returned if a key is used multiple - * times. - */ -template> -class MappedRingBuffer : public MappedRingBufferBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer) - using ElemAllocTraits = allocator::AllocRebind; - using ElemAlloc = typename ElemAllocTraits::allocator_type; - using ElemDeleter = allocator::Deleter; - - using ConstElemSharedPtr = std::shared_ptr; - using ElemUniquePtr = std::unique_ptr; - - /// Constructor. - /** - * The constructor will allocate memory while reserving space. - * - * \param size size of the ring buffer; must be positive and non-zero. - * \param allocator optional custom allocator - */ - explicit MappedRingBuffer(size_t size, std::shared_ptr allocator = nullptr) - : elements_(size), head_(0) - { - if (size == 0) { - throw std::invalid_argument("size must be a positive, non-zero value"); - } - if (!allocator) { - allocator_ = std::make_shared(); - } else { - allocator_ = std::make_shared(*allocator.get()); - } - } - - virtual ~MappedRingBuffer() {} - - /// Return a copy of the value stored in the ring buffer at the given key. - /** - * The key is matched if an element in the ring buffer has a matching key. - * This method will allocate in order to return a copy. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - get(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value = nullptr; - if (it != elements_.end() && it->in_use) { - if (it->unique_value) { - ElemDeleter deleter = it->unique_value.get_deleter(); - auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1); - ElemAllocTraits::construct(*allocator_.get(), ptr, *it->unique_value); - value = ElemUniquePtr(ptr, deleter); - } else if (it->shared_value) { - ElemDeleter * deleter = std::get_deleter(it->shared_value); - auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1); - ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value); - if (deleter) { - value = ElemUniquePtr(ptr, *deleter); - } else { - value = ElemUniquePtr(ptr); - } - } else { - throw std::runtime_error("Unexpected empty MappedRingBuffer element."); - } - } - } - - /// Share ownership of the value stored in the ring buffer at the given key. - /** - * The key is matched if an element in the ring buffer has a matching key. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - get(uint64_t key, ConstElemSharedPtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value.reset(); - if (it != elements_.end() && it->in_use) { - if (!it->shared_value) { - // The stored unique_ptr is upgraded to a shared_ptr here. - // All the remaining get and pop calls done with unique_ptr - // signature will receive a copy. - if (!it->unique_value) { - throw std::runtime_error("Unexpected empty MappedRingBuffer element."); - } - it->shared_value = std::move(it->unique_value); - } - value = it->shared_value; - } - } - - /// Give the ownership of the stored value to the caller if possible, or copy and release. - /** - * The key is matched if an element in the ring buffer has a matching key. - * This method may allocate in order to return a copy. - * - * If the stored value is a shared_ptr, it is not possible to downgrade it to a unique_ptr. - * In that case, a copy is returned and the stored value is released. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - pop(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value = nullptr; - if (it != elements_.end() && it->in_use) { - if (it->unique_value) { - value = std::move(it->unique_value); - } else if (it->shared_value) { - auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1); - ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value); - auto deleter = std::get_deleter(it->shared_value); - if (deleter) { - value = ElemUniquePtr(ptr, *deleter); - } else { - value = ElemUniquePtr(ptr); - } - it->shared_value.reset(); - } else { - throw std::runtime_error("Unexpected empty MappedRingBuffer element."); - } - it->in_use = false; - } - } - - /// Give the ownership of the stored value to the caller, at the given key. - /** - * The key is matched if an element in the ring buffer has a matching key. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - pop(uint64_t key, ConstElemSharedPtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - if (it != elements_.end() && it->in_use) { - if (it->shared_value) { - value = std::move(it->shared_value); - } else if (it->unique_value) { - value = std::move(it->unique_value); - } else { - throw std::runtime_error("Unexpected empty MappedRingBuffer element."); - } - it->in_use = false; - } - } - - /// Insert a key-value pair, displacing an existing pair if necessary. - /** - * The key's uniqueness is not checked on insertion. - * It is up to the user to ensure the key is unique. - * This method should not allocate memory. - * - * After insertion the value will be a nullptr. - * If a pair were replaced, its smart pointer is reset. - * - * \param key the key associated with the value to be stored - * \param value the value to store, and optionally the value displaced - */ - bool - push_and_replace(uint64_t key, ConstElemSharedPtr value) - { - std::lock_guard lock(data_mutex_); - bool did_replace = elements_[head_].in_use; - Element & element = elements_[head_]; - element.key = key; - element.unique_value.reset(); - element.shared_value.reset(); - element.shared_value = value; - element.in_use = true; - head_ = (head_ + 1) % elements_.size(); - return did_replace; - } - - /// Insert a key-value pair, displacing an existing pair if necessary. - /** - * See `bool push_and_replace(uint64_t key, const ConstElemSharedPtr & value)`. - */ - bool - push_and_replace(uint64_t key, ElemUniquePtr value) - { - std::lock_guard lock(data_mutex_); - bool did_replace = elements_[head_].in_use; - Element & element = elements_[head_]; - element.key = key; - element.unique_value.reset(); - element.shared_value.reset(); - element.unique_value = std::move(value); - element.in_use = true; - head_ = (head_ + 1) % elements_.size(); - return did_replace; - } - - /// Return true if the key is found in the ring buffer, otherwise false. - bool - has_key(uint64_t key) - { - std::lock_guard lock(data_mutex_); - return elements_.end() != get_iterator_of_key(key); - } - -private: - RCLCPP_DISABLE_COPY(MappedRingBuffer) - - struct Element - { - uint64_t key; - ElemUniquePtr unique_value; - ConstElemSharedPtr shared_value; - bool in_use; - }; - - using VectorAlloc = typename std::allocator_traits::template rebind_alloc; - - typename std::vector::iterator - get_iterator_of_key(uint64_t key) - { - auto it = std::find_if( - elements_.begin(), elements_.end(), - [key](Element & e) -> bool { - return e.key == key && e.in_use; - }); - return it; - } - - std::vector elements_; - size_t head_; - std::shared_ptr allocator_; - std::mutex data_mutex_; -}; - -} // namespace mapped_ring_buffer -} // namespace rclcpp - -#endif // RCLCPP__MAPPED_RING_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 8209984abc..5a5580e4da 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -68,7 +68,8 @@ class NodeTopics : public NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, + bool use_intra_process) override; RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface * diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 2beb7f0d3c..59657d205c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -27,6 +27,7 @@ #include "rclcpp/publisher_factory.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_factory.hpp" +#include "rclcpp/subscription_intra_process.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -72,7 +73,8 @@ class NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, + bool use_intra_process) = 0; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 9bde46e3df..4140add7f9 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -28,8 +28,6 @@ #include "rcl/error_handling.h" #include "rcl/publisher.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" @@ -127,15 +125,6 @@ class Publisher : public PublisherBase virtual ~Publisher() {} - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const override - { - return mapped_ring_buffer::MappedRingBuffer< - MessageT, - typename Publisher::MessageAllocator - >::make_shared(size, this->get_allocator()); - } - /// Loan memory for a ROS message from the middleware /** * If the middleware is capable of loaning memory for a ROS message instance, @@ -165,7 +154,7 @@ class Publisher : public PublisherBase publish(std::unique_ptr msg) { if (!intra_process_is_enabled_) { - this->do_inter_process_publish(msg.get()); + this->do_inter_process_publish(*msg); return; } // If an interprocess subscription exist, then the unique_ptr is promoted @@ -174,21 +163,15 @@ class Publisher : public PublisherBase // 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. - uint64_t message_seq; bool inter_process_publish_needed = get_subscription_count() > get_intra_process_subscription_count(); - MessageSharedPtr shared_msg; + if (inter_process_publish_needed) { - shared_msg = std::move(msg); - message_seq = - store_intra_process_message(intra_process_publisher_id_, shared_msg); + std::shared_ptr shared_msg = std::move(msg); + this->do_intra_process_publish(shared_msg); + this->do_inter_process_publish(*shared_msg); } else { - message_seq = - store_intra_process_message(intra_process_publisher_id_, std::move(msg)); - } - this->do_intra_process_publish(message_seq); - if (inter_process_publish_needed) { - this->do_inter_process_publish(shared_msg.get()); + this->do_intra_process_publish(std::move(msg)); } } @@ -198,7 +181,7 @@ class Publisher : public PublisherBase // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // In this case we're not using intra process. - return this->do_inter_process_publish(&msg); + return this->do_inter_process_publish(msg); } // 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. @@ -258,9 +241,9 @@ class Publisher : public PublisherBase protected: void - do_inter_process_publish(const MessageT * msg) + do_inter_process_publish(const MessageT & msg) { - auto status = rcl_publish(&publisher_handle_, msg, nullptr); + auto status = rcl_publish(&publisher_handle_, &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { rcl_reset_error(); // next call will reset error message if not context @@ -290,28 +273,6 @@ class Publisher : public PublisherBase } } - void - do_intra_process_publish(uint64_t message_seq) - { - rcl_interfaces::msg::IntraProcessMessage ipm; - ipm.publisher_id = intra_process_publisher_id_; - ipm.message_sequence = message_seq; - auto status = rcl_publish(&intra_process_publisher_handle_, &ipm, nullptr); - if (RCL_RET_PUBLISHER_INVALID == status) { - rcl_reset_error(); // next call will reset error message if not context - if (rcl_publisher_is_valid_except_context(&intra_process_publisher_handle_)) { - rcl_context_t * context = rcl_publisher_get_context(&intra_process_publisher_handle_); - if (nullptr != context && !rcl_context_is_valid(context)) { - // publisher is invalid due to context being shutdown - return; - } - } - } - if (RCL_RET_OK != status) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message"); - } - } - void do_loaned_message_publish(MessageT * msg) { @@ -332,10 +293,8 @@ class Publisher : public PublisherBase } } - uint64_t - store_intra_process_message( - uint64_t publisher_id, - std::shared_ptr msg) + void + do_intra_process_publish(std::shared_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -343,17 +302,17 @@ class Publisher : public PublisherBase "intra process publish called after destruction of intra process manager"); } if (!msg) { - throw std::runtime_error("cannot publisher msg which is a null pointer"); + throw std::runtime_error("cannot publish msg which is a null pointer"); } - uint64_t message_seq = - ipm->template store_intra_process_message(publisher_id, msg); - return message_seq; + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); } - uint64_t - store_intra_process_message( - uint64_t publisher_id, - std::unique_ptr msg) + void + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -361,11 +320,13 @@ class Publisher : public PublisherBase "intra process publish called after destruction of intra process manager"); } if (!msg) { - throw std::runtime_error("cannot publisher msg which is a null pointer"); + throw std::runtime_error("cannot publish msg which is a null pointer"); } - uint64_t message_seq = - ipm->template store_intra_process_message(publisher_id, std::move(msg)); - return message_seq; + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); } /// Copy of original options passed during construction. diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index c5837f670e..c6743188b8 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -28,7 +28,6 @@ #include "rcl/publisher.h" #include "rclcpp/macros.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" @@ -97,12 +96,6 @@ class PublisherBase : public std::enable_shared_from_this const rmw_gid_t & get_gid() const; - /// Get the global identifier for this publisher used by intra-process communication. - /** \return The intra-process gid. */ - RCLCPP_PUBLIC - const rmw_gid_t & - get_intra_process_gid() const; - /// Get the rcl publisher handle. /** \return The rcl publisher handle. */ RCLCPP_PUBLIC @@ -193,19 +186,12 @@ class PublisherBase : public std::enable_shared_from_this using IntraProcessManagerSharedPtr = std::shared_ptr; - /// Implementation utility function that creates a typed mapped ring buffer. - RCLCPP_PUBLIC - virtual - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const; - /// Implementation utility function used to setup intra process publishing after creation. RCLCPP_PUBLIC void setup_intra_process( uint64_t intra_process_publisher_id, - IntraProcessManagerSharedPtr ipm, - const rcl_publisher_options_t & intra_process_options); + IntraProcessManagerSharedPtr ipm); protected: template @@ -225,7 +211,6 @@ class PublisherBase : public std::enable_shared_from_this std::shared_ptr rcl_node_handle_; rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); - rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); std::vector> event_handlers_; @@ -236,7 +221,6 @@ class PublisherBase : public std::enable_shared_from_this uint64_t intra_process_publisher_id_; rmw_gid_t rmw_gid_; - rmw_gid_t intra_process_rmw_gid_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1ada13491e..973b0941cf 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -167,16 +167,15 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy group->find_subscription_ptrs_if( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); - if (subscription->get_intra_process_subscription_handle()) { - subscription_handles_.push_back( - subscription->get_intra_process_subscription_handle()); - } return false; }); + group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) { + if (service) { service_handles_.push_back(service->get_service_handle()); return false; - }); + } + }); group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); return false; @@ -262,11 +261,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy while (it != subscription_handles_.end()) { auto subscription = get_subscription_by_handle(*it, weak_nodes); if (subscription) { - // Figure out if this is for intra-process or not. - bool is_intra_process = false; - if (subscription->get_intra_process_subscription_handle()) { - is_intra_process = subscription->get_intra_process_subscription_handle() == *it; - } // Find the group for this handle and see if it can be serviced auto group = get_group_by_subscription(subscription, weak_nodes); if (!group) { @@ -282,11 +276,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy continue; } // Otherwise it is safe to set and return the any_exec - if (is_intra_process) { - any_exec.subscription_intra_process = subscription; - } else { - any_exec.subscription = subscription; - } + any_exec.subscription = subscription; any_exec.callback_group = group; any_exec.node_base = get_node_by_group(group, weak_nodes); subscription_handles_.erase(it); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 3789b05679..aaff8eeb3a 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,13 +29,10 @@ #include "rcl/error_handling.h" #include "rcl/subscription.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -210,117 +207,13 @@ class Subscription : public SubscriptionBase message_memory_strategy_->return_serialized_message(message); } - void handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) override - { - if (!use_intra_process_) { - // throw std::runtime_error( - // "handle_intra_process_message called before setup_intra_process"); - // TODO(wjwwood): for now, this could mean that intra process was just not enabled. - // However, this can only really happen if this node has it disabled, but the other doesn't. - return; - } - - if (!matches_any_intra_process_publishers(&message_info.publisher_gid)) { - // This intra-process message has not been created by a publisher from this context. - // we should ignore this copy of the message. - return; - } - - if (any_callback_.use_take_shared_method()) { - ConstMessageSharedPtr msg; - take_intra_process_message( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); - if (!msg) { - // This can happen when having two nodes in different process both using intraprocess - // communication. It could happen too if the publisher no longer exists or the requested - // message is not longer being stored. - // TODO(ivanpauno): Print a warn message in the last two cases described above, - // but not in the first one. - return; - } - any_callback_.dispatch_intra_process(msg, message_info); - } else { - MessageUniquePtr msg; - take_intra_process_message( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); - if (!msg) { - // This can happen when having two nodes in different process both using intraprocess - // communication. It could happen too if the publisher no longer exists or the requested - // message is not longer being stored. - // TODO(ivanpauno): Print a warn message in the last two cases described above, - // but not in the first one. - return; - } - any_callback_.dispatch_intra_process(std::move(msg), message_info); - } - } - - /// Implemenation detail. - const std::shared_ptr - get_intra_process_subscription_handle() const override - { - if (!use_intra_process_) { - return nullptr; - } - return intra_process_subscription_handle_; - } - -private: - void - take_intra_process_message( - uint64_t publisher_id, - uint64_t message_sequence, - uint64_t subscription_id, - MessageUniquePtr & message) - { - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process take called after destruction of intra process manager"); - } - ipm->template take_intra_process_message( - publisher_id, message_sequence, subscription_id, message); - } - - void - take_intra_process_message( - uint64_t publisher_id, - uint64_t message_sequence, - uint64_t subscription_id, - ConstMessageSharedPtr & message) - { - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process take called after destruction of intra process manager"); - } - ipm->template take_intra_process_message( - publisher_id, message_sequence, subscription_id, message); - } - bool - matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) + use_take_shared_method() const { - if (!use_intra_process_) { - return false; - } - auto ipm = weak_ipm_.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publisher check called " - "after destruction of intra process manager"); - } - return ipm->matches_any_publishers(sender_gid); + return any_callback_.use_take_shared_method(); } +private: RCLCPP_DISABLE_COPY(Subscription) AnySubscriptionCallback any_callback_; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f3df9974b3..b029fcacff 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -21,8 +21,6 @@ #include "rcl/subscription.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - #include "rmw/rmw.h" #include "rclcpp/any_subscription_callback.hpp" @@ -89,10 +87,6 @@ class SubscriptionBase : public std::enable_shared_from_this const std::shared_ptr get_subscription_handle() const; - RCLCPP_PUBLIC - virtual const std::shared_ptr - get_intra_process_subscription_handle() const; - /// Get all the QoS event handlers associated with this subscription. /** \return The vector of QoS event handlers. */ RCLCPP_PUBLIC @@ -158,13 +152,6 @@ class SubscriptionBase : public std::enable_shared_from_this void return_serialized_message(std::shared_ptr & message) = 0; - RCLCPP_PUBLIC - virtual - void - handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) = 0; - RCLCPP_PUBLIC const rosidl_message_type_support_t & get_message_type_support_handle() const; @@ -198,8 +185,11 @@ class SubscriptionBase : public std::enable_shared_from_this void setup_intra_process( uint64_t intra_process_subscription_id, - IntraProcessManagerWeakPtr weak_ipm, - const rcl_subscription_options_t & intra_process_options); + IntraProcessManagerWeakPtr weak_ipm); + + RCLCPP_PUBLIC + uint64_t + get_intra_process_id() const; protected: template @@ -216,6 +206,10 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_.emplace_back(handler); } + RCLCPP_PUBLIC + bool + matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a5175c85cb..77ac4e3a5f 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -26,6 +26,7 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_traits.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/subscription_intra_process.hpp b/rclcpp/include/rclcpp/subscription_intra_process.hpp new file mode 100644 index 0000000000..845fc8e12b --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_intra_process.hpp @@ -0,0 +1,157 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ + +#include + +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/buffers/intra_process_buffer.hpp" +#include "rclcpp/create_intra_process_buffer.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_intra_process_base.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + typename CallbackMessageT = MessageT> +class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using BufferUniquePtr = + typename intra_process_buffer::IntraProcessBuffer::UniquePtr; + + SubscriptionIntraProcess( + AnySubscriptionCallback callback, + std::shared_ptr allocator, + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + rmw_qos_profile_t qos_profile, + rclcpp::IntraProcessBufferType buffer_type) + : SubscriptionIntraProcessBase(topic_name, qos_profile), + any_callback_(callback) + { + if (!std::is_same::value) { + throw std::runtime_error("SubscriptionIntraProcess wrong callback type"); + } + + // Create the intra-process buffer. + buffer_ = rclcpp::create_intra_process_buffer( + buffer_type, + qos_profile, + allocator); + + // Create the guard condition. + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options(); + + gc_ = rcl_get_zero_initialized_guard_condition(); + rcl_ret_t ret = rcl_guard_condition_init( + &gc_, context->get_rcl_context().get(), guard_condition_options); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition"); + } + } + + bool + is_ready(rcl_wait_set_t * wait_set) + { + (void)wait_set; + return buffer_->has_data(); + } + + void execute() + { + execute_impl(); + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) + { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } + + void + provide_intra_process_message(MessageUniquePtr message) + { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } + + bool + use_take_shared_method() const + { + return buffer_->use_take_shared_method(); + } + +private: + void + trigger_guard_condition() + { + rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); + (void)ret; + } + + template + typename std::enable_if::value, void>::type + execute_impl() + { + throw std::runtime_error("Subscription intra-process can't handle serialized messages"); + } + + template + typename std::enable_if::value, void>::type + execute_impl() + { + rmw_message_info_t msg_info; + msg_info.from_intra_process = true; + + if (any_callback_.use_take_shared_method()) { + ConstMessageSharedPtr msg = buffer_->consume_shared(); + any_callback_.dispatch_intra_process(msg, msg_info); + } else { + MessageUniquePtr msg = buffer_->consume_unique(); + any_callback_.dispatch_intra_process(std::move(msg), msg_info); + } + } + + AnySubscriptionCallback any_callback_; + BufferUniquePtr buffer_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/subscription_intra_process_base.hpp new file mode 100644 index 0000000000..3eb725ab5f --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_intra_process_base.hpp @@ -0,0 +1,84 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +class SubscriptionIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + RCLCPP_PUBLIC + SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile) + : topic_name_(topic_name), qos_profile_(qos_profile) + {} + + virtual ~SubscriptionIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + bool + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual void + execute() = 0; + + virtual bool + use_take_shared_method() const = 0; + + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + RCLCPP_PUBLIC + rmw_qos_profile_t + get_actual_qos() const; + +protected: + std::recursive_mutex reentrant_mutex_; + rcl_guard_condition_t gc_; + +private: + virtual void + trigger_guard_condition() = 0; + + std::string topic_name_; + rmw_qos_profile_t qos_profile_; +}; +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 8ee6e7ffb4..64fcac5f12 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -21,6 +21,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" @@ -44,6 +45,9 @@ struct SubscriptionOptionsBase /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + /// Setting the data-type stored in the intraprocess buffer + IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::CallbackDefault; + /// Optional RMW implementation specific payload to be used during creation of the subscription. std::shared_ptr rmw_implementation_payload = nullptr; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 48f8405b86..6953610007 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -287,9 +287,6 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (any_exec.subscription) { execute_subscription(any_exec.subscription); } - if (any_exec.subscription_intra_process) { - execute_intra_process_subscription(any_exec.subscription_intra_process); - } if (any_exec.service) { execute_service(any_exec.service); } @@ -375,30 +372,6 @@ Executor::execute_subscription( } } -void -Executor::execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rcl_interfaces::msg::IntraProcessMessage ipm; - rmw_message_info_t message_info; - rcl_ret_t status = rcl_take( - subscription->get_intra_process_subscription_handle().get(), - &ipm, - &message_info, - nullptr); - - if (status == RCL_RET_OK) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take failed for intra process subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } -} - void Executor::execute_timer( rclcpp::TimerBase::SharedPtr timer) @@ -565,7 +538,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) } // Check the subscriptions to see if there are any that are ready memory_strategy_->get_next_subscription(any_executable, weak_nodes_); - if (any_executable.subscription || any_executable.subscription_intra_process) { + if (any_executable.subscription) { return true; } // Check the services to see if there are any that are ready diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 56e30ab3c0..4fd756b7f0 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -21,9 +21,7 @@ namespace intra_process_manager static std::atomic _next_unique_id {1}; -IntraProcessManager::IntraProcessManager( - rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl) -: impl_(impl) +IntraProcessManager::IntraProcessManager() {} IntraProcessManager::~IntraProcessManager() @@ -31,50 +29,133 @@ IntraProcessManager::~IntraProcessManager() uint64_t IntraProcessManager::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher, - size_t buffer_size) + rclcpp::PublisherBase::SharedPtr publisher) { + std::unique_lock lock(mutex_); + auto id = IntraProcessManager::get_next_unique_id(); - size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size(); - auto mrb = publisher->make_mapped_ring_buffer(size); - impl_->add_publisher(id, publisher, mrb, size); - if (!mrb) { - throw std::runtime_error("failed to create a mapped ring buffer"); + + publishers_[id].publisher = publisher; + publishers_[id].topic_name = publisher->get_topic_name(); + publishers_[id].qos = publisher->get_actual_qos(); + + // Initialize the subscriptions storage for this publisher. + pub_to_subs_[id] = SplittedSubscriptions(); + + // create an entry for the publisher id and populate with already existing subscriptions + for (auto & pair : subscriptions_) { + if (can_communicate(publishers_[id], pair.second)) { + insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method); + } } + return id; } uint64_t IntraProcessManager::add_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) + SubscriptionIntraProcessBase::SharedPtr subscription) { + std::unique_lock lock(mutex_); + auto id = IntraProcessManager::get_next_unique_id(); - impl_->add_subscription(id, subscription); + + subscriptions_[id].subscription = subscription; + subscriptions_[id].topic_name = subscription->get_topic_name(); + subscriptions_[id].qos = subscription->get_actual_qos(); + subscriptions_[id].use_take_shared_method = + subscription->use_take_shared_method(); + + // adds the subscription id to all the matchable publishers + for (auto & pair : publishers_) { + if (can_communicate(pair.second, subscriptions_[id])) { + insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method); + } + } + return id; } void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { - impl_->remove_subscription(intra_process_subscription_id); + std::unique_lock lock(mutex_); + + subscriptions_.erase(intra_process_subscription_id); + + for (auto & pair : pub_to_subs_) { + pair.second.take_shared_subscriptions.erase(std::remove( + pair.second.take_shared_subscriptions.begin(), + pair.second.take_shared_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_shared_subscriptions.end()); + + pair.second.take_ownership_subscriptions.erase(std::remove( + pair.second.take_ownership_subscriptions.begin(), + pair.second.take_ownership_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_ownership_subscriptions.end()); + } } void IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) { - impl_->remove_publisher(intra_process_publisher_id); + std::unique_lock lock(mutex_); + + publishers_.erase(intra_process_publisher_id); + pub_to_subs_.erase(intra_process_publisher_id); } bool IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const { - return impl_->matches_any_publishers(id); + std::shared_lock lock(mutex_); + + for (auto & publisher_pair : publishers_) { + auto publisher = publisher_pair.second.publisher.lock(); + if (!publisher) { + continue; + } + if (*publisher.get() == id) { + return true; + } + } + return false; } size_t IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const { - return impl_->get_subscription_count(intra_process_publisher_id); + 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 get_subscription_count for invalid or no longer existing publisher id"); + return 0; + } + + auto count = + publisher_it->second.take_shared_subscriptions.size() + + publisher_it->second.take_ownership_subscriptions.size(); + + return count; +} + +SubscriptionIntraProcessBase::SharedPtr +IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subscription_id) +{ + std::shared_lock lock(mutex_); + + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it == subscriptions_.end()) { + return nullptr; + } else { + return subscription_it->second.subscription; + } } uint64_t @@ -99,5 +180,44 @@ IntraProcessManager::get_next_unique_id() return next_id; } +void +IntraProcessManager::insert_sub_id_for_pub( + uint64_t sub_id, + uint64_t pub_id, + bool use_take_shared_method) +{ + if (use_take_shared_method) { + pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id); + } else { + pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id); + } +} + +bool +IntraProcessManager::can_communicate( + PublisherInfo pub_info, + SubscriptionInfo sub_info) const +{ + // publisher and subscription must be on the same topic + if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) { + return false; + } + + // TODO(alsora): the following checks for qos compatibility should be provided by the RMW + // a reliable subscription can't be connected with a best effort publisher + if (sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE && + pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) + { + return false; + } + + // a publisher and a subscription with different durability can't communicate + if (sub_info.qos.durability != pub_info.qos.durability) { + return false; + } + + return true; +} + } // namespace intra_process_manager } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 26c3e3c98c..5a2a5da313 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -34,9 +34,7 @@ MemoryStrategy::get_subscription_by_handle( } auto match_subscription = group->find_subscription_ptrs_if( [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool { - return - (subscription->get_subscription_handle() == subscriber_handle) || - (subscription->get_intra_process_subscription_handle() == subscriber_handle); + return subscription->get_subscription_handle() == subscriber_handle; }); if (match_subscription) { return match_subscription; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 74e034ed03..518824bd51 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -82,7 +82,8 @@ NodeTopics::create_subscription( void NodeTopics::add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, + bool use_intra_process) { // Assign to a group. if (callback_group) { @@ -99,6 +100,20 @@ NodeTopics::add_subscription( callback_group->add_waitable(subscription_event); } + if (use_intra_process) { + // Get the intra process manager for this context. + auto context = node_base_->get_context(); + auto ipm = + context->get_sub_context(); + + // Use the id to retrieve the subscription intra-process from the intra-process manager. + auto subscription_intra_process = + ipm->get_subscription_intra_process(subscription->get_intra_process_id()); + + // Add to the callback group to be notified about intra-process msgs. + callback_group->add_waitable(subscription_intra_process); + } + // Notify the executor that a new subscription was created using the parent Node. { auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 38d3995ed3..0c489c51d4 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -84,14 +84,6 @@ PublisherBase::~PublisherBase() // must fini the events before fini-ing the publisher event_handlers_.clear(); - if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of intra process rcl publisher handle: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -139,12 +131,6 @@ PublisherBase::get_gid() const return rmw_gid_; } -const rmw_gid_t & -PublisherBase::get_intra_process_gid() const -{ - return intra_process_rmw_gid_; -} - rcl_publisher_t * PublisherBase::get_publisher_handle() { @@ -246,81 +232,15 @@ PublisherBase::operator==(const rmw_gid_t * gid) const rmw_reset_error(); throw std::runtime_error(msg); } - if (!result) { - ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); - if (ret != RMW_RET_OK) { - auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str; - rmw_reset_error(); - throw std::runtime_error(msg); - } - } return result; } -rclcpp::mapped_ring_buffer::MappedRingBufferBase::SharedPtr -PublisherBase::make_mapped_ring_buffer(size_t size) const -{ - (void)size; - return nullptr; -} - void PublisherBase::setup_intra_process( uint64_t intra_process_publisher_id, - IntraProcessManagerSharedPtr ipm, - const rcl_publisher_options_t & intra_process_options) + IntraProcessManagerSharedPtr ipm) { - // Intraprocess configuration is not allowed with "durability" qos policy non "volatile". - auto actual_durability = this->get_actual_qos().get_rmw_qos_profile().durability; - if (actual_durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { - throw std::invalid_argument( - "intraprocess communication is not allowed with durability qos policy non-volatile"); - } - const char * topic_name = this->get_topic_name(); - if (!topic_name) { - throw std::runtime_error("failed to get topic name"); - } - - auto intra_process_topic_name = std::string(topic_name) + "/_intra"; - - rcl_ret_t ret = rcl_publisher_init( - &intra_process_publisher_handle_, - rcl_node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - intra_process_topic_name.c_str(), - &intra_process_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = rcl_node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - intra_process_topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } - - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher"); - } - intra_process_publisher_id_ = intra_process_publisher_id; weak_ipm_ = ipm; intra_process_is_enabled_ = true; - - // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle( - &intra_process_publisher_handle_); - if (publisher_rmw_handle == nullptr) { - auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string().str; - rcl_reset_error(); - throw std::runtime_error(msg); - } - auto rmw_ret = rmw_get_gid_for_publisher( - publisher_rmw_handle, &intra_process_rmw_gid_); - if (rmw_ret != RMW_RET_OK) { - auto msg = - std::string("failed to create intra process publisher gid: ") + rmw_get_error_string().str; - rmw_reset_error(); - throw std::runtime_error(msg); - } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 99558d2562..1d0bb13e81 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -58,10 +58,6 @@ SubscriptionBase::SubscriptionBase( new rcl_subscription_t, custom_deletor); *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - intra_process_subscription_handle_ = std::shared_ptr( - new rcl_subscription_t, custom_deletor); - *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription(); - rcl_ret_t ret = rcl_subscription_init( subscription_handle_.get(), node_handle_.get(), @@ -117,12 +113,6 @@ SubscriptionBase::get_subscription_handle() const return subscription_handle_; } -const std::shared_ptr -SubscriptionBase::get_intra_process_subscription_handle() const -{ - return intra_process_subscription_handle_; -} - const std::vector> & SubscriptionBase::get_event_handlers() const { @@ -172,30 +162,8 @@ SubscriptionBase::get_publisher_count() const void SubscriptionBase::setup_intra_process( uint64_t intra_process_subscription_id, - IntraProcessManagerWeakPtr weak_ipm, - const rcl_subscription_options_t & intra_process_options) + IntraProcessManagerWeakPtr weak_ipm) { - std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; - rcl_ret_t ret = rcl_subscription_init( - intra_process_subscription_handle_.get(), - node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - intra_process_topic_name.c_str(), - &intra_process_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - intra_process_topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } - - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription"); - } - intra_process_subscription_id_ = intra_process_subscription_id; weak_ipm_ = weak_ipm; use_intra_process_ = true; @@ -206,3 +174,24 @@ SubscriptionBase::can_loan_messages() const { return rcl_subscription_can_loan_messages(subscription_handle_.get()); } + +uint64_t +SubscriptionBase::get_intra_process_id() const +{ + return intra_process_subscription_id_; +} + +bool +SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const +{ + if (!use_intra_process_) { + return false; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publisher check called " + "after destruction of intra process manager"); + } + return ipm->matches_any_publishers(sender_gid); +} diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp new file mode 100644 index 0000000000..15880ce568 --- /dev/null +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -0,0 +1,38 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/subscription_intra_process_base.hpp" + +using rclcpp::SubscriptionIntraProcessBase; + +bool +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(reentrant_mutex_); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); + return RCL_RET_OK == ret; +} + +const char * +SubscriptionIntraProcessBase::get_topic_name() const +{ + return topic_name_.c_str(); +} + +rmw_qos_profile_t +SubscriptionIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 4d74d70830..d9820442a9 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -249,6 +249,13 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); for (auto & it : filter.get_events()) { +<<<<<<< HEAD +======= + if (it.second->value.type != ParameterType::PARAMETER_BOOL) { + RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); + continue; + } +>>>>>>> basic ipc implementation from alsora/new_ipc_proposal if (it.second->value.bool_value) { parameter_state_ = SET_TRUE; enable_ros_time(); diff --git a/rclcpp/test/test_intra_process_buffer.cpp b/rclcpp/test/test_intra_process_buffer.cpp new file mode 100644 index 0000000000..724bbf979c --- /dev/null +++ b/rclcpp/test/test_intra_process_buffer.cpp @@ -0,0 +1,246 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +/* + Construtctor + */ +TEST(TestIntraProcessBuffer, constructor) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using UniqueIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + + auto shared_buffer_impl = + std::make_unique>(2); + + auto shared_intra_process_buffer = + SharedIntraProcessBufferT(std::move(shared_buffer_impl)); + + EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method()); + + auto unique_buffer_impl = + std::make_unique>(2); + + auto unique_intra_process_buffer = + UniqueIntraProcessBufferT(std::move(unique_buffer_impl)); + + EXPECT_EQ(false, unique_intra_process_buffer.use_take_shared_method()); +} + +/* + Add data to an intra-process buffer with an implementations that stores shared_ptr + Messages are extracted using the same data as the implementation, i.e. shared_ptr + - Add shared_ptr no copies are expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using SharedIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + + auto buffer_impl = + std::make_unique>(2); + + auto intra_process_buffer = + SharedIntraProcessBufferT(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2u, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_shared_msg = intra_process_buffer.consume_shared(); + popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(1u, popped_shared_msg.use_count()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Add data to an intra-process buffer with an implementations that stores unique_ptr + Messages are extracted using the same data as the implementation, i.e. unique_ptr + - Add shared_ptr a copy is expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + + auto buffer_impl = + std::make_unique>(2); + + auto intra_process_buffer = + UniqueIntraProcessBufferT(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(1u, original_shared_msg.use_count()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + auto popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Consume data from an intra-process buffer with an implementations that stores shared_ptr + Messages are inserted using the same data as the implementation, i.e. shared_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr a copy is expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + + auto buffer_impl = + std::make_unique>(2); + + auto intra_process_buffer = + SharedIntraProcessBufferT(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2u, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('b'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(1u, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); +} + +/* + Consume data from an intra-process buffer with an implementations that stores unique_ptr + Messages are inserted using the same data as the implementation, i.e. unique_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = + rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + + auto buffer_impl = + std::make_unique>(2); + + auto intra_process_buffer = + UniqueIntraProcessBufferT(std::move(buffer_impl)); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index fe3c535107..72ff4e20ce 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -12,60 +12,89 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include #include #include #include +#include #define RCLCPP_BUILDING_LIBRARY 1 -#include "gtest/gtest.h" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" #include "rmw/types.h" +#include "rmw/qos_profiles.h" + +// NOLINTNEXTLINE(build/include_order) +#include -// Mock up publisher and subscription base to avoid needing an rmw impl. namespace rclcpp { +// forward declaration +namespace intra_process_manager +{ +class IntraProcessManager; +} + namespace mock { +using IntraProcessManagerSharedPtr = + std::shared_ptr; + +using IntraProcessManagerWeakPtr = + std::weak_ptr; + class PublisherBase { public: RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) PublisherBase() - : mock_topic_name(""), mock_queue_size(0) {} + : topic_name("topic") + {} virtual ~PublisherBase() {} const char * get_topic_name() const { - return mock_topic_name.c_str(); + return topic_name.c_str(); } - size_t get_queue_size() const + + void set_intra_process_manager( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm) { - return mock_queue_size; + intra_process_publisher_id_ = intra_process_publisher_id; + weak_ipm_ = ipm; + } + + rmw_qos_profile_t + get_actual_qos() + { + return qos_profile; } bool - operator==(const rmw_gid_t * gid) const + operator==(const rmw_gid_t & gid) const { (void)gid; return false; } - virtual - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const + bool + operator==(const rmw_gid_t * gid) const { - (void)size; - return nullptr; + (void)gid; + return false; } - std::string mock_topic_name; - size_t mock_queue_size; + rmw_qos_profile_t qos_profile; + std::string topic_name; + uint64_t intra_process_publisher_id_; + IntraProcessManagerWeakPtr weak_ipm_; }; template> @@ -76,28 +105,23 @@ class Publisher : public PublisherBase using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; using MessageUniquePtr = std::unique_ptr; - std::shared_ptr allocator_; + using MessageSharedPtr = std::shared_ptr; RCLCPP_SMART_PTR_DEFINITIONS(Publisher) Publisher() { - allocator_ = std::make_shared(); + qos_profile = rmw_qos_profile_default; + auto allocator = std::make_shared(); + message_allocator_ = std::make_shared(*allocator.get()); } - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const override - { - return mapped_ring_buffer::MappedRingBuffer< - T, - typename Publisher::MessageAlloc - >::make_shared(size, allocator_); - } + // The following functions use the IntraProcessManager + // so they are declared after including it to avoid "invalid use of incomplete type" + void publish(MessageUniquePtr msg); + void publish(MessageSharedPtr msg); - std::shared_ptr get_allocator() - { - return allocator_; - } + std::shared_ptr message_allocator_; }; } // namespace mock @@ -105,756 +129,554 @@ class Publisher : public PublisherBase namespace rclcpp { +namespace intra_process_buffer +{ namespace mock { - -class SubscriptionBase +template +class IntraProcessBuffer { public: - RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionBase) + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; - SubscriptionBase() - : mock_topic_name(""), mock_queue_size(0) {} + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) - const char * get_topic_name() const + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) { - return mock_topic_name.c_str(); + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; } - size_t get_queue_size() const + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + } + + void pop(std::uintptr_t & msg_ptr) { - return mock_queue_size; + msg_ptr = message_ptr; + message_ptr = 0; } - std::string mock_topic_name; - size_t mock_queue_size; + // need to store the messages somewhere otherwise the memory address will be reused + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + std::uintptr_t message_ptr; }; } // namespace mock +} // namespace intra_process_buffer } // namespace rclcpp -// Prevent rclcpp/publisher_base.hpp and rclcpp/subscription.hpp from being imported. -#define RCLCPP__PUBLISHER_BASE_HPP_ -#define RCLCPP__SUBSCRIPTION_BASE_HPP_ -// Force ipm to use our mock publisher class. -#define Publisher mock::Publisher -#define PublisherBase mock::PublisherBase -#define SubscriptionBase mock::SubscriptionBase -#include "../src/rclcpp/intra_process_manager.cpp" -#include "../src/rclcpp/intra_process_manager_impl.cpp" -#undef SubscriptionBase -#undef Publisher -#undef PublisherBase -// NOLINTNEXTLINE(build/include_order) -#include +namespace rclcpp +{ +namespace mock +{ -/* - This tests the "normal" usage of the class: - - Creates two publishers on different topics. - - Creates a subscription which matches one of them. - - Publishes on each publisher with different message content. - - Try's to take the message from the non-matching publish, should fail. - - Try's to take the message from the matching publish, should work. - - Asserts the message it got back was the one that went in (since there's only one subscription). - - Try's to take the message again, should fail. - */ -TEST(TestIntraProcessManager, nominal) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal2"; - p2->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto p2_id = ipm.add_publisher(p2); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_original_address = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto p2_m1_id = ipm.store_intra_process_message(p2_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail since p2 and s1 don't have the same topic. - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(p1_m1_original_address, unique_msg.get()); - } +class SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + SubscriptionIntraProcessBase() + : qos_profile(rmw_qos_profile_default), topic_name("topic") + {} + + virtual bool + use_take_shared_method() const = 0; - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail, since the message was already taken. + rmw_qos_profile_t + get_actual_qos() + { + return qos_profile; + } - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + const char * + get_topic_name() + { + return topic_name; + } - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); + rmw_qos_profile_t qos_profile; + const char * topic_name; +}; - ipm_msg->message_sequence = 45; - ipm_msg->publisher_id = 45; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); +template +class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); + SubscriptionIntraProcess() + : take_shared_method(false) + { + buffer = std::make_unique>(); + } - ipm_msg->message_sequence = 46; - ipm_msg->publisher_id = 46; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + void + provide_intra_process_message(std::shared_ptr msg) + { + buffer->add(msg); + } - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); -} + void + provide_intra_process_message(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } -/* - Simulates the case where a publisher is removed between publishing and the matching take. - - Creates a publisher and subscription on the same topic. - - Publishes a message. - - Remove the publisher. - - Try's to take the message, should fail since the publisher (and its storage) is gone. - */ -TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; + std::uintptr_t + pop() + { + std::uintptr_t ptr; + buffer->pop(ptr); + return ptr; + } - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; + bool + use_take_shared_method() const + { + return take_shared_method; + } - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; + bool take_shared_method; - auto p1_id = ipm.add_publisher(p1); - auto s1_id = ipm.add_subscription(s1); + typename intra_process_buffer::mock::IntraProcessBuffer::UniquePtr buffer; +}; - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); +} // namespace mock +} // namespace rclcpp - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); +// Prevent the header files of the mocked classes to be included +#define RCLCPP__PUBLISHER_HPP_ +#define RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +// Force ipm to use our mock publisher class. +#define Publisher mock::Publisher +#define PublisherBase mock::PublisherBase +#define IntraProcessBuffer mock::IntraProcessBuffer +#define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase +#define SubscriptionIntraProcess mock::SubscriptionIntraProcess +#include "../src/rclcpp/intra_process_manager.cpp" +#undef Publisher +#undef PublisherBase +#undef IntraProcessBuffer +#undef SubscriptionIntraProcessBase +#undef SubscriptionIntraProcess - ipm.remove_publisher(p1_id); +using ::testing::_; +using ::testing::UnorderedElementsAre; - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail, since the publisher is gone. -} +namespace rclcpp +{ +namespace mock +{ -/* - Tests whether or not removed subscriptions affect take behavior. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message, keep the original point for later comparison. - - Take with one subscription, should work. - - Remove a different subscription. - - Take with the final subscription, should work. - - Assert the previous take returned ownership of the original object published. - */ -TEST(TestIntraProcessManager, removed_subscription_affects_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); +template +void Publisher::publish(MessageUniquePtr msg) +{ + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); } - unique_msg.reset(); - - ipm.remove_subscription(s2_id); - - // Take using s3, the remaining subscription. - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - // Should match the original pointer since s2 was removed first. - EXPECT_EQ(original_message_pointer, unique_msg.get()); + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); } - // Take using s2, should fail since s2 was removed. - unique_msg.reset(); - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); } -/* - This tests normal operation with multiple subscriptions and one publisher. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. - */ -TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); +// This function is actually deprecated in the real rclcpp::publisher, but +// here it is used to mimic what happens when publishing both inter and intra-process +template +void Publisher::publish(MessageSharedPtr msg) +{ + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - // Should match the original pointer. - EXPECT_EQ(original_message_pointer, unique_msg.get()); + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); } -} -/* - This tests normal operation with multiple publishers and one subscription. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. - */ -TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal1"; - p2->mock_queue_size = 10; - - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); - p3->mock_topic_name = "nominal1"; - p3->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto p2_id = ipm.add_publisher(p2); - auto p3_id = ipm.add_publisher(p3); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - // First publish - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer1 = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // Second publish - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p2_m1_id = ipm.store_intra_process_message(p2_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // Third publish - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer3 = unique_msg.get(); - auto p3_m1_id = ipm.store_intra_process_message(p3_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // First take - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - // Second take - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - // Third take - ipm.take_intra_process_message(p3_id, p3_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + msg, + message_allocator_); } -/* - This tests normal operation with multiple publishers and subscriptions. - - Creates three publishers and three subscriptions on the same topic. - - Publish a message on each publisher. - - Take from each publisher with each subscription, checking the pointer. - */ -TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal1"; - p2->mock_queue_size = 10; - - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); - p3->mock_topic_name = "nominal1"; - p3->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto p2_id = ipm.add_publisher(p2); - auto p3_id = ipm.add_publisher(p3); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - // First publish - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer1 = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // Second publish - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p2_m1_id = ipm.store_intra_process_message(p2_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // Third publish - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer3 = unique_msg.get(); - auto p3_m1_id = ipm.store_intra_process_message(p3_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // First take - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer1, unique_msg.get()); // Final take. - } - unique_msg.reset(); - - // Second take - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - // Third take - ipm.take_intra_process_message(p3_id, p3_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p3_id, p3_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p3_id, p3_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); -} +} // namespace mock +} // namespace rclcpp /* - Tests displacing a message from the ring buffer before take is called. - - Creates a publisher (buffer_size = 2) and a subscription on the same topic. - - Publish a message on the publisher. - - Publish another message. - - Take the second message. - - Publish a message. - - Try to take the first message, should fail. + This tests how the class connects and disconnects publishers and subscriptions: + - Creates 2 publishers on different topics and a subscription to one of them. + Add everything to the intra-process manager. + - All the entities are expected to have different ids. + - Check the subscriptions count for each publisher. + - One of the publishers is expected to have 1 subscription, while the other 0. + - Check the subscription count for a non existing publisher id, should return 0. + - Add a new publisher and a new subscription both with reliable QoS. + - The subscriptions count of the previous publisher is expected to remain unchanged, + while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). + - Remove the just added subscriptions. + - The count for the last publisher is expected to decrease to 1. */ -TEST(TestIntraProcessManager, ring_buffer_displacement) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = ipm.add_publisher(p1); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p1_m2_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m2_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - EXPECT_EQ(nullptr, unique_msg); - unique_msg.reset(); - - // Since it just got displaced it should no longer be there to take. - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); +TEST(TestIntraProcessManager, add_pub_sub) { + using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + p1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + + auto p2 = std::make_shared(); + p2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + p2->topic_name = "different_topic_name"; + + auto s1 = std::make_shared(); + s1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + + auto p1_id = ipm->add_publisher(p1); + auto p2_id = ipm->add_publisher(p2); + auto s1_id = ipm->add_subscription(s1); + + bool unique_ids = p1_id != p2_id && p2_id != s1_id; + ASSERT_TRUE(unique_ids); + + size_t p1_subs = ipm->get_subscription_count(p1_id); + size_t p2_subs = ipm->get_subscription_count(p2_id); + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + auto p3 = std::make_shared(); + p3->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + auto s2 = std::make_shared(); + s2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + auto s2_id = ipm->add_subscription(s2); + auto p3_id = ipm->add_publisher(p3); + + p1_subs = ipm->get_subscription_count(p1_id); + p2_subs = ipm->get_subscription_count(p2_id); + size_t p3_subs = ipm->get_subscription_count(p3_id); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(2u, p3_subs); + + ipm->remove_subscription(s2_id); + p1_subs = ipm->get_subscription_count(p1_id); + p2_subs = ipm->get_subscription_count(p2_id); + p3_subs = ipm->get_subscription_count(p3_id); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(1u, p3_subs); } /* - Simulates race condition where a subscription is created after publish. - - Creates a publisher. - - Publish a message on the publisher. - - Create a subscription on the same topic. - - Try to take the message with the newly created subscription, should fail. + This tests the minimal usage of the class where there is a single subscription per publisher: + - Publishes a unique_ptr message with a subscription requesting ownership. + - The received message is expected to be the same. + - Remove the first subscription from ipm and add a new one. + - Publishes a unique_ptr message with a subscription not requesting ownership. + - The received message is expected to be the same, the first subscription do not receive it. + - Publishes a shared_ptr message with a subscription not requesting ownership. + - The received message is expected to be the same. */ -TEST(TestIntraProcessManager, subscription_creation_race_condition) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto p1_id = ipm.add_publisher(p1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s1_id = ipm.add_subscription(s1); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); +TEST(TestIntraProcessManager, single_subscription) { + using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = false; + auto s1_id = ipm->add_subscription(s1); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_1); + + ipm->remove_subscription(s1_id); + auto s2 = std::make_shared(); + s2->take_shared_method = true; + auto s2_id = ipm->add_subscription(s2); + (void)s2_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + ASSERT_EQ(0u, received_message_pointer_1); + + auto shared_msg = std::make_shared(); + original_message_pointer = reinterpret_cast(shared_msg.get()); + p1->publish(shared_msg); + received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); } /* - Simulates race condition where a publisher goes out of scope before take. - - Create a subscription. - - Creates a publisher on the same topic in a scope. - - Publish a message on the publisher in a scope. - - Let the scope expire. - - Try to take the message with the subscription, should fail. + This tests the usage of the class where there are multiple subscriptions of the same type: + - Publishes a unique_ptr message with 2 subscriptions requesting ownership. + - One is expected to receive the published message, while the other will receive a copy. + - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. + - Both received messages are expected to be the same as the published one. + - Publishes a shared_ptr message with 2 subscriptions requesting ownership. + - Both received messages are expected to be a copy of the published one. + - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. + - Both received messages are expected to be the same as the published one. */ -TEST(TestIntraProcessManager, publisher_out_of_scope_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s1_id = ipm.add_subscription(s1); - - uint64_t p1_id; - uint64_t p1_m1_id; - { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - p1_id = ipm.add_publisher(p1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - // Explicitly remove publisher from ipm (emulate's publisher's destructor). - ipm.remove_publisher(p1_id); - } - - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg(nullptr); - // Should fail because the publisher is out of scope. - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); +TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { + using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = false; + auto s1_id = ipm->add_subscription(s1); + + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + bool received_original_1 = s1->pop() == original_message_pointer; + bool received_original_2 = s2->pop() == original_message_pointer; + std::vector received_original_vec = + {received_original_1, received_original_2}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared(); + s3->take_shared_method = true; + auto s3_id = ipm->add_subscription(s3); + + auto s4 = std::make_shared(); + s4->take_shared_method = true; + auto s4_id = ipm->add_subscription(s4); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_3); + ASSERT_EQ(original_message_pointer, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + + auto s5 = std::make_shared(); + s5->take_shared_method = false; + auto s5_id = ipm->add_subscription(s5); + + auto s6 = std::make_shared(); + s6->take_shared_method = false; + auto s6_id = ipm->add_subscription(s6); + + auto shared_msg = std::make_shared(); + original_message_pointer = reinterpret_cast(shared_msg.get()); + p1->publish(shared_msg); + auto received_message_pointer_5 = s5->pop(); + auto received_message_pointer_6 = s6->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_5); + ASSERT_NE(original_message_pointer, received_message_pointer_6); + + ipm->remove_subscription(s5_id); + ipm->remove_subscription(s6_id); + + auto s7 = std::make_shared(); + s7->take_shared_method = true; + auto s7_id = ipm->add_subscription(s7); + (void)s7_id; + + auto s8 = std::make_shared(); + s8->take_shared_method = true; + auto s8_id = ipm->add_subscription(s8); + (void)s8_id; + + shared_msg = std::make_shared(); + original_message_pointer = reinterpret_cast(shared_msg.get()); + p1->publish(shared_msg); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_7); + ASSERT_EQ(original_message_pointer, received_message_pointer_8); } /* - Simulates race condition where a publisher goes out of scope before store. - - Creates a publisher in a scope. - - Let the scope expire. - - Publish a message on the publisher in a scope, should throw. + This tests the usage of the class where there are multiple subscriptions of different types: + - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. + - The one requesting ownership is expected to receive the published message, + while the other is expected to receive a copy. + - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. + - One of the subscriptions requesting ownership is expected to receive the published message, + while both other subscriptions are expected to receive different copies. + - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. + - The 2 subscriptions not requesting ownership are expected to both receive the same copy + of the message, one of the subscription requesting ownership is expected to receive a + different copy, while the last is expected to receive the published message. + - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. + - The subscription requesting ownership is expected to receive a copy of the message, while + the other is expected to receive the published message */ -TEST(TestIntraProcessManager, publisher_out_of_scope_store) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - uint64_t p1_id; - { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - p1_id = ipm.add_publisher(p1); - } - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - EXPECT_THROW(ipm.store_intra_process_message(p1_id, std::move(unique_msg)), std::runtime_error); - ASSERT_EQ(nullptr, unique_msg); +TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { + using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared(); + s1->take_shared_method = true; + auto s1_id = ipm->add_subscription(s1); + + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_1); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared(); + s3->take_shared_method = false; + auto s3_id = ipm->add_subscription(s3); + + auto s4 = std::make_shared(); + s4->take_shared_method = false; + auto s4_id = ipm->add_subscription(s4); + + auto s5 = std::make_shared(); + s5->take_shared_method = true; + auto s5_id = ipm->add_subscription(s5); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + auto received_message_pointer_5 = s5->pop(); + bool received_original_3 = received_message_pointer_3 == original_message_pointer; + bool received_original_4 = received_message_pointer_4 == original_message_pointer; + bool received_original_5 = received_message_pointer_5 == original_message_pointer; + std::vector received_original_vec = + {received_original_3, received_original_4, received_original_5}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false, false)); + ASSERT_NE(received_message_pointer_3, received_message_pointer_4); + ASSERT_NE(received_message_pointer_5, received_message_pointer_3); + ASSERT_NE(received_message_pointer_5, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + ipm->remove_subscription(s5_id); + + auto s6 = std::make_shared(); + s6->take_shared_method = true; + auto s6_id = ipm->add_subscription(s6); + + auto s7 = std::make_shared(); + s7->take_shared_method = true; + auto s7_id = ipm->add_subscription(s7); + + auto s8 = std::make_shared(); + s8->take_shared_method = false; + auto s8_id = ipm->add_subscription(s8); + + auto s9 = std::make_shared(); + s9->take_shared_method = false; + auto s9_id = ipm->add_subscription(s9); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_6 = s6->pop(); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + auto received_message_pointer_9 = s9->pop(); + bool received_original_8 = received_message_pointer_8 == original_message_pointer; + bool received_original_9 = received_message_pointer_9 == original_message_pointer; + received_original_vec = {received_original_8, received_original_9}; + ASSERT_EQ(received_message_pointer_6, received_message_pointer_7); + ASSERT_NE(original_message_pointer, received_message_pointer_6); + ASSERT_NE(original_message_pointer, received_message_pointer_7); + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + ASSERT_NE(received_message_pointer_8, received_message_pointer_6); + ASSERT_NE(received_message_pointer_9, received_message_pointer_6); + + ipm->remove_subscription(s6_id); + ipm->remove_subscription(s7_id); + ipm->remove_subscription(s8_id); + ipm->remove_subscription(s9_id); + + auto s10 = std::make_shared(); + s10->take_shared_method = false; + auto s10_id = ipm->add_subscription(s10); + (void)s10_id; + + auto s11 = std::make_shared(); + s11->take_shared_method = true; + auto s11_id = ipm->add_subscription(s11); + (void)s11_id; + + auto shared_msg = std::make_shared(); + original_message_pointer = reinterpret_cast(shared_msg.get()); + p1->publish(shared_msg); + auto received_message_pointer_10 = s10->pop(); + auto received_message_pointer_11 = s11->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_10); + ASSERT_EQ(original_message_pointer, received_message_pointer_11); } diff --git a/rclcpp/test/test_mapped_ring_buffer.cpp b/rclcpp/test/test_mapped_ring_buffer.cpp deleted file mode 100644 index 5625804d13..0000000000 --- a/rclcpp/test/test_mapped_ring_buffer.cpp +++ /dev/null @@ -1,334 +0,0 @@ -// Copyright 2015 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 "gtest/gtest.h" - -#define RCLCPP_BUILDING_LIBRARY 1 // Prevent including unavailable symbols -#include "rclcpp/mapped_ring_buffer.hpp" - -/* - Tests get_copy and pop on an empty mrb. - */ -TEST(TestMappedRingBuffer, empty) { - // Cannot create a buffer of size zero. - EXPECT_THROW(rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(0), std::invalid_argument); - // Getting or popping an empty buffer should result in a nullptr. - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(1); - - std::unique_ptr unique; - mrb.get(1, unique); - EXPECT_EQ(nullptr, unique); - - mrb.pop(1, unique); - EXPECT_EQ(nullptr, unique); - - std::shared_ptr shared; - mrb.get(1, shared); - EXPECT_EQ(nullptr, shared); - - mrb.pop(1, shared); - EXPECT_EQ(nullptr, shared); -} - -/* - Tests push_and_replace with a temporary object, and using - get and pop methods with shared_ptr signature. - */ -TEST(TestMappedRingBuffer, temporary_l_value_with_shared_get_pop) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - // Pass in value with temporary object - mrb.push_and_replace(1, std::shared_ptr(new char('a'))); - - std::shared_ptr actual; - mrb.get(1, actual); - EXPECT_EQ('a', *actual); - - mrb.pop(1, actual); - EXPECT_EQ('a', *actual); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); -} - -/* - Tests push_and_replace with a temporary object, and using - get and pop methods with unique_ptr signature. - */ -TEST(TestMappedRingBuffer, temporary_l_value_with_unique_get_pop) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - // Pass in value with temporary object - mrb.push_and_replace(1, std::shared_ptr(new char('a'))); - - std::unique_ptr actual; - mrb.get(1, actual); - EXPECT_EQ('a', *actual); - - mrb.pop(1, actual); - EXPECT_EQ('a', *actual); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); -} - -/* - Tests normal usage of the mrb. - Using shared push_and_replace, get and pop methods. - */ -TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_shared) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::shared_ptr expected(new char('a')); - - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - EXPECT_EQ(2, expected.use_count()); - - std::shared_ptr actual; - mrb.get(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected, actual); - EXPECT_EQ(3, actual.use_count()); - - mrb.pop(1, actual); - EXPECT_EQ(expected, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - expected.reset(); - EXPECT_TRUE(actual.unique()); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - expected.reset(new char('a')); - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - - expected.reset(new char('b')); - EXPECT_FALSE(mrb.push_and_replace(2, expected)); - - expected.reset(new char('c')); - EXPECT_TRUE(mrb.push_and_replace(3, expected)); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.get(2, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } - - mrb.get(3, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('c', *actual); - } -} - -/* - Tests normal usage of the mrb. - Using shared push_and_replace, unique get and pop methods. - */ -TEST(TestMappedRingBuffer, nominal_push_shared_get_pop_unique) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::shared_ptr expected(new char('a')); - const char * expected_orig = expected.get(); - - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - EXPECT_EQ(2, expected.use_count()); - - std::unique_ptr actual; - mrb.get(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_NE(expected_orig, actual.get()); - mrb.pop(1, actual); - EXPECT_NE(expected_orig, actual.get()); - if (actual) { - EXPECT_EQ('a', *actual); - } - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - expected.reset(); - mrb.pop(1, actual); - EXPECT_NE(expected_orig, actual.get()); - if (actual) { - EXPECT_EQ('a', *actual); - } - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - expected.reset(new char('a')); - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - - expected.reset(new char('b')); - EXPECT_FALSE(mrb.push_and_replace(2, expected)); - - expected.reset(new char('c')); - EXPECT_TRUE(mrb.push_and_replace(3, expected)); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.get(2, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } - - mrb.get(3, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('c', *actual); - } -} - -/* - Tests normal usage of the mrb. - Using unique push_and_replace, get and pop methods. - */ -TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_unique) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::unique_ptr expected(new char('a')); - const char * expected_orig = expected.get(); - - EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected))); - - std::unique_ptr actual; - mrb.get(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_NE(expected_orig, actual.get()); - mrb.pop(1, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected_orig, actual.get()); - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - expected.reset(new char('a')); - EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected))); - - expected.reset(new char('b')); - EXPECT_FALSE(mrb.push_and_replace(2, std::move(expected))); - - expected.reset(new char('c')); - EXPECT_TRUE(mrb.push_and_replace(3, std::move(expected))); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.get(2, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } - - mrb.get(3, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('c', *actual); - } -} - -/* - Tests normal usage of the mrb. - Using unique push_and_replace, shared get and pop methods. - */ -TEST(TestMappedRingBuffer, nominal_push_unique_get_pop_shared) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::unique_ptr expected(new char('a')); - const char * expected_orig = expected.get(); - - EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected))); - - std::shared_ptr actual; - mrb.get(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected_orig, actual.get()); - mrb.pop(1, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected_orig, actual.get()); - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - expected.reset(new char('a')); - EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected))); - - expected.reset(new char('b')); - EXPECT_FALSE(mrb.push_and_replace(2, std::move(expected))); - - expected.reset(new char('c')); - EXPECT_TRUE(mrb.push_and_replace(3, std::move(expected))); - - mrb.get(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.get(2, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } - - mrb.get(3, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('c', *actual); - } -} - -/* - Tests the affect of reusing keys (non-unique keys) in a mrb. - */ -TEST(TestMappedRingBuffer, non_unique_keys) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - - std::shared_ptr input(new char('a')); - mrb.push_and_replace(1, input); - input.reset(new char('b')); - - // Different value, same key. - mrb.push_and_replace(1, input); - input.reset(); - - std::unique_ptr actual; - mrb.pop(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - - actual = nullptr; - mrb.pop(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } -} diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp index d86fdfcc65..3f75a95168 100644 --- a/rclcpp/test/test_publisher.cpp +++ b/rclcpp/test/test_publisher.cpp @@ -159,15 +159,11 @@ static std::vector invalid_qos_profiles() { std::vector parameters; - parameters.reserve(3); + parameters.reserve(2); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), "transient_local_qos")); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(0)), - "keep_last_qos_with_zero_history_depth")); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepAll()), diff --git a/rclcpp/test/test_ring_buffer_implementation.cpp b/rclcpp/test/test_ring_buffer_implementation.cpp new file mode 100644 index 0000000000..4b792bebd9 --- /dev/null +++ b/rclcpp/test/test_ring_buffer_implementation.cpp @@ -0,0 +1,80 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/buffers/buffer_implementation_base.hpp" +#include "rclcpp/buffers/ring_buffer_implementation.hpp" + +/* + Construtctor + */ +TEST(TestRingBufferImplementation, constructor) { + // Cannot create a buffer of size zero. + EXPECT_THROW( + rclcpp::intra_process_buffer::RingBufferImplementation rb(0), std::invalid_argument); + + rclcpp::intra_process_buffer::RingBufferImplementation rb(1); + + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} + +/* + Basic usage + - insert data and check that it has data + - extract data + - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage) { + rclcpp::intra_process_buffer::RingBufferImplementation rb(2); + + rb.enqueue('a'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + char v = rb.dequeue(); + + EXPECT_EQ('a', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue('b'); + rb.enqueue('c'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + rb.enqueue('d'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('c', v); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('d', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index f3a0db68ed..78b420b3a0 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" @@ -30,15 +31,15 @@ class TestSubscription : public ::testing::Test (void)msg; } -protected: static void SetUpTestCase() { rclcpp::init(0, nullptr); } - void SetUp() +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) { - node = std::make_shared("test_subscription", "/ns"); + node = std::make_shared("test_subscription", "/ns", node_options); } void TearDown() @@ -49,6 +50,25 @@ class TestSubscription : public ::testing::Test rclcpp::Node::SharedPtr node; }; +struct TestParameters +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + class TestSubscriptionSub : public ::testing::Test { public: @@ -120,6 +140,7 @@ class SubscriptionClass Testing subscription construction and destruction. */ TEST_F(TestSubscription, construction_and_destruction) { + initialize(); using rcl_interfaces::msg::IntraProcessMessage; auto callback = [](const IntraProcessMessage::SharedPtr msg) { (void)msg; @@ -173,6 +194,7 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { Testing subscription creation signatures. */ TEST_F(TestSubscription, various_creation_signatures) { + initialize(); using rcl_interfaces::msg::IntraProcessMessage; auto cb = [](rcl_interfaces::msg::IntraProcessMessage::SharedPtr) {}; { @@ -209,6 +231,7 @@ TEST_F(TestSubscription, various_creation_signatures) { Testing subscriptions using std::bind. */ TEST_F(TestSubscription, callback_bind) { + initialize(); using rcl_interfaces::msg::IntraProcessMessage; { // Member callback for plain class @@ -228,3 +251,51 @@ TEST_F(TestSubscription, callback_bind) { auto sub = node->create_subscription("topic", 1, callback); } } + +/* + Testing subscription with intraprocess enabled and invalid QoS + */ +TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::QoS qos = GetParam().qos; + using rcl_interfaces::msg::IntraProcessMessage; + { + auto callback = std::bind( + &TestSubscriptionInvalidIntraprocessQos::OnMessage, + this, + std::placeholders::_1); + + ASSERT_THROW( + {auto subscription = node->create_subscription( + "topic", + qos, + callback);}, + std::invalid_argument); + } +} + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(3); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), + "transient_local_qos")); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepLast(0)), + "keep_last_zero_depth_qos")); + + return parameters; +} + +INSTANTIATE_TEST_CASE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); From 2348af6fad10232cb8964c83a1d0912d53daf842 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 17 Oct 2019 15:53:28 -0700 Subject: [PATCH 02/18] fixup a few things after rebase Signed-off-by: William Woodall --- .../buffers/buffer_implementation_base.hpp | 2 ++ .../rclcpp/buffers/intra_process_buffer.hpp | 22 +++++++++++++------ .../buffers/ring_buffer_implementation.hpp | 1 + .../strategies/allocator_memory_strategy.hpp | 6 ++--- rclcpp/include/rclcpp/subscription_base.hpp | 1 + .../rclcpp/subscription_intra_process.hpp | 2 +- rclcpp/src/rclcpp/intra_process_manager.cpp | 2 +- rclcpp/src/rclcpp/time_source.cpp | 3 --- rclcpp/test/test_intra_process_buffer.cpp | 18 +++++---------- rclcpp/test/test_intra_process_manager.cpp | 22 +++++++++++-------- 10 files changed, 42 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp index 5121dbc095..2eb06e4bd0 100644 --- a/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp @@ -24,6 +24,8 @@ template class BufferImplementationBase { public: + virtual ~BufferImplementationBase() {} + virtual BufferT dequeue() = 0; virtual void enqueue(BufferT request) = 0; diff --git a/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp index c266f7183d..f289574e3d 100644 --- a/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp @@ -22,6 +22,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/buffers/buffer_implementation_base.hpp" +#include "rclcpp/macros.hpp" namespace rclcpp { @@ -33,6 +34,8 @@ class IntraProcessBufferBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + virtual ~IntraProcessBufferBase() {} + virtual void clear() = 0; virtual bool has_data() const = 0; @@ -48,6 +51,8 @@ class IntraProcessBuffer : public IntraProcessBufferBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer) + virtual ~IntraProcessBuffer() {} + using MessageUniquePtr = std::unique_ptr; using MessageSharedPtr = std::shared_ptr; @@ -73,6 +78,7 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer; using MessageSharedPtr = std::shared_ptr; + explicit TypedIntraProcessBuffer( std::unique_ptr> buffer_impl, std::shared_ptr allocator = nullptr) @@ -92,37 +98,39 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(std::move(msg)); } - void add_unique(MessageUniquePtr msg) + void add_unique(MessageUniquePtr msg) override { buffer_->enqueue(std::move(msg)); } - MessageSharedPtr consume_shared() + MessageSharedPtr consume_shared() override { return consume_shared_impl(); } - MessageUniquePtr consume_unique() + MessageUniquePtr consume_unique() override { return consume_unique_impl(); } - bool has_data() const + bool has_data() const override { return buffer_->has_data(); } - void clear() + void clear() override { buffer_->clear(); } - bool use_take_shared_method() const + bool use_take_shared_method() const override { return std::is_same::value; } diff --git a/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp index a8d94e457d..e60f120fe1 100644 --- a/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp @@ -24,6 +24,7 @@ #include #include +#include "rclcpp/buffers/buffer_implementation_base.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 973b0941cf..eab0949d08 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -171,10 +171,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy }); group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) { - if (service) { - service_handles_.push_back(service->get_service_handle()); - return false; - } + service_handles_.push_back(service->get_service_handle()); + return false; }); group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index b029fcacff..bb7df4a749 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -27,6 +27,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" +#include "rclcpp/subscription_intra_process_base.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/subscription_intra_process.hpp b/rclcpp/include/rclcpp/subscription_intra_process.hpp index 845fc8e12b..be95275281 100644 --- a/rclcpp/include/rclcpp/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/subscription_intra_process.hpp @@ -24,9 +24,9 @@ #include "rcl/error_handling.h" +#include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/buffers/intra_process_buffer.hpp" #include "rclcpp/create_intra_process_buffer.hpp" -#include "rclcpp/subscription.hpp" #include "rclcpp/subscription_intra_process_base.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 4fd756b7f0..3c7638b1d4 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -37,7 +37,7 @@ IntraProcessManager::add_publisher( publishers_[id].publisher = publisher; publishers_[id].topic_name = publisher->get_topic_name(); - publishers_[id].qos = publisher->get_actual_qos(); + publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile(); // Initialize the subscriptions storage for this publisher. pub_to_subs_[id] = SplittedSubscriptions(); diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index d9820442a9..ccf05bf65b 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -249,13 +249,10 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); for (auto & it : filter.get_events()) { -<<<<<<< HEAD -======= if (it.second->value.type != ParameterType::PARAMETER_BOOL) { RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); continue; } ->>>>>>> basic ipc implementation from alsora/new_ipc_proposal if (it.second->value.bool_value) { parameter_state_ = SET_TRUE; enable_ros_time(); diff --git a/rclcpp/test/test_intra_process_buffer.cpp b/rclcpp/test/test_intra_process_buffer.cpp index 724bbf979c..3d6a73ef61 100644 --- a/rclcpp/test/test_intra_process_buffer.cpp +++ b/rclcpp/test/test_intra_process_buffer.cpp @@ -37,16 +37,14 @@ TEST(TestIntraProcessBuffer, constructor) { auto shared_buffer_impl = std::make_unique>(2); - auto shared_intra_process_buffer = - SharedIntraProcessBufferT(std::move(shared_buffer_impl)); + SharedIntraProcessBufferT shared_intra_process_buffer(std::move(shared_buffer_impl)); EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method()); auto unique_buffer_impl = std::make_unique>(2); - auto unique_intra_process_buffer = - UniqueIntraProcessBufferT(std::move(unique_buffer_impl)); + UniqueIntraProcessBufferT unique_intra_process_buffer(std::move(unique_buffer_impl)); EXPECT_EQ(false, unique_intra_process_buffer.use_take_shared_method()); } @@ -68,8 +66,7 @@ TEST(TestIntraProcessBuffer, shared_buffer_add) { auto buffer_impl = std::make_unique>(2); - auto intra_process_buffer = - SharedIntraProcessBufferT(std::move(buffer_impl)); + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); auto original_shared_msg = std::make_shared('a'); auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); @@ -117,8 +114,7 @@ TEST(TestIntraProcessBuffer, unique_buffer_add) { auto buffer_impl = std::make_unique>(2); - auto intra_process_buffer = - UniqueIntraProcessBufferT(std::move(buffer_impl)); + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); auto original_shared_msg = std::make_shared('a'); auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); @@ -165,8 +161,7 @@ TEST(TestIntraProcessBuffer, shared_buffer_consume) { auto buffer_impl = std::make_unique>(2); - auto intra_process_buffer = - SharedIntraProcessBufferT(std::move(buffer_impl)); + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); auto original_shared_msg = std::make_shared('a'); auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); @@ -215,8 +210,7 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { auto buffer_impl = std::make_unique>(2); - auto intra_process_buffer = - UniqueIntraProcessBufferT(std::move(buffer_impl)); + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); auto original_unique_msg = std::make_unique('a'); auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 72ff4e20ce..31299c7c87 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -23,6 +23,7 @@ #define RCLCPP_BUILDING_LIBRARY 1 #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -52,7 +53,8 @@ class PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) PublisherBase() - : topic_name("topic") + : qos(rclcpp::QoS(10)), + topic_name("topic") {} virtual ~PublisherBase() @@ -71,10 +73,10 @@ class PublisherBase weak_ipm_ = ipm; } - rmw_qos_profile_t - get_actual_qos() + rclcpp::QoS + get_actual_qos() const { - return qos_profile; + return qos; } bool @@ -91,7 +93,7 @@ class PublisherBase return false; } - rmw_qos_profile_t qos_profile; + rclcpp::QoS qos; std::string topic_name; uint64_t intra_process_publisher_id_; IntraProcessManagerWeakPtr weak_ipm_; @@ -111,7 +113,7 @@ class Publisher : public PublisherBase Publisher() { - qos_profile = rmw_qos_profile_default; + qos = rclcpp::QoS(10); auto allocator = std::make_shared(); message_allocator_ = std::make_shared(*allocator.get()); } @@ -189,6 +191,8 @@ class SubscriptionIntraProcessBase : qos_profile(rmw_qos_profile_default), topic_name("topic") {} + virtual ~SubscriptionIntraProcessBase() {} + virtual bool use_take_shared_method() const = 0; @@ -344,10 +348,10 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto ipm = std::make_shared(); auto p1 = std::make_shared(); - p1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + p1->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; auto p2 = std::make_shared(); - p2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + p2->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; p2->topic_name = "different_topic_name"; auto s1 = std::make_shared(); @@ -368,7 +372,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { ASSERT_EQ(0u, non_existing_pub_subs); auto p3 = std::make_shared(); - p3->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + p3->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; auto s2 = std::make_shared(); s2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; From b944c4468cda576faee6708169c6899f95fb5c0b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 17 Oct 2019 15:54:52 -0700 Subject: [PATCH 03/18] refactor some API's and get code compiling again Signed-off-by: William Woodall --- .../resolve_intra_process_buffer_type.hpp | 54 +++++++++++++++++++ .../rclcpp/node_interfaces/node_topics.hpp | 3 +- .../node_interfaces/node_topics_interface.hpp | 3 +- rclcpp/include/rclcpp/publisher.hpp | 4 +- rclcpp/include/rclcpp/subscription.hpp | 44 ++++++++++----- rclcpp/include/rclcpp/subscription_base.hpp | 7 ++- .../rclcpp/node_interfaces/node_topics.cpp | 27 ++++------ rclcpp/src/rclcpp/subscription_base.cpp | 14 +++-- 8 files changed, 113 insertions(+), 43 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp diff --git a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp new file mode 100644 index 0000000000..e7196a1e11 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -0,0 +1,54 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ + +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" + +namespace rclcpp +{ + +namespace detail +{ + +/// Return the buffer type, resolving the "CallbackDefault" type to an actual type if needed. +template +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type, + const rclcpp::AnySubscriptionCallback & any_subscription_callback) +{ + rclcpp::IntraProcessBufferType resolved_buffer_type = buffer_type; + + // If the user has not specified a type for the intra-process buffer, use the callback's type. + if (resolved_buffer_type == IntraProcessBufferType::CallbackDefault) { + if (any_subscription_callback.use_take_shared_method()) { + resolved_buffer_type = IntraProcessBufferType::SharedPtr; + } else { + resolved_buffer_type = IntraProcessBufferType::UniquePtr; + } + } + + return resolved_buffer_type; +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 5a5580e4da..8209984abc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -68,8 +68,7 @@ class NodeTopics : public NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, - bool use_intra_process) override; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface * diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 59657d205c..c8214c59fb 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -73,8 +73,7 @@ class NodeTopicsInterface void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, - bool use_intra_process) = 0; + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4140add7f9..e02d86d60b 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -99,6 +99,7 @@ class Publisher : public PublisherBase { // Topic is unused for now. (void)topic; + (void)options; // If needed, setup intra process communication. if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { @@ -117,8 +118,7 @@ class Publisher : public PublisherBase uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); this->setup_intra_process( intra_process_publisher_id, - ipm, - options.template to_rcl_publisher_options(qos)); + ipm); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index aaff8eeb3a..1a7b499f03 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -31,13 +31,16 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_intra_process.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" @@ -115,6 +118,30 @@ class Subscription : public SubscriptionBase options.event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } + + // Setup intra process publishing if requested. + if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { + using rclcpp::detail::resolve_intra_process_buffer_type; + + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. + auto context = node_base->get_context(); + auto subscription_intra_process = + std::make_shared>( + callback, + options.get_allocator(), + context, + topic_name, + qos.get_rmw_qos_profile(), + resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) + ); + + // Add it to the intra process manager. + using rclcpp::intra_process_manager::IntraProcessManager; + auto ipm = context->get_sub_context(); + uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process); + this->setup_intra_process(intra_process_subscription_id, ipm); + } + TRACEPOINT( rclcpp_subscription_callback_added, (const void *)get_subscription_handle().get(), @@ -126,23 +153,14 @@ class Subscription : public SubscriptionBase } /// Called after construction to continue setup that requires shared_from_this(). - void - post_init_setup( + void post_init_setup( rclcpp::node_interfaces::NodeBaseInterface * node_base, const rclcpp::QoS & qos, const rclcpp::SubscriptionOptionsWithAllocator & options) { - // Setup intra process publishing if requested. - if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { - auto context = node_base->get_context(); - using rclcpp::intra_process_manager::IntraProcessManager; - auto ipm = context->get_sub_context(); - uint64_t intra_process_subscription_id = ipm->add_subscription(this->shared_from_this()); - this->setup_intra_process( - intra_process_subscription_id, - ipm, - options.template to_rcl_subscription_options(qos)); - } + (void)node_base; + (void)qos; + (void)options; } /// Support dynamically setting the message memory strategy. diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index bb7df4a749..915496a357 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -188,9 +188,10 @@ class SubscriptionBase : public std::enable_shared_from_this uint64_t intra_process_subscription_id, IntraProcessManagerWeakPtr weak_ipm); + /// Return the waitable for intra-process, or nullptr if intra-process is not setup. RCLCPP_PUBLIC - uint64_t - get_intra_process_id() const; + rclcpp::Waitable::SharedPtr + get_intra_process_waitable() const; protected: template @@ -211,6 +212,8 @@ class SubscriptionBase : public std::enable_shared_from_this bool matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + rclcpp::node_interfaces::NodeBaseInterface * const node_base_; + std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 518824bd51..cb5f3881fe 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -82,8 +82,7 @@ NodeTopics::create_subscription( void NodeTopics::add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group, - bool use_intra_process) + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) { // Assign to a group. if (callback_group) { @@ -96,32 +95,24 @@ NodeTopics::add_subscription( } callback_group->add_subscription(subscription); + for (auto & subscription_event : subscription->get_event_handlers()) { callback_group->add_waitable(subscription_event); } - if (use_intra_process) { - // Get the intra process manager for this context. - auto context = node_base_->get_context(); - auto ipm = - context->get_sub_context(); - - // Use the id to retrieve the subscription intra-process from the intra-process manager. - auto subscription_intra_process = - ipm->get_subscription_intra_process(subscription->get_intra_process_id()); - + auto intra_process_waitable = subscription->get_intra_process_waitable(); + if (nullptr != intra_process_waitable) { // Add to the callback group to be notified about intra-process msgs. - callback_group->add_waitable(subscription_intra_process); + callback_group->add_waitable(intra_process_waitable); } // Notify the executor that a new subscription was created using the parent Node. { auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on subscription creation: ") + - rmw_get_error_string().str - ); + auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); + if (ret != RCL_RET_OK) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); } } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 1d0bb13e81..3383dc871c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -36,7 +36,8 @@ SubscriptionBase::SubscriptionBase( const std::string & topic_name, const rcl_subscription_options_t & subscription_options, bool is_serialized) -: node_handle_(node_base->get_shared_rcl_node_handle()), +: node_base_(node_base), + node_handle_(node_base_->get_shared_rcl_node_handle()), use_intra_process_(false), intra_process_subscription_id_(0), type_support_(type_support_handle), @@ -175,10 +176,15 @@ SubscriptionBase::can_loan_messages() const return rcl_subscription_can_loan_messages(subscription_handle_.get()); } -uint64_t -SubscriptionBase::get_intra_process_id() const +rclcpp::Waitable::SharedPtr +SubscriptionBase::get_intra_process_waitable() const { - return intra_process_subscription_id_; + // Get the intra process manager for this context. + auto context = node_base_->get_context(); + auto ipm = context->get_sub_context(); + + // Use the id to retrieve the subscription intra-process from the intra-process manager. + return ipm->get_subscription_intra_process(intra_process_subscription_id_); } bool From b026cb2dfb60b19e982d8427a5294dbae3f9b5fc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 17 Oct 2019 15:55:11 -0700 Subject: [PATCH 04/18] docs and style changes (whitespace) Signed-off-by: William Woodall --- .../detail/resolve_use_intra_process.hpp | 1 + .../include/rclcpp/intra_process_manager.hpp | 50 ++++++++++--------- rclcpp/include/rclcpp/subscription.hpp | 3 +- .../include/rclcpp/subscription_factory.hpp | 8 +-- rclcpp/src/rclcpp/intra_process_manager.cpp | 6 +-- 5 files changed, 35 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp index cab3542617..cefdee2b22 100644 --- a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp @@ -25,6 +25,7 @@ namespace rclcpp namespace detail { +/// Return the whether or not intra process is enabled, resolving "NodeDefault" if needed. template bool resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base) diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp index 73d0eb0e45..28345d9789 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager.hpp @@ -51,36 +51,40 @@ namespace intra_process_manager * A singleton instance of this class is owned by a rclcpp::Context and a * rclcpp::Node can use an associated Context to get an instance of this class. * Nodes which do not have a common Context will not exchange intra process - * messages because they will not share access to an instance of this class. + * messages because they do not share access to the same instance of this class. * - * When a Node creates a subscription, it can also create an additional - * wrapper meant to receive intra process messages. - * This structure can be registered with this class. + * When a Node creates a subscription, it can also create a helper class, + * called SubscriptionIntraProcess, meant to receive intra process messages. + * It can be registered with this class. * It is also allocated an id which is unique among all publishers * and subscriptions in this process and that is associated to the subscription. * - * When a Node creates a publisher, as before this can be registered with this class. + * When a Node creates a publisher, as with subscriptions, a helper class can + * be registered with this class. * This is required in order to publish intra-process messages. * It is also allocated an id which is unique among all publishers * and subscriptions in this process and that is associated to the publisher. * - * When a publisher or a subscription are registered with this class, an internal - * structure is updated in order to store which of them can communicate. + * When a publisher or a subscription are registered, this class checks to see + * which other subscriptions or publishers it will communicate with, * i.e. they have the same topic and compatible QoS. * * When the user publishes a message, if intra-process communication is enabled - * on the publisher, the message is handed to this class. + * on the publisher, the message is given to this class. * Using the publisher id, a list of recipients for the message is selected. - * For each item in the list, this class stores its intra-process wrapper. + * For each subscription in the list, this class stores the message, whether + * sharing ownership or making a copy, in a buffer associated with the + * subscription helper class. * - * The wrapper contains a buffer where published intra-process messages are stored - * until the subscription picks them up. - * Depending on the data type stored in the buffer, the subscription intra process - * can request ownership on the inserted messages. + * The subscription helper class contains a buffer where published + * intra-process messages are stored until they are taken from the subscription. + * Depending on the data type stored in the buffer, the subscription helper + * class can request either shared or exclusive ownership on the message. * * Thus, when an intra-process message is published, this class knows how many * intra-process subscriptions needs it and how many require ownership. - * This information allows to efficiently perform a minimum number of copies of the message. + * This information allows this class to operate efficiently by performing the + * fewest number of copies of the message required. * * This class is neither CopyConstructable nor CopyAssignable. */ @@ -110,8 +114,7 @@ class IntraProcessManager */ RCLCPP_PUBLIC uint64_t - add_subscription( - SubscriptionIntraProcessBase::SharedPtr subscription); + add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription); /// Unregister a subscription using the subscription's unique id. /** @@ -345,17 +348,18 @@ class IntraProcessManager std::vector take_ownership_subscriptions; }; - using SubscriptionMap = std::unordered_map< - uint64_t, SubscriptionInfo>; + using SubscriptionMap = + std::unordered_map; - using PublisherMap = std::unordered_map< - uint64_t, PublisherInfo>; + using PublisherMap = + std::unordered_map; - using PublisherToSubscriptionIdsMap = std::unordered_map< - uint64_t, SplittedSubscriptions>; + using PublisherToSubscriptionIdsMap = + std::unordered_map; RCLCPP_PUBLIC - static uint64_t + static + uint64_t get_next_unique_id(); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 1a7b499f03..8473118505 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -225,8 +225,7 @@ class Subscription : public SubscriptionBase message_memory_strategy_->return_serialized_message(message); } - bool - use_take_shared_method() const + bool use_take_shared_method() const { return any_callback_.use_take_shared_method(); } diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 77ac4e3a5f..25caaafe6c 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -34,10 +34,10 @@ namespace rclcpp { -/// Factory with functions used to create a Subscription. +/// Factory containing a function used to create a Subscription. /** - * This factory class is used to encapsulate the template generated functions - * which are used during the creation of a Message type specific subscription + * This factory class is used to encapsulate the template generated function + * which is used during the creation of a Message type specific subscription * within a non-templated class. * * It is created using the create_subscription_factory function, which is @@ -60,7 +60,7 @@ struct SubscriptionFactory const SubscriptionFactoryFunction create_typed_subscription; }; -/// Return a SubscriptionFactory with functions for creating a SubscriptionT. +/// Return a SubscriptionFactory setup to create a SubscriptionT. template< typename MessageT, typename CallbackT, diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 3c7638b1d4..5b17943a9d 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -28,8 +28,7 @@ IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) +IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) { std::unique_lock lock(mutex_); @@ -53,8 +52,7 @@ IntraProcessManager::add_publisher( } uint64_t -IntraProcessManager::add_subscription( - SubscriptionIntraProcessBase::SharedPtr subscription) +IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription) { std::unique_lock lock(mutex_); From fef8202c9337fcb258baa8b85e691434a73a46b1 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 16:48:11 -0700 Subject: [PATCH 05/18] move new intra process internals into experimental namespace Signed-off-by: William Woodall --- rclcpp/include/rclcpp/any_executable.hpp | 1 - rclcpp/include/rclcpp/experimental/README.md | 4 ++ .../buffers/buffer_implementation_base.hpp | 13 +++--- .../buffers/intra_process_buffer.hpp | 15 ++++--- .../buffers/ring_buffer_implementation.hpp | 15 ++++--- .../create_intra_process_buffer.hpp | 26 +++++++----- .../intra_process_manager.hpp | 35 ++++++++-------- .../subscription_intra_process.hpp | 24 +++++++---- .../subscription_intra_process_base.hpp | 10 +++-- rclcpp/include/rclcpp/node_impl.hpp | 1 - .../node_interfaces/node_topics_interface.hpp | 1 - rclcpp/include/rclcpp/publisher.hpp | 4 +- rclcpp/include/rclcpp/publisher_base.hpp | 10 ++--- rclcpp/include/rclcpp/publisher_factory.hpp | 3 +- rclcpp/include/rclcpp/subscription.hpp | 25 +++++------ rclcpp/include/rclcpp/subscription_base.hpp | 9 ++-- .../include/rclcpp/subscription_factory.hpp | 8 ++-- rclcpp/src/rclcpp/any_executable.cpp | 1 - rclcpp/src/rclcpp/intra_process_manager.cpp | 22 ++++++---- .../rclcpp/node_interfaces/node_topics.cpp | 1 - rclcpp/src/rclcpp/publisher_base.cpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 12 ++++-- .../subscription_intra_process_base.cpp | 4 +- rclcpp/test/test_intra_process_buffer.cpp | 36 ++++++++-------- rclcpp/test/test_intra_process_manager.cpp | 42 +++++++++++-------- .../test/test_ring_buffer_implementation.cpp | 11 ++--- 26 files changed, 190 insertions(+), 145 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/README.md rename rclcpp/include/rclcpp/{ => experimental}/buffers/buffer_implementation_base.hpp (74%) rename rclcpp/include/rclcpp/{ => experimental}/buffers/intra_process_buffer.hpp (94%) rename rclcpp/include/rclcpp/{ => experimental}/buffers/ring_buffer_implementation.hpp (86%) rename rclcpp/include/rclcpp/{ => experimental}/create_intra_process_buffer.hpp (67%) rename rclcpp/include/rclcpp/{ => experimental}/intra_process_manager.hpp (93%) rename rclcpp/include/rclcpp/{ => experimental}/subscription_intra_process.hpp (86%) rename rclcpp/include/rclcpp/{ => experimental}/subscription_intra_process_base.hpp (87%) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 7b9bac00aa..aae25dca78 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -42,7 +42,6 @@ struct AnyExecutable // Only one of the following pointers will be set. rclcpp::SubscriptionBase::SharedPtr subscription; - rclcpp::SubscriptionBase::SharedPtr subscription_intra_process; rclcpp::TimerBase::SharedPtr timer; rclcpp::ServiceBase::SharedPtr service; rclcpp::ClientBase::SharedPtr client; diff --git a/rclcpp/include/rclcpp/experimental/README.md b/rclcpp/include/rclcpp/experimental/README.md new file mode 100644 index 0000000000..38ca07c1cf --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/README.md @@ -0,0 +1,4 @@ +Notice that headers in this folder should only provide symbols in the rclcpp::experimental namespace. + +Also notice that these headers are not considered part of the public API as they have not yet been stabilized. +And therefore they are subject to change without notice. diff --git a/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp similarity index 74% rename from rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp rename to rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp index 2eb06e4bd0..1e5346116a 100644 --- a/rclcpp/include/rclcpp/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ -#define RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ namespace rclcpp { -namespace intra_process_buffer +namespace experimental +{ +namespace buffers { template @@ -33,7 +35,8 @@ class BufferImplementationBase virtual bool has_data() const = 0; }; -} // namespace intra_process_buffer +} // namespace buffers +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp similarity index 94% rename from rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp rename to rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index f289574e3d..a8634c5a3a 100644 --- a/rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ -#define RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ #include #include @@ -21,12 +21,14 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" #include "rclcpp/macros.hpp" namespace rclcpp { -namespace intra_process_buffer +namespace experimental +{ +namespace buffers { class IntraProcessBufferBase @@ -231,8 +233,9 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer #include @@ -24,7 +24,7 @@ #include #include -#include "rclcpp/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" @@ -34,7 +34,9 @@ namespace rclcpp { -namespace intra_process_buffer +namespace experimental +{ +namespace buffers { template @@ -115,7 +117,8 @@ class RingBufferImplementation : public BufferImplementationBase std::mutex mutex_; }; -} // namespace intra_process_buffer +} // namespace buffers +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ diff --git a/rclcpp/include/rclcpp/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp similarity index 67% rename from rclcpp/include/rclcpp/create_intra_process_buffer.hpp rename to rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp index c8c069bc7e..ff57a849c3 100644 --- a/rclcpp/include/rclcpp/create_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ -#define RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ #include #include @@ -21,18 +21,20 @@ #include "rcl/subscription.h" -#include "rclcpp/buffers/ring_buffer_implementation.hpp" -#include "rclcpp/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" #include "rclcpp/intra_process_buffer_type.hpp" namespace rclcpp { +namespace experimental +{ template< typename MessageT, typename Alloc = std::allocator, typename Deleter = std::default_delete> -typename intra_process_buffer::IntraProcessBuffer::UniquePtr +typename rclcpp::experimental::buffers::IntraProcessBuffer::UniquePtr create_intra_process_buffer( IntraProcessBufferType buffer_type, rmw_qos_profile_t qos, @@ -43,7 +45,8 @@ create_intra_process_buffer( size_t buffer_size = qos.depth; - typename intra_process_buffer::IntraProcessBuffer::UniquePtr buffer; + using rclcpp::experimental::buffers::IntraProcessBuffer; + typename IntraProcessBuffer::UniquePtr buffer; switch (buffer_type) { case IntraProcessBufferType::SharedPtr: @@ -51,12 +54,12 @@ create_intra_process_buffer( using BufferT = MessageSharedPtr; auto buffer_implementation = - std::make_unique>( + std::make_unique>( buffer_size); // Construct the intra_process_buffer buffer = - std::make_unique>( std::move(buffer_implementation), allocator); @@ -68,12 +71,12 @@ create_intra_process_buffer( using BufferT = MessageUniquePtr; auto buffer_implementation = - std::make_unique>( + std::make_unique>( buffer_size); // Construct the intra_process_buffer buffer = - std::make_unique>( std::move(buffer_implementation), allocator); @@ -90,6 +93,7 @@ create_intra_process_buffer( return buffer; } +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#endif // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp similarity index 93% rename from rclcpp/include/rclcpp/intra_process_manager.hpp rename to rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 28345d9789..034dc7361e 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ #include @@ -31,18 +31,18 @@ #include #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" -#include "rclcpp/subscription_intra_process.hpp" -#include "rclcpp/subscription_intra_process_base.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -namespace intra_process_manager +namespace experimental { /// This class performs intra process communication between nodes. @@ -114,7 +114,7 @@ class IntraProcessManager */ RCLCPP_PUBLIC uint64_t - add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription); + add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); /// Unregister a subscription using the subscription's unique id. /** @@ -138,8 +138,7 @@ class IntraProcessManager */ RCLCPP_PUBLIC uint64_t - add_publisher( - PublisherBase::SharedPtr publisher); + add_publisher(rclcpp::PublisherBase::SharedPtr publisher); /// Unregister a publisher using the publisher's unique id. /** @@ -319,7 +318,7 @@ class IntraProcessManager get_subscription_count(uint64_t intra_process_publisher_id) const; RCLCPP_PUBLIC - SubscriptionIntraProcessBase::SharedPtr + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr get_subscription_intra_process(uint64_t intra_process_subscription_id); private: @@ -327,7 +326,7 @@ class IntraProcessManager { SubscriptionInfo() = default; - SubscriptionIntraProcessBase::SharedPtr subscription; + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription; rmw_qos_profile_t qos; const char * topic_name; bool use_take_shared_method; @@ -337,7 +336,7 @@ class IntraProcessManager { PublisherInfo() = default; - PublisherBase::WeakPtr publisher; + rclcpp::PublisherBase::WeakPtr publisher; rmw_qos_profile_t qos; const char * topic_name; }; @@ -383,8 +382,9 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - auto subscription = - std::static_pointer_cast>(subscription_base); + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); subscription->provide_intra_process_message(message); } @@ -410,8 +410,9 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription; - auto subscription = - std::static_pointer_cast>(subscription_base); + auto subscription = std::static_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcess + >(subscription_base); if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership @@ -436,7 +437,7 @@ class IntraProcessManager mutable std::shared_timed_mutex mutex_; }; -} // namespace intra_process_manager +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_ +#endif // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp similarity index 86% rename from rclcpp/include/rclcpp/subscription_intra_process.hpp rename to rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index be95275281..5fcf3f5342 100644 --- a/rclcpp/include/rclcpp/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ -#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #include @@ -25,14 +25,16 @@ #include "rcl/error_handling.h" #include "rclcpp/any_subscription_callback.hpp" -#include "rclcpp/buffers/intra_process_buffer.hpp" -#include "rclcpp/create_intra_process_buffer.hpp" -#include "rclcpp/subscription_intra_process_base.hpp" +#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/type_support_decl.hpp" #include "rclcpp/waitable.hpp" namespace rclcpp { +namespace experimental +{ template< typename MessageT, @@ -49,8 +51,11 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase using ConstMessageSharedPtr = std::shared_ptr; using MessageUniquePtr = std::unique_ptr; - using BufferUniquePtr = - typename intra_process_buffer::IntraProcessBuffer::UniquePtr; + using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + MessageT, + Alloc, + Deleter + >::UniquePtr; SubscriptionIntraProcess( AnySubscriptionCallback callback, @@ -67,7 +72,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } // Create the intra-process buffer. - buffer_ = rclcpp::create_intra_process_buffer( + buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, allocator); @@ -152,6 +157,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase BufferUniquePtr buffer_; }; +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp similarity index 87% rename from rclcpp/include/rclcpp/subscription_intra_process_base.hpp rename to rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 3eb725ab5f..7afd68abef 100644 --- a/rclcpp/include/rclcpp/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ -#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #include @@ -30,6 +30,8 @@ namespace rclcpp { +namespace experimental +{ class SubscriptionIntraProcessBase : public rclcpp::Waitable { @@ -79,6 +81,8 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::string topic_name_; rmw_qos_profile_t qos_profile_; }; + +} // namespace experimental } // namespace rclcpp -#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 56bca44dc3..ab0c8f806f 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -40,7 +40,6 @@ #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index c8214c59fb..2beb7f0d3c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -27,7 +27,6 @@ #include "rclcpp/publisher_factory.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_factory.hpp" -#include "rclcpp/subscription_intra_process.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index e02d86d60b..fa8e706994 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -31,7 +31,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/loaned_message.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -105,7 +105,7 @@ class Publisher : public PublisherBase if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { auto context = node_base->get_context(); // Get the intra process manager instance for this context. - auto ipm = context->get_sub_context(); + auto ipm = context->get_sub_context(); // Register the publisher with the intra process manager. if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { throw std::invalid_argument( diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index c6743188b8..4fbcf46bf6 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -41,16 +41,16 @@ namespace node_interfaces { class NodeBaseInterface; class NodeTopicsInterface; -} +} // namespace node_interfaces -namespace intra_process_manager +namespace experimental { /** * IntraProcessManager is forward declared here, avoiding a circular inclusion between * `intra_process_manager.hpp` and `publisher_base.hpp`. */ class IntraProcessManager; -} +} // namespace experimental class PublisherBase : public std::enable_shared_from_this { @@ -184,7 +184,7 @@ class PublisherBase : public std::enable_shared_from_this operator==(const rmw_gid_t * gid) const; using IntraProcessManagerSharedPtr = - std::shared_ptr; + std::shared_ptr; /// Implementation utility function used to setup intra process publishing after creation. RCLCPP_PUBLIC @@ -215,7 +215,7 @@ class PublisherBase : public std::enable_shared_from_this std::vector> event_handlers_; using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::weak_ptr; bool intra_process_is_enabled_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_publisher_id_; diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 65e3c1f11d..87def3cc17 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -24,9 +24,10 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_base.hpp" #include "rclcpp/publisher_options.hpp" -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 8473118505..fc13dd757c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -34,13 +34,13 @@ #include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_intra_process.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" @@ -125,18 +125,19 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); - auto subscription_intra_process = - std::make_shared>( - callback, - options.get_allocator(), - context, - topic_name, - qos.get_rmw_qos_profile(), - resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) - ); + auto subscription_intra_process = std::make_shared< + rclcpp::experimental::SubscriptionIntraProcess + >( + callback, + options.get_allocator(), + context, + topic_name, + qos.get_rmw_qos_profile(), + resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) + ); // Add it to the intra process manager. - using rclcpp::intra_process_manager::IntraProcessManager; + using rclcpp::experimental::IntraProcessManager; auto ipm = context->get_sub_context(); uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process); this->setup_intra_process(intra_process_subscription_id, ipm); diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 915496a357..06d161c62e 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -24,10 +24,11 @@ #include "rmw/rmw.h" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" -#include "rclcpp/subscription_intra_process_base.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -39,14 +40,14 @@ namespace node_interfaces class NodeBaseInterface; } // namespace node_interfaces -namespace intra_process_manager +namespace experimental { /** * IntraProcessManager is forward declared here, avoiding a circular inclusion between * `intra_process_manager.hpp` and `subscription_base.hpp`. */ class IntraProcessManager; -} +} // namespace experimental /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. @@ -179,7 +180,7 @@ class SubscriptionBase : public std::enable_shared_from_this can_loan_messages() const; using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::weak_ptr; /// Implemenation detail. RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 25caaafe6c..a0f265c803 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -24,11 +24,13 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/intra_process_buffer_type.hpp" -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033bbad..e02d10701d 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -18,7 +18,6 @@ using rclcpp::executor::AnyExecutable; AnyExecutable::AnyExecutable() : subscription(nullptr), - subscription_intra_process(nullptr), timer(nullptr), service(nullptr), client(nullptr), diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 5b17943a9d..0b9c9d6670 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" + +#include +#include +#include namespace rclcpp { -namespace intra_process_manager +namespace experimental { static std::atomic _next_unique_id {1}; @@ -61,8 +65,7 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su subscriptions_[id].subscription = subscription; subscriptions_[id].topic_name = subscription->get_topic_name(); subscriptions_[id].qos = subscription->get_actual_qos(); - subscriptions_[id].use_take_shared_method = - subscription->use_take_shared_method(); + subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method(); // adds the subscription id to all the matchable publishers for (auto & pair : publishers_) { @@ -82,13 +85,15 @@ IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) subscriptions_.erase(intra_process_subscription_id); for (auto & pair : pub_to_subs_) { - pair.second.take_shared_subscriptions.erase(std::remove( + pair.second.take_shared_subscriptions.erase( + std::remove( pair.second.take_shared_subscriptions.begin(), pair.second.take_shared_subscriptions.end(), intra_process_subscription_id), pair.second.take_shared_subscriptions.end()); - pair.second.take_ownership_subscriptions.erase(std::remove( + pair.second.take_ownership_subscriptions.erase( + std::remove( pair.second.take_ownership_subscriptions.begin(), pair.second.take_ownership_subscriptions.end(), intra_process_subscription_id), @@ -203,7 +208,8 @@ IntraProcessManager::can_communicate( // TODO(alsora): the following checks for qos compatibility should be provided by the RMW // a reliable subscription can't be connected with a best effort publisher - if (sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE && + if ( + sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE && pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) { return false; @@ -217,5 +223,5 @@ IntraProcessManager::can_communicate( return true; } -} // namespace intra_process_manager +} // namespace experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index cb5f3881fe..11de4a3278 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -16,7 +16,6 @@ #include -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/exceptions.hpp" using rclcpp::exceptions::throw_from_rcl_error; diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 0c489c51d4..cb96a14b51 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -32,7 +32,7 @@ #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 3383dc871c..7814793743 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -21,7 +21,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -179,9 +179,13 @@ SubscriptionBase::can_loan_messages() const rclcpp::Waitable::SharedPtr SubscriptionBase::get_intra_process_waitable() const { - // Get the intra process manager for this context. - auto context = node_base_->get_context(); - auto ipm = context->get_sub_context(); + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "SubscriptionBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } // Use the id to retrieve the subscription intra-process from the intra-process manager. return ipm->get_subscription_intra_process(intra_process_subscription_id_); diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 15880ce568..3b951f34de 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" -using rclcpp::SubscriptionIntraProcessBase; +using rclcpp::experimental::SubscriptionIntraProcessBase; bool SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) diff --git a/rclcpp/test/test_intra_process_buffer.cpp b/rclcpp/test/test_intra_process_buffer.cpp index 3d6a73ef61..87d2baece6 100644 --- a/rclcpp/test/test_intra_process_buffer.cpp +++ b/rclcpp/test/test_intra_process_buffer.cpp @@ -29,20 +29,20 @@ TEST(TestIntraProcessBuffer, constructor) { using Deleter = std::default_delete; using SharedMessageT = std::shared_ptr; using UniqueMessageT = std::unique_ptr; - using SharedIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; - using UniqueIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; auto shared_buffer_impl = - std::make_unique>(2); + std::make_unique>(2); SharedIntraProcessBufferT shared_intra_process_buffer(std::move(shared_buffer_impl)); EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method()); auto unique_buffer_impl = - std::make_unique>(2); + std::make_unique>(2); UniqueIntraProcessBufferT unique_intra_process_buffer(std::move(unique_buffer_impl)); @@ -60,11 +60,11 @@ TEST(TestIntraProcessBuffer, shared_buffer_add) { using Alloc = std::allocator; using Deleter = std::default_delete; using SharedMessageT = std::shared_ptr; - using SharedIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; auto buffer_impl = - std::make_unique>(2); + std::make_unique>(2); SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); @@ -108,11 +108,11 @@ TEST(TestIntraProcessBuffer, unique_buffer_add) { using Alloc = std::allocator; using Deleter = std::default_delete; using UniqueMessageT = std::unique_ptr; - using UniqueIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; auto buffer_impl = - std::make_unique>(2); + std::make_unique>(2); UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); @@ -155,11 +155,11 @@ TEST(TestIntraProcessBuffer, shared_buffer_consume) { using Deleter = std::default_delete; using SharedMessageT = std::shared_ptr; using UniqueMessageT = std::unique_ptr; - using SharedIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; auto buffer_impl = - std::make_unique>(2); + std::make_unique>(2); SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); @@ -204,11 +204,11 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { using Deleter = std::default_delete; using SharedMessageT = std::shared_ptr; using UniqueMessageT = std::unique_ptr; - using UniqueIntraProcessBufferT = - rclcpp::intra_process_buffer::TypedIntraProcessBuffer; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; auto buffer_impl = - std::make_unique>(2); + std::make_unique>(2); UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 31299c7c87..f12025a0d1 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -33,19 +33,19 @@ namespace rclcpp { // forward declaration -namespace intra_process_manager +namespace experimental { class IntraProcessManager; -} +} // namespace experimental namespace mock { using IntraProcessManagerSharedPtr = - std::shared_ptr; + std::shared_ptr; using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::weak_ptr; class PublisherBase { @@ -131,7 +131,9 @@ class Publisher : public PublisherBase namespace rclcpp { -namespace intra_process_buffer +namespace experimental +{ +namespace buffers { namespace mock { @@ -173,12 +175,15 @@ class IntraProcessBuffer }; } // namespace mock -} // namespace intra_process_buffer +} // namespace buffers +} // namespace experimental } // namespace rclcpp namespace rclcpp { +namespace experimental +{ namespace mock { @@ -221,7 +226,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase SubscriptionIntraProcess() : take_shared_method(false) { - buffer = std::make_unique>(); + buffer = std::make_unique>(); } void @@ -252,17 +257,18 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase bool take_shared_method; - typename intra_process_buffer::mock::IntraProcessBuffer::UniquePtr buffer; + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; }; } // namespace mock +} // namespace experimental } // namespace rclcpp // Prevent the header files of the mocked classes to be included #define RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_BASE_HPP_ -#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_ -#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ // Force ipm to use our mock publisher class. #define Publisher mock::Publisher #define PublisherBase mock::PublisherBase @@ -340,10 +346,10 @@ void Publisher::publish(MessageSharedPtr msg) - The count for the last publisher is expected to decrease to 1. */ TEST(TestIntraProcessManager, add_pub_sub) { - using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; using MessageT = rcl_interfaces::msg::Log; using PublisherT = rclcpp::mock::Publisher; - using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; auto ipm = std::make_shared(); @@ -407,10 +413,10 @@ TEST(TestIntraProcessManager, add_pub_sub) { - The received message is expected to be the same. */ TEST(TestIntraProcessManager, single_subscription) { - using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; using MessageT = rcl_interfaces::msg::Log; using PublisherT = rclcpp::mock::Publisher; - using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; auto ipm = std::make_shared(); @@ -461,10 +467,10 @@ TEST(TestIntraProcessManager, single_subscription) { - Both received messages are expected to be the same as the published one. */ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { - using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; using MessageT = rcl_interfaces::msg::Log; using PublisherT = rclcpp::mock::Publisher; - using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; auto ipm = std::make_shared(); @@ -566,10 +572,10 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { the other is expected to receive the published message */ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { - using IntraProcessManagerT = rclcpp::intra_process_manager::IntraProcessManager; + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; using MessageT = rcl_interfaces::msg::Log; using PublisherT = rclcpp::mock::Publisher; - using SubscriptionIntraProcessT = rclcpp::mock::SubscriptionIntraProcess; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; auto ipm = std::make_shared(); diff --git a/rclcpp/test/test_ring_buffer_implementation.cpp b/rclcpp/test/test_ring_buffer_implementation.cpp index 4b792bebd9..07dfd8d584 100644 --- a/rclcpp/test/test_ring_buffer_implementation.cpp +++ b/rclcpp/test/test_ring_buffer_implementation.cpp @@ -18,8 +18,8 @@ #include "gtest/gtest.h" -#include "rclcpp/buffers/buffer_implementation_base.hpp" -#include "rclcpp/buffers/ring_buffer_implementation.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" /* Construtctor @@ -27,9 +27,10 @@ TEST(TestRingBufferImplementation, constructor) { // Cannot create a buffer of size zero. EXPECT_THROW( - rclcpp::intra_process_buffer::RingBufferImplementation rb(0), std::invalid_argument); + rclcpp::experimental::buffers::RingBufferImplementation rb(0), + std::invalid_argument); - rclcpp::intra_process_buffer::RingBufferImplementation rb(1); + rclcpp::experimental::buffers::RingBufferImplementation rb(1); EXPECT_EQ(false, rb.has_data()); EXPECT_EQ(false, rb.is_full()); @@ -42,7 +43,7 @@ TEST(TestRingBufferImplementation, constructor) { - overwrite old data writing over the buffer capacity */ TEST(TestRingBufferImplementation, basic_usage) { - rclcpp::intra_process_buffer::RingBufferImplementation rb(2); + rclcpp::experimental::buffers::RingBufferImplementation rb(2); rb.enqueue('a'); From 6331bb23935dc3dc769b0de6ab5eaf8e43049c01 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 16:51:00 -0700 Subject: [PATCH 06/18] uncrustify Signed-off-by: William Woodall --- .../include/rclcpp/experimental/intra_process_manager.hpp | 4 ++-- .../rclcpp/experimental/subscription_intra_process.hpp | 2 +- .../include/rclcpp/strategies/allocator_memory_strategy.hpp | 6 +++--- rclcpp/include/rclcpp/subscription.hpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 034dc7361e..3f7ec6d519 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -384,7 +384,7 @@ class IntraProcessManager auto subscription = std::static_pointer_cast< rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); + >(subscription_base); subscription->provide_intra_process_message(message); } @@ -412,7 +412,7 @@ class IntraProcessManager auto subscription = std::static_pointer_cast< rclcpp::experimental::SubscriptionIntraProcess - >(subscription_base); + >(subscription_base); if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 5fcf3f5342..a948311709 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -55,7 +55,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase MessageT, Alloc, Deleter - >::UniquePtr; + >::UniquePtr; SubscriptionIntraProcess( AnySubscriptionCallback callback, diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index eab0949d08..339e5a0f8d 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -171,9 +171,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy }); group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) { - service_handles_.push_back(service->get_service_handle()); - return false; - }); + service_handles_.push_back(service->get_service_handle()); + return false; + }); group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); return false; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index fc13dd757c..43096ec59f 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -127,14 +127,14 @@ class Subscription : public SubscriptionBase auto context = node_base->get_context(); auto subscription_intra_process = std::make_shared< rclcpp::experimental::SubscriptionIntraProcess - >( + >( callback, options.get_allocator(), context, topic_name, qos.get_rmw_qos_profile(), resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) - ); + ); // Add it to the intra process manager. using rclcpp::experimental::IntraProcessManager; From 1c9d5346d7d6ee3cc8a5090ec87c0f936aadac2e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 17:34:02 -0700 Subject: [PATCH 07/18] fix issues with LoanedMessages after rebase Signed-off-by: William Woodall --- rclcpp/include/rclcpp/loaned_message.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 6f3226a776..09ad4a888f 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -18,6 +18,7 @@ #include #include +#include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/publisher_base.hpp" @@ -30,7 +31,7 @@ namespace rclcpp template> class LoanedMessage { - using MessageAllocatorTraits = allocator::AllocRebind; + using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; using MessageAllocator = typename MessageAllocatorTraits::allocator_type; public: From 75ba1eae456dc4e53c0f9cf41f5e83aeab300b3f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 18:48:08 -0700 Subject: [PATCH 08/18] more fixups Signed-off-by: William Woodall --- rclcpp/include/rclcpp/publisher.hpp | 2 +- .../include/rclcpp_lifecycle/lifecycle_node_impl.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index fa8e706994..acc0eaa318 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -229,7 +229,7 @@ class Publisher : public PublisherBase } else { // we don't release the ownership, let the middleware copy the ros message // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. - this->do_inter_process_publish(&loaned_msg.get()); + this->do_inter_process_publish(loaned_msg.get()); } } diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 6fe3532e81..c09031926a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,8 +22,8 @@ #include #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" From e1afde5ad456f1f45b3768457b82e88e32d6bbdd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 21:20:09 -0700 Subject: [PATCH 09/18] readd logic for avoiding in compatible QoS Signed-off-by: William Woodall --- rclcpp/include/rclcpp/publisher.hpp | 4 ++++ rclcpp/include/rclcpp/subscription.hpp | 14 ++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index acc0eaa318..49f944bbfc 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -115,6 +115,10 @@ class Publisher : public PublisherBase throw std::invalid_argument( "intraprocess communication is not allowed with a zero qos history depth value"); } + if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); this->setup_intra_process( intra_process_publisher_id, diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 43096ec59f..c4a3f746de 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -123,6 +123,20 @@ class Subscription : public SubscriptionBase if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { using rclcpp::detail::resolve_intra_process_buffer_type; + // Check if the QoS is compatible with intra-process. + if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) { + throw std::invalid_argument( + "intraprocess communication is not allowed with keep all history qos policy"); + } + if (qos.get_rmw_qos_profile().depth == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); auto subscription_intra_process = std::make_shared< From 9b3299485726a61ec7ceb075d6348d88de3deeb6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 18 Oct 2019 21:20:46 -0700 Subject: [PATCH 10/18] avoid an error when intra process is disabled Signed-off-by: William Woodall --- rclcpp/src/rclcpp/subscription_base.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 7814793743..314a8b837c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -179,6 +179,10 @@ SubscriptionBase::can_loan_messages() const rclcpp::Waitable::SharedPtr SubscriptionBase::get_intra_process_waitable() const { + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } // Get the intra process manager. auto ipm = weak_ipm_.lock(); if (!ipm) { From 3013e02cc0f4a97083510a29a99f3d99304d6d87 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 16:06:34 -0700 Subject: [PATCH 11/18] change intra process to preserve pointer in cyclic_pipeline Signed-off-by: William Woodall --- .../experimental/intra_process_manager.hpp | 129 ++++++++---------- rclcpp/include/rclcpp/publisher.hpp | 11 +- rclcpp/test/test_intra_process_manager.cpp | 45 ++---- 3 files changed, 73 insertions(+), 112 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 3f7ec6d519..7c216c6725 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -150,75 +150,6 @@ class IntraProcessManager void remove_publisher(uint64_t intra_process_publisher_id); - /// Publishes an intra-process message, passed as a shared pointer. - /** - * This is one of the two methods for publishing intra-process. - * - * Using the intra-process publisher id, a list of recipients is obtained. - * This list is split in half, depending whether they require ownership or not. - * - * This particular method takes a shared pointer as input. - * This can be safely passed to all the subscriptions that do not require ownership. - * No copies are needed in this case. - * For every subscription requiring ownership, a copy has to be made. - * - * The total number of copies is always equal to the number - * of subscriptions requiring ownership. - * - * This method can throw an exception if the publisher id is not found or - * if the publisher shared_ptr given to add_publisher has gone out of scope. - * - * This method does allocate memory. - * - * \param intra_process_publisher_id the id of the publisher of this message. - * \param message the message that is being stored. - */ - template< - typename MessageT, - typename Alloc = std::allocator> - void - do_intra_process_publish( - uint64_t intra_process_publisher_id, - std::shared_ptr message, - std::shared_ptr::allocator_type> allocator) - { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - - 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; - } - const auto & sub_ids = publisher_it->second; - - this->template add_shared_msg_to_buffers(message, sub_ids.take_shared_subscriptions); - - if (sub_ids.take_ownership_subscriptions.size() > 0) { - MessageUniquePtr unique_msg; - MessageDeleter * deleter = std::get_deleter(message); - auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); - MessageAllocTraits::construct(*allocator.get(), ptr, *message); - if (deleter) { - unique_msg = MessageUniquePtr(ptr, *deleter); - } else { - unique_msg = MessageUniquePtr(ptr); - } - - this->template add_owned_msg_to_buffers( - std::move(unique_msg), - sub_ids.take_ownership_subscriptions, - allocator); - } - } - /// Publishes an intra-process message, passed as a unique pointer. /** * This is one of the two methods for publishing intra-process. @@ -253,6 +184,7 @@ class IntraProcessManager std::shared_ptr::allocator_type> allocator) { using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; std::shared_lock lock(mutex_); @@ -293,10 +225,7 @@ class IntraProcessManager { // Construct a new shared pointer from the message // for the buffers that do not require ownership - std::shared_ptr shared_msg; - auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); - MessageAllocTraits::construct(*allocator.get(), ptr, *message); - shared_msg = std::shared_ptr(ptr); + auto shared_msg = std::allocate_shared(*allocator, *message); this->template add_shared_msg_to_buffers(shared_msg, sub_ids.take_shared_subscriptions); @@ -307,6 +236,60 @@ class IntraProcessManager } } + template< + typename MessageT, + 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, + std::shared_ptr::allocator_type> allocator) + { + 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); + } + 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); + + if (!sub_ids.take_shared_subscriptions.empty()) { + 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( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + + return shared_msg; + } + } + /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC bool diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 49f944bbfc..31de94ea70 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -171,8 +171,7 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - std::shared_ptr shared_msg = std::move(msg); - this->do_intra_process_publish(shared_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)); @@ -298,7 +297,7 @@ class Publisher : public PublisherBase } void - do_intra_process_publish(std::shared_ptr msg) + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -315,8 +314,8 @@ class Publisher : public PublisherBase message_allocator_); } - void - do_intra_process_publish(std::unique_ptr msg) + std::shared_ptr + do_intra_process_publish_and_return_shared(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -327,7 +326,7 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + return ipm->template do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), message_allocator_); diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index f12025a0d1..b296a30816 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -121,7 +121,6 @@ class Publisher : public PublisherBase // The following functions use the IntraProcessManager // so they are declared after including it to avoid "invalid use of incomplete type" void publish(MessageUniquePtr msg); - void publish(MessageSharedPtr msg); std::shared_ptr message_allocator_; }; @@ -308,26 +307,6 @@ void Publisher::publish(MessageUniquePtr msg) message_allocator_); } -// This function is actually deprecated in the real rclcpp::publisher, but -// here it is used to mimic what happens when publishing both inter and intra-process -template -void Publisher::publish(MessageSharedPtr 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_, - msg, - message_allocator_); -} - } // namespace mock } // namespace rclcpp @@ -448,9 +427,9 @@ TEST(TestIntraProcessManager, single_subscription) { ASSERT_EQ(original_message_pointer, received_message_pointer_2); ASSERT_EQ(0u, received_message_pointer_1); - auto shared_msg = std::make_shared(); - original_message_pointer = reinterpret_cast(shared_msg.get()); - p1->publish(shared_msg); + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); received_message_pointer_2 = s2->pop(); ASSERT_EQ(original_message_pointer, received_message_pointer_2); } @@ -525,9 +504,9 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { s6->take_shared_method = false; auto s6_id = ipm->add_subscription(s6); - auto shared_msg = std::make_shared(); - original_message_pointer = reinterpret_cast(shared_msg.get()); - p1->publish(shared_msg); + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); auto received_message_pointer_5 = s5->pop(); auto received_message_pointer_6 = s6->pop(); ASSERT_NE(original_message_pointer, received_message_pointer_5); @@ -546,9 +525,9 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s8_id = ipm->add_subscription(s8); (void)s8_id; - shared_msg = std::make_shared(); - original_message_pointer = reinterpret_cast(shared_msg.get()); - p1->publish(shared_msg); + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); auto received_message_pointer_7 = s7->pop(); auto received_message_pointer_8 = s8->pop(); ASSERT_EQ(original_message_pointer, received_message_pointer_7); @@ -682,9 +661,9 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s11_id = ipm->add_subscription(s11); (void)s11_id; - auto shared_msg = std::make_shared(); - original_message_pointer = reinterpret_cast(shared_msg.get()); - p1->publish(shared_msg); + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); auto received_message_pointer_10 = s10->pop(); auto received_message_pointer_11 = s11->pop(); ASSERT_NE(original_message_pointer, received_message_pointer_10); From 035f145f2d10028e2de6f2ad69dfb9913f0c41d1 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 16:06:52 -0700 Subject: [PATCH 12/18] fix issue matching topics in intra process Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index c4a3f746de..7cbf354fc2 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -145,7 +145,7 @@ class Subscription : public SubscriptionBase callback, options.get_allocator(), context, - topic_name, + this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos.get_rmw_qos_profile(), resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) ); From 437c1f7248ef943030bc9bc221480b91b61ad49b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 16:51:26 -0700 Subject: [PATCH 13/18] fix some issues with the tests after latest behavior change Signed-off-by: William Woodall --- rclcpp/test/test_intra_process_manager.cpp | 7 ++++--- rclcpp/test/test_publisher.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index b296a30816..b71fb1d061 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -510,7 +510,8 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto received_message_pointer_5 = s5->pop(); auto received_message_pointer_6 = s6->pop(); ASSERT_NE(original_message_pointer, received_message_pointer_5); - ASSERT_NE(original_message_pointer, received_message_pointer_6); + // Someone gets the original unique_ptr, the last one to take. + ASSERT_EQ(original_message_pointer, received_message_pointer_6); ipm->remove_subscription(s5_id); ipm->remove_subscription(s6_id); @@ -666,6 +667,6 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { p1->publish(std::move(unique_msg)); auto received_message_pointer_10 = s10->pop(); auto received_message_pointer_11 = s11->pop(); - ASSERT_NE(original_message_pointer, received_message_pointer_10); - ASSERT_EQ(original_message_pointer, received_message_pointer_11); + EXPECT_EQ(original_message_pointer, received_message_pointer_10); + EXPECT_NE(original_message_pointer, received_message_pointer_11); } diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp index 3f75a95168..6719197ce9 100644 --- a/rclcpp/test/test_publisher.cpp +++ b/rclcpp/test/test_publisher.cpp @@ -28,7 +28,9 @@ class TestPublisher : public ::testing::Test public: static void SetUpTestCase() { - rclcpp::init(0, nullptr); + if (!rclcpp::is_initialized()) { + rclcpp::init(0, nullptr); + } } protected: @@ -47,7 +49,7 @@ class TestPublisher : public ::testing::Test struct TestParameters { - TestParameters(rclcpp::QoS qos, std::string description) + TestParameters(rclcpp::QoS qos, const std::string & description) : qos(qos), description(description) {} rclcpp::QoS qos; std::string description; From 41eab14387f1ad8919ac0e5c96b51842dd534bf3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 17:05:41 -0700 Subject: [PATCH 14/18] address review feedback Signed-off-by: William Woodall --- .../rclcpp/detail/resolve_use_intra_process.hpp | 2 +- .../buffers/intra_process_buffer.hpp | 2 +- .../buffers/ring_buffer_implementation.hpp | 17 +++++++---------- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp index cefdee2b22..9098bfe695 100644 --- a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp @@ -25,7 +25,7 @@ namespace rclcpp namespace detail { -/// Return the whether or not intra process is enabled, resolving "NodeDefault" if needed. +/// Return whether or not intra process is enabled, resolving "NodeDefault" if needed. template bool resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base) diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index a8634c5a3a..aef38a028d 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -178,7 +178,7 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer typename std::enable_if< - (std::is_same::value), + std::is_same::value, MessageSharedPtr >::type consume_shared_impl() diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 724a72df54..25caf99138 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -30,8 +30,6 @@ #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" -#include "iostream" - namespace rclcpp { namespace experimental @@ -44,13 +42,12 @@ class RingBufferImplementation : public BufferImplementationBase { public: explicit RingBufferImplementation(size_t capacity) - : ring_buffer_(capacity) + : ring_buffer_(capacity), + capacity_(capacity), + write_index_(capacity_ - 1), + read_index_(0), + size_(0) { - capacity_ = capacity; - write_index_ = capacity_ - 1; - read_index_ = 0; - size_ = 0; - if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); } @@ -74,13 +71,13 @@ class RingBufferImplementation : public BufferImplementationBase BufferT dequeue() { + std::lock_guard lock(mutex_); + if (!has_data()) { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); throw std::runtime_error("Calling dequeue on empty intra-process buffer"); } - std::lock_guard lock(mutex_); - auto request = std::move(ring_buffer_[read_index_]); read_index_ = next(read_index_); From 0309c6aabe81ee3de08ebb872bebf3c5466a44fc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 20:46:48 -0700 Subject: [PATCH 15/18] fix the initialization order Signed-off-by: William Woodall --- .../experimental/buffers/ring_buffer_implementation.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 25caf99138..3e75039da2 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -43,10 +43,10 @@ class RingBufferImplementation : public BufferImplementationBase public: explicit RingBufferImplementation(size_t capacity) : ring_buffer_(capacity), - capacity_(capacity), write_index_(capacity_ - 1), read_index_(0), - size_(0) + size_(0), + capacity_(capacity) { if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); From ee3c345deecc9e46956be9e4798e85a23c9fc6d6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 21:17:24 -0700 Subject: [PATCH 16/18] avoid possible loss of data warning Signed-off-by: William Woodall --- .../rclcpp/experimental/buffers/ring_buffer_implementation.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 3e75039da2..b2f661108b 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -86,7 +86,7 @@ class RingBufferImplementation : public BufferImplementationBase return request; } - inline uint32_t next(uint32_t val) + inline size_t next(size_t val) { return (val + 1) % capacity_; } From 7a44cee167f1918a0c88722d4c4914832e0bc5a2 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 21:51:02 -0700 Subject: [PATCH 17/18] more fixes related to initialization Signed-off-by: William Woodall --- .../experimental/buffers/ring_buffer_implementation.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index b2f661108b..ea37c23019 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -42,11 +42,11 @@ class RingBufferImplementation : public BufferImplementationBase { public: explicit RingBufferImplementation(size_t capacity) - : ring_buffer_(capacity), + : capacity_(capacity), + ring_buffer_(capacity), write_index_(capacity_ - 1), read_index_(0), - size_(0), - capacity_(capacity) + size_(0) { if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); @@ -104,12 +104,13 @@ class RingBufferImplementation : public BufferImplementationBase void clear() {} private: + size_t capacity_; + std::vector ring_buffer_; size_t write_index_; size_t read_index_; size_t size_; - size_t capacity_; std::mutex mutex_; }; From ca59a8ac52c60473b9219d90fbcba00c110b4791 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Sun, 20 Oct 2019 23:58:32 -0700 Subject: [PATCH 18/18] fix use of custom allocators Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 7cbf354fc2..81310d72c6 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -140,12 +140,15 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); auto subscription_intra_process = std::make_shared< - rclcpp::experimental::SubscriptionIntraProcess - >( + rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type + >>( callback, options.get_allocator(), context, - this->get_topic_name(), // important to get like this, as it has the fully-qualified name + this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos.get_rmw_qos_profile(), resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) );