diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 16a67f0..275b9c6 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,17 +200,7 @@ 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 - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp" - ) - endif() - ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) + 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" @@ -219,6 +209,27 @@ if(BUILD_TESTING) "rosidl_generator_cpp" "rosidl_typesupport_cpp" ) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) + endif() + 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_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 7b9bac0..aae25dc 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/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 26aeaba..f6b669f 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -227,7 +227,7 @@ public: 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/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 0f267cb..9248254 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/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp new file mode 100644 index 0000000..e7196a1 --- /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/detail/resolve_use_intra_process.hpp b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp index cab3542..9098bfe 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 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/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 095a765..cbeda82 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -305,11 +305,6 @@ protected: 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/experimental/README.md b/rclcpp/include/rclcpp/experimental/README.md new file mode 100644 index 0000000..38ca07c --- /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/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp new file mode 100644 index 0000000..1e53461 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -0,0 +1,42 @@ +// 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__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +template +class BufferImplementationBase +{ +public: + virtual ~BufferImplementationBase() {} + + virtual BufferT dequeue() = 0; + virtual void enqueue(BufferT request) = 0; + + virtual void clear() = 0; + virtual bool has_data() const = 0; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp new file mode 100644 index 0000000..aef38a0 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -0,0 +1,241 @@ +// 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__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/macros.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~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) + + virtual ~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; + + explicit + 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()); + } + } + + virtual ~TypedIntraProcessBuffer() {} + + void add_shared(MessageSharedPtr msg) override + { + add_shared_impl(std::move(msg)); + } + + void add_unique(MessageUniquePtr msg) override + { + buffer_->enqueue(std::move(msg)); + } + + MessageSharedPtr consume_shared() override + { + return consume_shared_impl(); + } + + MessageUniquePtr consume_unique() override + { + return consume_unique_impl(); + } + + bool has_data() const override + { + return buffer_->has_data(); + } + + void clear() override + { + buffer_->clear(); + } + + bool use_take_shared_method() const override + { + 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 buffers +} // namespace experimental +} // namespace rclcpp + + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp new file mode 100644 index 0000000..ea37c23 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -0,0 +1,122 @@ +// 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__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +template +class RingBufferImplementation : public BufferImplementationBase +{ +public: + explicit RingBufferImplementation(size_t capacity) + : capacity_(capacity), + ring_buffer_(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() + { + 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"); + } + + auto request = std::move(ring_buffer_[read_index_]); + read_index_ = next(read_index_); + + size_--; + + return request; + } + + inline size_t next(size_t val) + { + return (val + 1) % capacity_; + } + + inline bool has_data() const + { + return size_ != 0; + } + + inline bool is_full() + { + return size_ == capacity_; + } + + void clear() {} + +private: + size_t capacity_; + + std::vector ring_buffer_; + + size_t write_index_; + size_t read_index_; + size_t size_; + + std::mutex mutex_; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp new file mode 100644 index 0000000..ff57a84 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -0,0 +1,99 @@ +// 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__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rcl/subscription.h" + +#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 rclcpp::experimental::buffers::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; + + using rclcpp::experimental::buffers::IntraProcessBuffer; + typename 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 experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp new file mode 100644 index 0000000..7c216c6 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -0,0 +1,426 @@ +// 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__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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/visibility_control.hpp" + +namespace rclcpp +{ + +namespace experimental +{ + +/// 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 + * 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 do not share access to the same instance of 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 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, 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 given to this class. + * Using the publisher id, a list of recipients for the message is selected. + * 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 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 this class to operate efficiently by performing the + * fewest number of copies of the message required. + * + * This class is neither CopyConstructable nor CopyAssignable. + */ +class IntraProcessManager +{ +private: + RCLCPP_DISABLE_COPY(IntraProcessManager) + +public: + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) + + RCLCPP_PUBLIC + IntraProcessManager(); + + RCLCPP_PUBLIC + virtual ~IntraProcessManager(); + + /// Register a subscription with the manager, returns subscriptions unique id. + /** + * This method stores the subscription intra process object, together with + * the information of its wrapped subscription (i.e. topic name and QoS). + * + * In addition this generates a unique intra process id for the subscription. + * + * \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(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + + /// Unregister a subscription using the subscription's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_subscription_id id of the subscription to remove. + */ + RCLCPP_PUBLIC + void + remove_subscription(uint64_t intra_process_subscription_id); + + /// Register a publisher with the manager, returns the publisher unique id. + /** + * This method stores the publisher intra process object, together with + * the information of its wrapped publisher (i.e. topic name and QoS). + * + * In addition this generates a unique intra process id for the publisher. + * + * \param publisher publisher to be registered with the manager. + * \return an unsigned 64-bit integer which is the publisher's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_publisher(rclcpp::PublisherBase::SharedPtr publisher); + + /// Unregister a publisher using the publisher's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_publisher_id id of the publisher to remove. + */ + RCLCPP_PUBLIC + void + remove_publisher(uint64_t intra_process_publisher_id); + + /// Publishes an intra-process message, passed as a unique 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 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. + * + * This method can save an additional copy compared to the shared pointer one. + * + * 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, + typename Deleter = std::default_delete> + void + do_intra_process_publish( + 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; + } + 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 + auto shared_msg = std::allocate_shared(*allocator, *message); + + this->template add_shared_msg_to_buffers(shared_msg, + sub_ids.take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + } + + template< + typename MessageT, + typename 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 + matches_any_publishers(const rmw_gid_t * id) const; + + /// 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 + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr + get_subscription_intra_process(uint64_t intra_process_subscription_id); + +private: + struct SubscriptionInfo + { + SubscriptionInfo() = default; + + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription; + rmw_qos_profile_t qos; + const char * topic_name; + bool use_take_shared_method; + }; + + struct PublisherInfo + { + PublisherInfo() = default; + + rclcpp::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; + + using PublisherMap = + std::unordered_map; + + using PublisherToSubscriptionIdsMap = + std::unordered_map; + + RCLCPP_PUBLIC + static + uint64_t + get_next_unique_id(); + + 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< + rclcpp::experimental::SubscriptionIntraProcess + >(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< + rclcpp::experimental::SubscriptionIntraProcess + >(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 experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp new file mode 100644 index 0000000..a948311 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -0,0 +1,163 @@ +// 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ + +#include + +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.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, + 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 rclcpp::experimental::buffers::IntraProcessBuffer< + MessageT, + Alloc, + Deleter + >::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::experimental::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 experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp new file mode 100644 index 0000000..7afd68a --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -0,0 +1,88 @@ +// 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__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 +{ +namespace experimental +{ + +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 experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ 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 0000000..828a96c --- /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 deleted file mode 100644 index 5d3abed..0000000 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ /dev/null @@ -1,420 +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_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_ - -#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/macros.hpp" -#include "rclcpp/publisher_base.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace intra_process_manager -{ - -/// This class facilitates 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 - * 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. - * - * 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 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. - * - * 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. - * - * 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. - * - * 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. - * - * /TODO(wjwwood): update to include information about handling latching. - * /TODO(wjwwood): consider thread safety of the class. - * - * This class is neither CopyConstructable nor CopyAssignable. - */ -class IntraProcessManager -{ -private: - RCLCPP_DISABLE_COPY(IntraProcessManager) - -public: - RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) - - RCLCPP_PUBLIC - explicit IntraProcessManager( - IntraProcessManagerImplBase::SharedPtr state = create_default_impl()); - - 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 is normally called during the creation of a subscription, - * but after it creates the internal intra process rmw_subscription_t. - * - * This method will allocate memory. - * - * \param subscription the Subscription to register. - * \return an unsigned 64-bit integer which is the subscription's unique id. - */ - RCLCPP_PUBLIC - uint64_t - add_subscription(SubscriptionBase::SharedPtr subscription); - - /// Unregister a subscription using the subscription's unique id. - /** - * This method does not allocate memory. - * - * \param intra_process_subscription_id id of the subscription to remove. - */ - RCLCPP_PUBLIC - void - remove_subscription(uint64_t intra_process_subscription_id); - - /// 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 will allocate memory. - * - * \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); - - /// Unregister a publisher using the publisher's unique id. - /** - * This method does not allocate memory. - * - * \param intra_process_publisher_id id of the publisher to remove. - */ - RCLCPP_PUBLIC - void - remove_publisher(uint64_t intra_process_publisher_id); - - /// Store a message in the manager, and return the message sequence number. - /** - * 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. - * - * 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. - * - * 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. - * \return the message sequence number. - */ - template< - typename MessageT, typename Alloc = std::allocator> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::shared_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"); - } - - // 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); - - // Return the message sequence which is sent to the subscription. - return message_seq; - } - - 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"); - } - - // 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. - - impl_->store_intra_process_message(intra_process_publisher_id, message_seq); - - // Return the message sequence which is sent to the subscription. - return message_seq; - } - - /// Take an intra process message. - /** - * The intra_process_publisher_id and message_sequence_number parameters - * uniquely identify a message instance, which should be taken. - * - * 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. - * - * 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. - * - * The message parameter can be set to nullptr if: - * - * - 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 may allocate memory to copy the stored message. - * - * \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. - */ - template< - typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> - void - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - std::unique_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) { - 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); - } - } - - 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) { - 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); - } - } - - /// Return true if the given rmw_gid_t matches any stored Publishers. - RCLCPP_PUBLIC - bool - matches_any_publishers(const rmw_gid_t * id) const; - - /// Return the number of intraprocess subscriptions to a topic, given the publisher id. - RCLCPP_PUBLIC - size_t - get_subscription_count(uint64_t intra_process_publisher_id) const; - -private: - RCLCPP_PUBLIC - static uint64_t - get_next_unique_id(); - - IntraProcessManagerImplBase::SharedPtr impl_; - std::mutex take_mutex_; -}; - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_ 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 ab29af7..0000000 --- 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/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 6f3226a..09ad4a8 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: diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index f224abe..d13c5d2 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 c8fdf64..0000000 --- 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_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 56bca44..ab0c8f8 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/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 9bde46e..31de94e 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -28,12 +28,10 @@ #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" -#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" @@ -101,12 +99,13 @@ public: { // 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)) { 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( @@ -116,26 +115,20 @@ public: 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, - ipm, - options.template to_rcl_publisher_options(qos)); + ipm); } } 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 +158,7 @@ public: 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 +167,14 @@ public: // 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); + auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(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 +184,7 @@ public: // 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. @@ -246,7 +232,7 @@ public: } 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()); } } @@ -258,9 +244,9 @@ public: 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 +276,6 @@ protected: } } - 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 +296,8 @@ protected: } } - uint64_t - store_intra_process_message( - uint64_t publisher_id, - std::shared_ptr msg) + void + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -343,17 +305,17 @@ protected: "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) + std::shared_ptr + do_intra_process_publish_and_return_shared(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -361,11 +323,13 @@ protected: "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; + + return ipm->template do_intra_process_publish_and_return_shared( + 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 c5837f6..4fbcf46 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" @@ -42,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 { @@ -97,12 +96,6 @@ public: 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 @@ -191,21 +184,14 @@ public: operator==(const rmw_gid_t * gid) const; 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; + std::shared_ptr; /// 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,18 +211,16 @@ protected: 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_; using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::weak_ptr; bool intra_process_is_enabled_; IntraProcessManagerWeakPtr weak_ipm_; 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/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index 65e3c1f..87def3c 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/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1ada134..339e5a0 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -167,12 +167,9 @@ public: 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) { service_handles_.push_back(service->get_service_handle()); return false; @@ -262,11 +259,6 @@ public: 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 +274,7 @@ public: 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 3789b05..81310d7 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,13 +29,13 @@ #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/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" @@ -118,6 +118,48 @@ public: 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; + + // 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< + 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 + 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; + 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(), @@ -129,23 +171,14 @@ public: } /// 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. @@ -210,117 +243,12 @@ public: 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 + bool use_take_shared_method() const { - 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_; + return any_callback_.use_take_shared_method(); } 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) - { - 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); - } - RCLCPP_DISABLE_COPY(Subscription) AnySubscriptionCallback any_callback_; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f3df997..06d161c 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -21,11 +21,11 @@ #include "rcl/subscription.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - #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" @@ -40,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. @@ -89,10 +89,6 @@ public: 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 +154,6 @@ public: 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; @@ -191,15 +180,19 @@ public: can_loan_messages() const; using IntraProcessManagerWeakPtr = - std::weak_ptr; + std::weak_ptr; /// Implemenation detail. RCLCPP_PUBLIC void setup_intra_process( uint64_t intra_process_subscription_id, - IntraProcessManagerWeakPtr weak_ipm, - const rcl_subscription_options_t & intra_process_options); + IntraProcessManagerWeakPtr weak_ipm); + + /// Return the waitable for intra-process, or nullptr if intra-process is not setup. + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable() const; protected: template @@ -216,6 +209,12 @@ protected: event_handlers_.emplace_back(handler); } + RCLCPP_PUBLIC + 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/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a5175c8..a0f265c 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -24,19 +24,22 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/subscription_traits.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/intra_process_buffer_type.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 { -/// 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 @@ -59,7 +62,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/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 8ee6e7f..64fcac5 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/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033b..e02d107 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/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 48f8405..6953610 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 56e30ab..0b9c9d6 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -12,69 +12,153 @@ // 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}; -IntraProcessManager::IntraProcessManager( - rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl) -: impl_(impl) +IntraProcessManager::IntraProcessManager() {} IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher, - size_t buffer_size) +IntraProcessManager::add_publisher(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().get_rmw_qos_profile(); + + // 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) +IntraProcessManager::add_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 +183,45 @@ IntraProcessManager::get_next_unique_id() return next_id; } -} // namespace intra_process_manager +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 experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp b/rclcpp/src/rclcpp/intra_process_manager_impl.cpp deleted file mode 100644 index 2fa1d2f..0000000 --- a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp +++ /dev/null @@ -1,23 +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 "rclcpp/intra_process_manager_impl.hpp" - -#include - -rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr -rclcpp::intra_process_manager::create_default_impl() -{ - return std::make_shared>(); -} diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 26c3e3c..5a2a5da 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 74e034e..11de4a3 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; @@ -95,18 +94,24 @@ NodeTopics::add_subscription( } callback_group->add_subscription(subscription); + for (auto & subscription_event : subscription->get_event_handlers()) { callback_group->add_waitable(subscription_event); } + 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(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/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 38d3995..cb96a14 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" @@ -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 99558d2..314a8b8 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" @@ -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), @@ -58,10 +59,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 +114,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 +163,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 +175,37 @@ SubscriptionBase::can_loan_messages() const { return rcl_subscription_can_loan_messages(subscription_handle_.get()); } + +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) { + 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_); +} + +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 0000000..3b951f3 --- /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/experimental/subscription_intra_process_base.hpp" + +using rclcpp::experimental::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 4d74d70..ccf05bf 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -249,6 +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()) { + if (it.second->value.type != ParameterType::PARAMETER_BOOL) { + RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); + continue; + } 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 0000000..87d2bae --- /dev/null +++ b/rclcpp/test/test_intra_process_buffer.cpp @@ -0,0 +1,240 @@ +// 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::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); + + 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); + + UniqueIntraProcessBufferT unique_intra_process_buffer(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::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + 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()); + + 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::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + 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()); + + 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::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + 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()); + + 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::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + 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()); + 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 fe3c535..b71fb1d 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -12,41 +12,78 @@ // 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 "rclcpp/qos.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 experimental +{ +class IntraProcessManager; +} // namespace experimental + 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) {} + : qos(rclcpp::QoS(10)), + 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; + } + + rclcpp::QoS + get_actual_qos() const + { + return qos; + } + + bool + operator==(const rmw_gid_t & gid) const + { + (void)gid; + return false; } bool @@ -56,16 +93,10 @@ public: return false; } - virtual - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - make_mapped_ring_buffer(size_t size) const - { - (void)size; - return nullptr; - } - - std::string mock_topic_name; - size_t mock_queue_size; + rclcpp::QoS qos; + std::string topic_name; + uint64_t intra_process_publisher_id_; + IntraProcessManagerWeakPtr weak_ipm_; }; template> @@ -76,28 +107,22 @@ public: 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 = rclcpp::QoS(10); + 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); - std::shared_ptr get_allocator() - { - return allocator_; - } + std::shared_ptr message_allocator_; }; } // namespace mock @@ -105,756 +130,543 @@ public: namespace rclcpp { +namespace experimental +{ +namespace buffers +{ +namespace mock +{ +template +class IntraProcessBuffer +{ +public: + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) + + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; + } + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + } + + void pop(std::uintptr_t & msg_ptr) + { + msg_ptr = message_ptr; + message_ptr = 0; + } + + // 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 buffers +} // namespace experimental +} // namespace rclcpp + + +namespace rclcpp +{ +namespace experimental +{ namespace mock { -class SubscriptionBase +class SubscriptionIntraProcessBase { public: - RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionBase) + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - SubscriptionBase() - : mock_topic_name(""), mock_queue_size(0) {} + SubscriptionIntraProcessBase() + : qos_profile(rmw_qos_profile_default), topic_name("topic") + {} - const char * get_topic_name() const + virtual ~SubscriptionIntraProcessBase() {} + + virtual bool + use_take_shared_method() const = 0; + + rmw_qos_profile_t + get_actual_qos() { - return mock_topic_name.c_str(); - } - size_t get_queue_size() const - { - return mock_queue_size; + return qos_profile; } - std::string mock_topic_name; - size_t mock_queue_size; + const char * + get_topic_name() + { + return topic_name; + } + + rmw_qos_profile_t qos_profile; + const char * topic_name; }; +template +class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + SubscriptionIntraProcess() + : take_shared_method(false) + { + buffer = std::make_unique>(); + } + + void + provide_intra_process_message(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_message(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + + std::uintptr_t + pop() + { + std::uintptr_t ptr; + buffer->pop(ptr); + return ptr; + } + + bool + use_take_shared_method() const + { + return take_shared_method; + } + + bool take_shared_method; + + 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__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 +#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 + +using ::testing::_; +using ::testing::UnorderedElementsAre; + +namespace rclcpp +{ +namespace mock +{ + +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"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + message_allocator_); +} + } // namespace mock } // 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 - /* - 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. + 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, nominal) { - rclcpp::intra_process_manager::IntraProcessManager ipm; +TEST(TestIntraProcessManager, add_pub_sub) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; + auto ipm = std::make_shared(); - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal2"; - p2->mock_queue_size = 10; + auto p1 = std::make_shared(); + p1->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; + auto p2 = std::make_shared(); + p2->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + p2->topic_name = "different_topic_name"; - auto p1_id = ipm.add_publisher(p1); - auto p2_id = ipm.add_publisher(p2); - auto s1_id = ipm.add_subscription(s1); + auto s1 = std::make_shared(); + s1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - 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_id = ipm->add_publisher(p1); + auto p2_id = ipm->add_publisher(p2); + auto s1_id = ipm->add_subscription(s1); - 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); + bool unique_ids = p1_id != p2_id && p2_id != s1_id; + ASSERT_TRUE(unique_ids); - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); + 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 p2_m1_id = ipm.store_intra_process_message(p2_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); + auto p3 = std::make_shared(); + p3->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - 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(); + auto s2 = std::make_shared(); + s2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - 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()); - } + auto s2_id = ipm->add_subscription(s2); + auto p3_id = ipm->add_publisher(p3); - 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. + 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_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)); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 45; - ipm_msg->publisher_id = 45; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 46; - ipm_msg->publisher_id = 46; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); + 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 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. + 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, remove_publisher_before_trying_to_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; +TEST(TestIntraProcessManager, single_subscription) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; + auto ipm = std::make_shared(); - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); - auto p1_id = ipm.add_publisher(p1); - auto s1_id = ipm.add_subscription(s1); + auto s1 = std::make_shared(); + s1->take_shared_method = false; + 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 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); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, std::move(unique_msg)); - ASSERT_EQ(nullptr, unique_msg); + 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; - ipm.remove_publisher(p1_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); - 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. + 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); } /* - 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. + 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, removed_subscription_affects_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; +TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; + auto ipm = std::make_shared(); - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; + auto s1 = std::make_shared(); + s1->take_shared_method = false; + auto s1_id = ipm->add_subscription(s1); - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); - 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 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)); - 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) - ); + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); - 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); + auto s3 = std::make_shared(); + s3->take_shared_method = true; + auto s3_id = ipm->add_subscription(s3); - 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(); + auto s4 = std::make_shared(); + s4->take_shared_method = true; + auto s4_id = ipm->add_subscription(s4); - ipm.remove_subscription(s2_id); + 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); - // 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()); - } + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); - // 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); + 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); + + 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); + // 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); + + 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; + + 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); + ASSERT_EQ(original_message_pointer, received_message_pointer_8); } /* - 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. + 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, multiple_subscriptions_one_publisher) { - rclcpp::intra_process_manager::IntraProcessManager ipm; +TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; + auto ipm = std::make_shared(); - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; + auto p1 = std::make_shared(); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; + auto s1 = std::make_shared(); + s1->take_shared_method = true; + auto s1_id = ipm->add_subscription(s1); - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; + auto s2 = std::make_shared(); + s2->take_shared_method = false; + auto s2_id = ipm->add_subscription(s2); - 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 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); - 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) - ); + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); - 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); + auto s3 = std::make_shared(); + s3->take_shared_method = false; + auto s3_id = ipm->add_subscription(s3); - 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(); + auto s4 = std::make_shared(); + s4->take_shared_method = false; + auto s4_id = ipm->add_subscription(s4); - 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()); - } - unique_msg.reset(); + auto s5 = std::make_shared(); + s5->take_shared_method = true; + auto s5_id = ipm->add_subscription(s5); - 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()); - } -} - -/* - 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(); -} - -/* - 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(); -} - -/* - 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. - */ -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); -} - -/* - 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. - */ -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); -} - -/* - 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. - */ -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); -} - -/* - 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. - */ -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); + 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; + + 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(); + EXPECT_EQ(original_message_pointer, received_message_pointer_10); + EXPECT_NE(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 5625804..0000000 --- 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 d86fdfc..6719197 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 @@ protected: 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; @@ -159,15 +161,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 0000000..07dfd8d --- /dev/null +++ b/rclcpp/test/test_ring_buffer_implementation.cpp @@ -0,0 +1,81 @@ +// 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/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" + +/* + Construtctor + */ +TEST(TestRingBufferImplementation, constructor) { + // Cannot create a buffer of size zero. + EXPECT_THROW( + rclcpp::experimental::buffers::RingBufferImplementation rb(0), + std::invalid_argument); + + rclcpp::experimental::buffers::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::experimental::buffers::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 f3a0db6..78b420b 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 @@ public: (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 @@ protected: 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 @@ public: 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()); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 6fe3532..c090319 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"