Compare commits
1 Commits
runtime_in
...
wjwwood/al
| Author | SHA1 | Date | |
|---|---|---|---|
|
933b802d8a |
@@ -46,7 +46,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
|
||||
@@ -74,6 +73,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
|
||||
@@ -196,17 +196,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"
|
||||
@@ -215,6 +205,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_node test/test_node.cpp)
|
||||
if(TARGET test_node)
|
||||
|
||||
@@ -219,7 +219,7 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
bool use_take_shared_method()
|
||||
bool use_take_shared_method() const
|
||||
{
|
||||
return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_;
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
// Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
@@ -12,12 +12,26 @@
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp/intra_process_manager_impl.hpp"
|
||||
#ifndef RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
|
||||
#define RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
|
||||
|
||||
#include <memory>
|
||||
|
||||
rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr
|
||||
rclcpp::intra_process_manager::create_default_impl()
|
||||
namespace rclcpp
|
||||
{
|
||||
return std::make_shared<IntraProcessManagerImpl<>>();
|
||||
}
|
||||
namespace intra_process_buffer
|
||||
{
|
||||
|
||||
template<typename BufferT>
|
||||
class BufferImplementationBase
|
||||
{
|
||||
public:
|
||||
virtual BufferT dequeue() = 0;
|
||||
virtual void enqueue(BufferT request) = 0;
|
||||
|
||||
virtual void clear() = 0;
|
||||
virtual bool has_data() const = 0;
|
||||
};
|
||||
|
||||
} // namespace intra_process_buffer
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
|
||||
230
rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp
Normal file
230
rclcpp/include/rclcpp/buffers/intra_process_buffer.hpp
Normal file
@@ -0,0 +1,230 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
|
||||
#define RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include "rclcpp/allocator/allocator_common.hpp"
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/buffers/buffer_implementation_base.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace intra_process_buffer
|
||||
{
|
||||
|
||||
class IntraProcessBufferBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase)
|
||||
|
||||
virtual void clear() = 0;
|
||||
|
||||
virtual bool has_data() const = 0;
|
||||
virtual bool use_take_shared_method() const = 0;
|
||||
};
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>,
|
||||
typename MessageDeleter = std::default_delete<MessageT>>
|
||||
class IntraProcessBuffer : public IntraProcessBufferBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer)
|
||||
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||
using MessageSharedPtr = std::shared_ptr<const MessageT>;
|
||||
|
||||
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<void>,
|
||||
typename MessageDeleter = std::default_delete<MessageT>,
|
||||
typename BufferT = std::unique_ptr<MessageT>>
|
||||
class TypedIntraProcessBuffer : public IntraProcessBuffer<MessageT, Alloc, MessageDeleter>
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer)
|
||||
|
||||
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||
using MessageSharedPtr = std::shared_ptr<const MessageT>;
|
||||
|
||||
TypedIntraProcessBuffer(
|
||||
std::unique_ptr<BufferImplementationBase<BufferT>> buffer_impl,
|
||||
std::shared_ptr<Alloc> allocator = nullptr)
|
||||
{
|
||||
bool valid_type = (std::is_same<BufferT, MessageSharedPtr>::value ||
|
||||
std::is_same<BufferT, MessageUniquePtr>::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<MessageAlloc>();
|
||||
} else {
|
||||
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
|
||||
}
|
||||
}
|
||||
|
||||
void add_shared(MessageSharedPtr msg)
|
||||
{
|
||||
add_shared_impl<BufferT>(std::move(msg));
|
||||
}
|
||||
|
||||
void add_unique(MessageUniquePtr msg)
|
||||
{
|
||||
buffer_->enqueue(std::move(msg));
|
||||
}
|
||||
|
||||
MessageSharedPtr consume_shared()
|
||||
{
|
||||
return consume_shared_impl<BufferT>();
|
||||
}
|
||||
|
||||
MessageUniquePtr consume_unique()
|
||||
{
|
||||
return consume_unique_impl<BufferT>();
|
||||
}
|
||||
|
||||
bool has_data() const
|
||||
{
|
||||
return buffer_->has_data();
|
||||
}
|
||||
|
||||
void clear()
|
||||
{
|
||||
buffer_->clear();
|
||||
}
|
||||
|
||||
bool use_take_shared_method() const
|
||||
{
|
||||
return std::is_same<BufferT, MessageSharedPtr>::value;
|
||||
}
|
||||
|
||||
private:
|
||||
std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
|
||||
|
||||
std::shared_ptr<MessageAlloc> message_allocator_;
|
||||
|
||||
// MessageSharedPtr to MessageSharedPtr
|
||||
template<typename DestinationT>
|
||||
typename std::enable_if<
|
||||
std::is_same<DestinationT, MessageSharedPtr>::value
|
||||
>::type
|
||||
add_shared_impl(MessageSharedPtr shared_msg)
|
||||
{
|
||||
buffer_->enqueue(std::move(shared_msg));
|
||||
}
|
||||
|
||||
// MessageSharedPtr to MessageUniquePtr
|
||||
template<typename DestinationT>
|
||||
typename std::enable_if<
|
||||
std::is_same<DestinationT, MessageUniquePtr>::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<MessageDeleter, const MessageT>(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 OriginT>
|
||||
typename std::enable_if<
|
||||
(std::is_same<OriginT, MessageSharedPtr>::value),
|
||||
MessageSharedPtr
|
||||
>::type
|
||||
consume_shared_impl()
|
||||
{
|
||||
return buffer_->dequeue();
|
||||
}
|
||||
|
||||
// MessageUniquePtr to MessageSharedPtr
|
||||
template<typename OriginT>
|
||||
typename std::enable_if<
|
||||
(std::is_same<OriginT, MessageUniquePtr>::value),
|
||||
MessageSharedPtr
|
||||
>::type
|
||||
consume_shared_impl()
|
||||
{
|
||||
// automatic cast from unique ptr to shared ptr
|
||||
return buffer_->dequeue();
|
||||
}
|
||||
|
||||
// MessageSharedPtr to MessageUniquePtr
|
||||
template<typename OriginT>
|
||||
typename std::enable_if<
|
||||
(std::is_same<OriginT, MessageSharedPtr>::value),
|
||||
MessageUniquePtr
|
||||
>::type
|
||||
consume_unique_impl()
|
||||
{
|
||||
MessageSharedPtr buffer_msg = buffer_->dequeue();
|
||||
|
||||
MessageUniquePtr unique_msg;
|
||||
MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(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 OriginT>
|
||||
typename std::enable_if<
|
||||
(std::is_same<OriginT, MessageUniquePtr>::value),
|
||||
MessageUniquePtr
|
||||
>::type
|
||||
consume_unique_impl()
|
||||
{
|
||||
return buffer_->dequeue();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace intra_process_buffer
|
||||
} // namespace rclcpp
|
||||
|
||||
|
||||
#endif // RCLCPP__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
|
||||
120
rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp
Normal file
120
rclcpp/include/rclcpp/buffers/ring_buffer_implementation.hpp
Normal file
@@ -0,0 +1,120 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
|
||||
#define RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
#include "iostream"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
namespace intra_process_buffer
|
||||
{
|
||||
|
||||
template<typename BufferT>
|
||||
class RingBufferImplementation : public BufferImplementationBase<BufferT>
|
||||
{
|
||||
public:
|
||||
explicit RingBufferImplementation(size_t capacity)
|
||||
: ring_buffer_(capacity)
|
||||
{
|
||||
capacity_ = capacity;
|
||||
write_index_ = capacity_ - 1;
|
||||
read_index_ = 0;
|
||||
size_ = 0;
|
||||
|
||||
if (capacity == 0) {
|
||||
throw std::invalid_argument("capacity must be a positive, non-zero value");
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~RingBufferImplementation() {}
|
||||
|
||||
void enqueue(BufferT request)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
write_index_ = next(write_index_);
|
||||
ring_buffer_[write_index_] = std::move(request);
|
||||
|
||||
if (is_full()) {
|
||||
read_index_ = next(read_index_);
|
||||
} else {
|
||||
size_++;
|
||||
}
|
||||
}
|
||||
|
||||
BufferT dequeue()
|
||||
{
|
||||
if (!has_data()) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
|
||||
throw std::runtime_error("Calling dequeue on empty intra-process buffer");
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
auto request = std::move(ring_buffer_[read_index_]);
|
||||
read_index_ = next(read_index_);
|
||||
|
||||
size_--;
|
||||
|
||||
return request;
|
||||
}
|
||||
|
||||
inline uint32_t next(uint32_t val)
|
||||
{
|
||||
return (val + 1) % capacity_;
|
||||
}
|
||||
|
||||
inline bool has_data() const
|
||||
{
|
||||
return size_ != 0;
|
||||
}
|
||||
|
||||
inline bool is_full()
|
||||
{
|
||||
return size_ == capacity_;
|
||||
}
|
||||
|
||||
void clear() {}
|
||||
|
||||
private:
|
||||
std::vector<BufferT> ring_buffer_;
|
||||
|
||||
size_t write_index_;
|
||||
size_t read_index_;
|
||||
size_t size_;
|
||||
size_t capacity_;
|
||||
|
||||
std::mutex mutex_;
|
||||
};
|
||||
|
||||
} // namespace intra_process_buffer
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
|
||||
95
rclcpp/include/rclcpp/create_intra_process_buffer.hpp
Normal file
95
rclcpp/include/rclcpp/create_intra_process_buffer.hpp
Normal file
@@ -0,0 +1,95 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_
|
||||
#define RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl/subscription.h"
|
||||
|
||||
#include "rclcpp/buffers/ring_buffer_implementation.hpp"
|
||||
#include "rclcpp/buffers/intra_process_buffer.hpp"
|
||||
#include "rclcpp/intra_process_buffer_type.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>,
|
||||
typename Deleter = std::default_delete<MessageT>>
|
||||
typename intra_process_buffer::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
|
||||
create_intra_process_buffer(
|
||||
IntraProcessBufferType buffer_type,
|
||||
rmw_qos_profile_t qos,
|
||||
std::shared_ptr<Alloc> allocator)
|
||||
{
|
||||
using MessageSharedPtr = std::shared_ptr<const MessageT>;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
|
||||
|
||||
size_t buffer_size = qos.depth;
|
||||
|
||||
typename intra_process_buffer::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;
|
||||
|
||||
switch (buffer_type) {
|
||||
case IntraProcessBufferType::SharedPtr:
|
||||
{
|
||||
using BufferT = MessageSharedPtr;
|
||||
|
||||
auto buffer_implementation =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<BufferT>>(
|
||||
buffer_size);
|
||||
|
||||
// Construct the intra_process_buffer
|
||||
buffer =
|
||||
std::make_unique<rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc,
|
||||
Deleter, BufferT>>(
|
||||
std::move(buffer_implementation),
|
||||
allocator);
|
||||
|
||||
break;
|
||||
}
|
||||
case IntraProcessBufferType::UniquePtr:
|
||||
{
|
||||
using BufferT = MessageUniquePtr;
|
||||
|
||||
auto buffer_implementation =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<BufferT>>(
|
||||
buffer_size);
|
||||
|
||||
// Construct the intra_process_buffer
|
||||
buffer =
|
||||
std::make_unique<rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc,
|
||||
Deleter, BufferT>>(
|
||||
std::move(buffer_implementation),
|
||||
allocator);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
throw std::runtime_error("Unrecognized IntraProcessBufferType value");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return buffer;
|
||||
}
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__CREATE_INTRA_PROCESS_BUFFER_HPP_
|
||||
@@ -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<SubscriptionT>(sub);
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
35
rclcpp/include/rclcpp/intra_process_buffer_type.hpp
Normal file
35
rclcpp/include/rclcpp/intra_process_buffer_type.hpp
Normal file
@@ -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<MessageT>
|
||||
SharedPtr,
|
||||
/// Set the data type used in the intra-process buffer as std::unique_ptr<MessageT>
|
||||
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_
|
||||
@@ -1,4 +1,4 @@
|
||||
// Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
@@ -17,31 +17,35 @@
|
||||
|
||||
#include <rmw/types.h>
|
||||
|
||||
#include <shared_mutex>
|
||||
|
||||
#include <algorithm>
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/intra_process_manager_impl.hpp"
|
||||
#include "rclcpp/mapped_ring_buffer.hpp"
|
||||
#include "rclcpp/logger.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/publisher_base.hpp"
|
||||
#include "rclcpp/subscription_base.hpp"
|
||||
#include "rclcpp/subscription_intra_process.hpp"
|
||||
#include "rclcpp/subscription_intra_process_base.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
namespace intra_process_manager
|
||||
{
|
||||
|
||||
/// This class facilitates intra process communication between nodes.
|
||||
/// This class performs intra process communication between nodes.
|
||||
/**
|
||||
* This class is used in the creation of publishers and subscriptions.
|
||||
* A singleton instance of this class is owned by a rclcpp::Context and a
|
||||
@@ -49,74 +53,34 @@ namespace intra_process_manager
|
||||
* Nodes which do not have a common Context will not exchange intra process
|
||||
* messages because they will not share access to an instance of this class.
|
||||
*
|
||||
* When a Node creates a publisher or subscription, it will register them
|
||||
* with this class.
|
||||
* The node will also hook into the publisher's publish call
|
||||
* in order to do intra process related work.
|
||||
* When a Node creates a subscription, it can also create an additional
|
||||
* wrapper meant to receive intra process messages.
|
||||
* This structure can be registered with this class.
|
||||
* It is also allocated an id which is unique among all publishers
|
||||
* and subscriptions in this process and that is associated to the subscription.
|
||||
*
|
||||
* When 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 Node creates a publisher, as before this can be registered with this class.
|
||||
* This is required in order to publish intra-process messages.
|
||||
* It is also allocated an id which is unique among all publishers
|
||||
* and subscriptions in this process and that is associated to the publisher.
|
||||
*
|
||||
* 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 a publisher or a subscription are registered with this class, an internal
|
||||
* structure is updated in order to store which of them can communicate.
|
||||
* i.e. they have the same topic and compatible QoS.
|
||||
*
|
||||
* 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 the user publishes a message, if intra-process communication is enabled
|
||||
* on the publisher, the message is handed to this class.
|
||||
* Using the publisher id, a list of recipients for the message is selected.
|
||||
* For each item in the list, this class stores its intra-process wrapper.
|
||||
*
|
||||
* 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.
|
||||
* The wrapper contains a buffer where published intra-process messages are stored
|
||||
* until the subscription picks them up.
|
||||
* Depending on the data type stored in the buffer, the subscription intra process
|
||||
* can request ownership on the inserted messages.
|
||||
*
|
||||
* 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.
|
||||
* Thus, when an intra-process message is published, this class knows how many
|
||||
* intra-process subscriptions needs it and how many require ownership.
|
||||
* This information allows to efficiently perform a minimum number of copies of the message.
|
||||
*
|
||||
* This class is neither CopyConstructable nor CopyAssignable.
|
||||
*/
|
||||
@@ -129,28 +93,25 @@ public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
explicit IntraProcessManager(
|
||||
IntraProcessManagerImplBase::SharedPtr state = create_default_impl());
|
||||
IntraProcessManager();
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual ~IntraProcessManager();
|
||||
|
||||
/// Register a subscription with the manager, returns subscriptions unique id.
|
||||
/**
|
||||
* In addition to generating a unique intra process id for the subscription,
|
||||
* this method also stores the topic name of the subscription.
|
||||
* This method stores the subscription intra process object, together with
|
||||
* the information of its wrapped subscription (i.e. topic name and QoS).
|
||||
*
|
||||
* This method is normally called during the creation of a subscription,
|
||||
* but after it creates the internal intra process rmw_subscription_t.
|
||||
* In addition this generates a unique intra process id for the subscription.
|
||||
*
|
||||
* This method will allocate memory.
|
||||
*
|
||||
* \param subscription the Subscription to register.
|
||||
* \param subscription the SubscriptionIntraProcess to register.
|
||||
* \return an unsigned 64-bit integer which is the subscription's unique id.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
add_subscription(SubscriptionBase::SharedPtr subscription);
|
||||
add_subscription(
|
||||
SubscriptionIntraProcessBase::SharedPtr subscription);
|
||||
|
||||
/// Unregister a subscription using the subscription's unique id.
|
||||
/**
|
||||
@@ -164,32 +125,18 @@ public:
|
||||
|
||||
/// 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.
|
||||
* This method stores the publisher intra process object, together with
|
||||
* the information of its wrapped publisher (i.e. topic name and QoS).
|
||||
*
|
||||
* 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.
|
||||
* In addition this generates a unique intra process id for the publisher.
|
||||
*
|
||||
* \param publisher publisher to be registered with the manager.
|
||||
* \param buffer_size if 0 (default) a size is calculated based on the QoS.
|
||||
* \return an unsigned 64-bit integer which is the publisher's unique id.
|
||||
*/
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
add_publisher(
|
||||
rclcpp::PublisherBase::SharedPtr publisher,
|
||||
size_t buffer_size = 0);
|
||||
PublisherBase::SharedPtr publisher);
|
||||
|
||||
/// Unregister a publisher using the publisher's unique id.
|
||||
/**
|
||||
@@ -201,27 +148,20 @@ public:
|
||||
void
|
||||
remove_publisher(uint64_t intra_process_publisher_id);
|
||||
|
||||
/// Store a message in the manager, and return the message sequence number.
|
||||
/// Publishes an intra-process message, passed as a shared pointer.
|
||||
/**
|
||||
* The given message is stored in internal storage using the given publisher
|
||||
* id and the newly generated message sequence, which is also returned.
|
||||
* The combination of publisher id and message sequence number can later
|
||||
* be used with a subscription id to retrieve the message by calling
|
||||
* take_intra_process_message.
|
||||
* The number of times take_intra_process_message can be called with this
|
||||
* unique pair of id's is determined by the number of subscriptions currently
|
||||
* subscribed to the same topic and which share the same Context, i.e. once
|
||||
* for each subscription which should receive the intra process message.
|
||||
* This is one of the two methods for publishing intra-process.
|
||||
*
|
||||
* 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.
|
||||
* Using the intra-process publisher id, a list of recipients is obtained.
|
||||
* This list is split in half, depending whether they require ownership or not.
|
||||
*
|
||||
* This particular method takes a shared pointer as input.
|
||||
* This can be safely passed to all the subscriptions that do not require ownership.
|
||||
* No copies are needed in this case.
|
||||
* For every subscription requiring ownership, a copy has to be made.
|
||||
*
|
||||
* The total number of copies is always equal to the number
|
||||
* of subscriptions requiring ownership.
|
||||
*
|
||||
* This method can throw an exception if the publisher id is not found or
|
||||
* if the publisher shared_ptr given to add_publisher has gone out of scope.
|
||||
@@ -230,168 +170,138 @@ public:
|
||||
*
|
||||
* \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<void>>
|
||||
uint64_t
|
||||
store_intra_process_message(
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>>
|
||||
void
|
||||
do_intra_process_publish(
|
||||
uint64_t intra_process_publisher_id,
|
||||
std::shared_ptr<const MessageT> message)
|
||||
std::shared_ptr<const MessageT> message,
|
||||
std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
|
||||
{
|
||||
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||
using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
|
||||
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<TypedMRB>(buffer);
|
||||
if (!typed_buffer) {
|
||||
throw std::runtime_error("Typecast failed due to incorrect message type");
|
||||
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||
|
||||
std::shared_lock<std::shared_timed_mutex> 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;
|
||||
|
||||
// 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.
|
||||
this->template add_shared_msg_to_buffers<MessageT>(message, sub_ids.take_shared_subscriptions);
|
||||
|
||||
impl_->store_intra_process_message(intra_process_publisher_id, message_seq);
|
||||
if (sub_ids.take_ownership_subscriptions.size() > 0) {
|
||||
MessageUniquePtr unique_msg;
|
||||
MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(message);
|
||||
auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
|
||||
MessageAllocTraits::construct(*allocator.get(), ptr, *message);
|
||||
if (deleter) {
|
||||
unique_msg = MessageUniquePtr(ptr, *deleter);
|
||||
} else {
|
||||
unique_msg = MessageUniquePtr(ptr);
|
||||
}
|
||||
|
||||
// Return the message sequence which is sent to the subscription.
|
||||
return message_seq;
|
||||
this->template add_owned_msg_to_buffers<MessageT, Alloc, MessageDeleter>(
|
||||
std::move(unique_msg),
|
||||
sub_ids.take_ownership_subscriptions,
|
||||
allocator);
|
||||
}
|
||||
}
|
||||
|
||||
template<
|
||||
typename MessageT, typename Alloc = std::allocator<void>,
|
||||
typename Deleter = std::default_delete<MessageT>>
|
||||
uint64_t
|
||||
store_intra_process_message(
|
||||
uint64_t intra_process_publisher_id,
|
||||
std::unique_ptr<MessageT, Deleter> message)
|
||||
{
|
||||
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||
using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
|
||||
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<TypedMRB>(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.
|
||||
/// Publishes an intra-process message, passed as a unique pointer.
|
||||
/**
|
||||
* The intra_process_publisher_id and message_sequence_number parameters
|
||||
* uniquely identify a message instance, which should be taken.
|
||||
* This is one of the two methods for publishing intra-process.
|
||||
*
|
||||
* The requesting_subscriptions_intra_process_id parameter is used to make
|
||||
* sure the requesting subscription was intended to receive this message
|
||||
* instance.
|
||||
* This check is made because it could happen that the requester
|
||||
* comes up after the publish event, so it still receives the notification of
|
||||
* a new intra process message, but it wasn't registered with the manager at
|
||||
* the time of publishing, causing it to take when it wasn't intended.
|
||||
* This should be avioded unless latching-like behavior is involved.
|
||||
* Using the intra-process publisher id, a list of recipients is obtained.
|
||||
* This list is split in half, depending whether they require ownership or not.
|
||||
*
|
||||
* The message parameter is used to store the taken message.
|
||||
* On the last expected call to this method, the ownership is transfered out
|
||||
* of internal storage and into the message parameter.
|
||||
* On all previous calls a copy of the internally stored message is made and
|
||||
* the ownership of the copy is transfered to the message parameter.
|
||||
* TODO(wjwwood): update this documentation when latching is supported.
|
||||
* This particular method takes a unique pointer as input.
|
||||
* The pointer can be promoted to a shared pointer and passed to all the subscriptions
|
||||
* that do not require ownership.
|
||||
* In case of subscriptions requiring ownership, the message will be copied for all of
|
||||
* them except the last one, when ownership can be transferred.
|
||||
*
|
||||
* The message parameter can be set to nullptr if:
|
||||
* This method can save an additional copy compared to the shared pointer one.
|
||||
*
|
||||
* - The publisher id is not found.
|
||||
* - The message sequence is not found for the given publisher id.
|
||||
* - The requesting subscription's id is not in the list of intended takers.
|
||||
* - The requesting subscription's id has been used before with this message.
|
||||
* This method can throw an exception if the publisher id is not found or
|
||||
* if the publisher shared_ptr given to add_publisher has gone out of scope.
|
||||
*
|
||||
* This method may allocate memory to copy the stored message.
|
||||
* This method does allocate memory.
|
||||
*
|
||||
* \param intra_process_publisher_id the id of the message's publisher.
|
||||
* \param message_sequence_number the sequence number of the message.
|
||||
* \param requesting_subscriptions_intra_process_id the subscription's id.
|
||||
* \param message the message typed unique_ptr used to return the message.
|
||||
* \param intra_process_publisher_id the id of the publisher of this message.
|
||||
* \param message the message that is being stored.
|
||||
*/
|
||||
template<
|
||||
typename MessageT, typename Alloc = std::allocator<void>,
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>,
|
||||
typename Deleter = std::default_delete<MessageT>>
|
||||
void
|
||||
take_intra_process_message(
|
||||
do_intra_process_publish(
|
||||
uint64_t intra_process_publisher_id,
|
||||
uint64_t message_sequence_number,
|
||||
uint64_t requesting_subscriptions_intra_process_id,
|
||||
std::unique_ptr<MessageT, Deleter> & message)
|
||||
std::unique_ptr<MessageT, Deleter> message,
|
||||
std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
|
||||
{
|
||||
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||
using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
|
||||
message = nullptr;
|
||||
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||
|
||||
size_t target_subs_size = 0;
|
||||
std::lock_guard<std::mutex> 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<TypedMRB>(buffer);
|
||||
if (!typed_buffer) {
|
||||
std::shared_lock<std::shared_timed_mutex> lock(mutex_);
|
||||
|
||||
auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
|
||||
if (publisher_it == pub_to_subs_.end()) {
|
||||
// Publisher is either invalid or no longer exists.
|
||||
RCLCPP_WARN(
|
||||
rclcpp::get_logger("rclcpp"),
|
||||
"Calling do_intra_process_publish for invalid or no longer existing publisher id");
|
||||
return;
|
||||
}
|
||||
// Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
|
||||
if (target_subs_size) {
|
||||
// There are more subscriptions to serve, return a copy.
|
||||
typed_buffer->get(message_sequence_number, message);
|
||||
} else {
|
||||
// This is the last one to be returned, transfer ownership.
|
||||
typed_buffer->pop(message_sequence_number, message);
|
||||
}
|
||||
}
|
||||
const auto & sub_ids = publisher_it->second;
|
||||
|
||||
template<
|
||||
typename MessageT, typename Alloc = std::allocator<void>>
|
||||
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<const MessageT> & message)
|
||||
{
|
||||
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||
using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
|
||||
message = nullptr;
|
||||
if (sub_ids.take_ownership_subscriptions.empty()) {
|
||||
// None of the buffers require ownership, so we promote the pointer
|
||||
std::shared_ptr<MessageT> msg = std::move(message);
|
||||
|
||||
size_t target_subs_size = 0;
|
||||
std::lock_guard<std::mutex> 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<TypedMRB>(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);
|
||||
this->template add_shared_msg_to_buffers<MessageT>(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<uint64_t> 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<MessageT, Alloc, Deleter>(
|
||||
std::move(message),
|
||||
concatenated_vector,
|
||||
allocator);
|
||||
} else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
|
||||
sub_ids.take_shared_subscriptions.size() > 1)
|
||||
{
|
||||
// Construct a new shared pointer from the message
|
||||
// for the buffers that do not require ownership
|
||||
std::shared_ptr<MessageT> shared_msg;
|
||||
auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
|
||||
MessageAllocTraits::construct(*allocator.get(), ptr, *message);
|
||||
shared_msg = std::shared_ptr<MessageT>(ptr);
|
||||
|
||||
this->template add_shared_msg_to_buffers<MessageT>(shared_msg,
|
||||
sub_ids.take_shared_subscriptions);
|
||||
this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
|
||||
std::move(message),
|
||||
sub_ids.take_ownership_subscriptions,
|
||||
allocator);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -400,18 +310,126 @@ public:
|
||||
bool
|
||||
matches_any_publishers(const rmw_gid_t * id) const;
|
||||
|
||||
/// Return the number of intraprocess subscriptions to a topic, given the publisher id.
|
||||
/// Return the number of intraprocess subscriptions that are matched with a given publisher id.
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_subscription_count(uint64_t intra_process_publisher_id) const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
SubscriptionIntraProcessBase::SharedPtr
|
||||
get_subscription_intra_process(uint64_t intra_process_subscription_id);
|
||||
|
||||
private:
|
||||
struct SubscriptionInfo
|
||||
{
|
||||
SubscriptionInfo() = default;
|
||||
|
||||
SubscriptionIntraProcessBase::SharedPtr subscription;
|
||||
rmw_qos_profile_t qos;
|
||||
const char * topic_name;
|
||||
bool use_take_shared_method;
|
||||
};
|
||||
|
||||
struct PublisherInfo
|
||||
{
|
||||
PublisherInfo() = default;
|
||||
|
||||
PublisherBase::WeakPtr publisher;
|
||||
rmw_qos_profile_t qos;
|
||||
const char * topic_name;
|
||||
};
|
||||
|
||||
struct SplittedSubscriptions
|
||||
{
|
||||
std::vector<uint64_t> take_shared_subscriptions;
|
||||
std::vector<uint64_t> take_ownership_subscriptions;
|
||||
};
|
||||
|
||||
using SubscriptionMap = std::unordered_map<
|
||||
uint64_t, SubscriptionInfo>;
|
||||
|
||||
using PublisherMap = std::unordered_map<
|
||||
uint64_t, PublisherInfo>;
|
||||
|
||||
using PublisherToSubscriptionIdsMap = std::unordered_map<
|
||||
uint64_t, SplittedSubscriptions>;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
static uint64_t
|
||||
get_next_unique_id();
|
||||
|
||||
IntraProcessManagerImplBase::SharedPtr impl_;
|
||||
std::mutex take_mutex_;
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
|
||||
|
||||
template<typename MessageT>
|
||||
void
|
||||
add_shared_msg_to_buffers(
|
||||
std::shared_ptr<const MessageT> message,
|
||||
std::vector<uint64_t> 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<SubscriptionIntraProcess<MessageT>>(subscription_base);
|
||||
|
||||
subscription->provide_intra_process_message(message);
|
||||
}
|
||||
}
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>,
|
||||
typename Deleter = std::default_delete<MessageT>>
|
||||
void
|
||||
add_owned_msg_to_buffers(
|
||||
std::unique_ptr<MessageT, Deleter> message,
|
||||
std::vector<uint64_t> subscription_ids,
|
||||
std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
|
||||
{
|
||||
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
|
||||
|
||||
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<SubscriptionIntraProcess<MessageT>>(subscription_base);
|
||||
|
||||
if (std::next(it) == subscription_ids.end()) {
|
||||
// If this is the last subscription, give up ownership
|
||||
subscription->provide_intra_process_message(std::move(message));
|
||||
} else {
|
||||
// Copy the message since we have additional subscriptions to serve
|
||||
MessageUniquePtr copy_message;
|
||||
Deleter deleter = message.get_deleter();
|
||||
auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
|
||||
MessageAllocTraits::construct(*allocator.get(), ptr, *message);
|
||||
copy_message = MessageUniquePtr(ptr, deleter);
|
||||
|
||||
subscription->provide_intra_process_message(std::move(copy_message));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PublisherToSubscriptionIdsMap pub_to_subs_;
|
||||
SubscriptionMap subscriptions_;
|
||||
PublisherMap publishers_;
|
||||
|
||||
mutable std::shared_timed_mutex mutex_;
|
||||
};
|
||||
|
||||
} // namespace intra_process_manager
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
#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<typename Allocator = std::allocator<void>>
|
||||
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<uint64_t>::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<std::mutex> 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<std::mutex> 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_t>(), 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<std::mutex> 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<char, RMW_TOPIC_MAX_NAME_LENGTH + 1>;
|
||||
|
||||
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<typename T>
|
||||
using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
|
||||
|
||||
RebindAlloc<uint64_t> uint64_allocator;
|
||||
|
||||
using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
|
||||
using SubscriptionMap = std::unordered_map<
|
||||
uint64_t, SubscriptionBase::WeakPtr,
|
||||
std::hash<uint64_t>, std::equal_to<uint64_t>,
|
||||
RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
|
||||
|
||||
using IDTopicMap = std::map<
|
||||
FixedSizeString,
|
||||
AllocSet,
|
||||
strcmp_wrapper,
|
||||
RebindAlloc<std::pair<const FixedSizeString, AllocSet>>>;
|
||||
|
||||
SubscriptionMap subscriptions_;
|
||||
|
||||
IDTopicMap subscription_ids_by_topic_;
|
||||
|
||||
struct PublisherInfo
|
||||
{
|
||||
RCLCPP_DISABLE_COPY(PublisherInfo)
|
||||
|
||||
PublisherInfo() = default;
|
||||
|
||||
PublisherBase::WeakPtr publisher;
|
||||
std::atomic<uint64_t> sequence_number;
|
||||
mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
|
||||
|
||||
using TargetSubscriptionsMap = std::unordered_map<
|
||||
uint64_t, AllocSet,
|
||||
std::hash<uint64_t>, std::equal_to<uint64_t>,
|
||||
RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
|
||||
TargetSubscriptionsMap target_subscriptions_by_message_sequence;
|
||||
};
|
||||
|
||||
using PublisherMap = std::unordered_map<
|
||||
uint64_t, PublisherInfo,
|
||||
std::hash<uint64_t>, std::equal_to<uint64_t>,
|
||||
RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
|
||||
|
||||
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_
|
||||
@@ -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(...) \
|
||||
|
||||
@@ -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 <algorithm>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#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<typename T, typename Alloc = std::allocator<void>>
|
||||
class MappedRingBuffer : public MappedRingBufferBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>)
|
||||
using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
|
||||
using ElemAlloc = typename ElemAllocTraits::allocator_type;
|
||||
using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
|
||||
|
||||
using ConstElemSharedPtr = std::shared_ptr<const T>;
|
||||
using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
|
||||
|
||||
/// 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<Alloc> 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<ElemAlloc>();
|
||||
} else {
|
||||
allocator_ = std::make_shared<ElemAlloc>(*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<std::mutex> 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<ElemDeleter, const T>(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<std::mutex> 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<std::mutex> 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<ElemDeleter, const T>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(data_mutex_);
|
||||
return elements_.end() != get_iterator_of_key(key);
|
||||
}
|
||||
|
||||
private:
|
||||
RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
|
||||
|
||||
struct Element
|
||||
{
|
||||
uint64_t key;
|
||||
ElemUniquePtr unique_value;
|
||||
ConstElemSharedPtr shared_value;
|
||||
bool in_use;
|
||||
};
|
||||
|
||||
using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<Element>;
|
||||
|
||||
typename std::vector<Element, VectorAlloc>::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<Element, VectorAlloc> elements_;
|
||||
size_t head_;
|
||||
std::shared_ptr<ElemAlloc> allocator_;
|
||||
std::mutex data_mutex_;
|
||||
};
|
||||
|
||||
} // namespace mapped_ring_buffer
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__MAPPED_RING_BUFFER_HPP_
|
||||
@@ -68,7 +68,8 @@ public:
|
||||
void
|
||||
add_subscription(
|
||||
rclcpp::SubscriptionBase::SharedPtr subscription,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) override;
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group,
|
||||
bool use_intra_process) override;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rclcpp::node_interfaces::NodeBaseInterface *
|
||||
|
||||
@@ -27,6 +27,7 @@
|
||||
#include "rclcpp/publisher_factory.hpp"
|
||||
#include "rclcpp/subscription.hpp"
|
||||
#include "rclcpp/subscription_factory.hpp"
|
||||
#include "rclcpp/subscription_intra_process.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
@@ -72,7 +73,8 @@ public:
|
||||
void
|
||||
add_subscription(
|
||||
rclcpp::SubscriptionBase::SharedPtr subscription,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0;
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group,
|
||||
bool use_intra_process) = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
|
||||
@@ -28,8 +28,6 @@
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/publisher.h"
|
||||
|
||||
#include "rcl_interfaces/msg/intra_process_message.hpp"
|
||||
|
||||
#include "rclcpp/allocator/allocator_common.hpp"
|
||||
#include "rclcpp/allocator/allocator_deleter.hpp"
|
||||
#include "rclcpp/detail/resolve_use_intra_process.hpp"
|
||||
@@ -123,15 +121,6 @@ public:
|
||||
virtual ~Publisher()
|
||||
{}
|
||||
|
||||
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
|
||||
make_mapped_ring_buffer(size_t size) const override
|
||||
{
|
||||
return mapped_ring_buffer::MappedRingBuffer<
|
||||
MessageT,
|
||||
typename Publisher<MessageT, AllocatorT>::MessageAllocator
|
||||
>::make_shared(size, this->get_allocator());
|
||||
}
|
||||
|
||||
/// Send a message to the topic for this publisher.
|
||||
/**
|
||||
* This function is templated on the input message type, MessageT.
|
||||
@@ -141,7 +130,7 @@ public:
|
||||
publish(std::unique_ptr<MessageT, MessageDeleter> 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
|
||||
@@ -150,21 +139,15 @@ 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);
|
||||
std::shared_ptr<MessageT> shared_msg = std::move(msg);
|
||||
this->do_intra_process_publish(shared_msg);
|
||||
this->do_inter_process_publish(*shared_msg);
|
||||
} else {
|
||||
message_seq =
|
||||
store_intra_process_message(intra_process_publisher_id_, std::move(msg));
|
||||
}
|
||||
this->do_intra_process_publish(message_seq);
|
||||
if (inter_process_publish_needed) {
|
||||
this->do_inter_process_publish(shared_msg.get());
|
||||
this->do_intra_process_publish(std::move(msg));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -174,7 +157,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.
|
||||
@@ -199,9 +182,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
|
||||
if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
|
||||
@@ -231,31 +214,7 @@ 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");
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t
|
||||
store_intra_process_message(
|
||||
uint64_t publisher_id,
|
||||
std::shared_ptr<const MessageT> msg)
|
||||
do_intra_process_publish(std::shared_ptr<const MessageT> msg)
|
||||
{
|
||||
auto ipm = weak_ipm_.lock();
|
||||
if (!ipm) {
|
||||
@@ -263,17 +222,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<MessageT, AllocatorT>(publisher_id, msg);
|
||||
return message_seq;
|
||||
|
||||
ipm->template do_intra_process_publish<MessageT, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
std::move(msg),
|
||||
message_allocator_);
|
||||
}
|
||||
|
||||
uint64_t
|
||||
store_intra_process_message(
|
||||
uint64_t publisher_id,
|
||||
std::unique_ptr<MessageT, MessageDeleter> msg)
|
||||
void
|
||||
do_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg)
|
||||
{
|
||||
auto ipm = weak_ipm_.lock();
|
||||
if (!ipm) {
|
||||
@@ -281,11 +240,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<MessageT, AllocatorT>(publisher_id, std::move(msg));
|
||||
return message_seq;
|
||||
|
||||
ipm->template do_intra_process_publish<MessageT, AllocatorT>(
|
||||
intra_process_publisher_id_,
|
||||
std::move(msg),
|
||||
message_allocator_);
|
||||
}
|
||||
|
||||
/// Copy of original options passed during construction.
|
||||
|
||||
@@ -28,7 +28,6 @@
|
||||
#include "rcl/publisher.h"
|
||||
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/mapped_ring_buffer.hpp"
|
||||
#include "rclcpp/qos.hpp"
|
||||
#include "rclcpp/qos_event.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
@@ -97,12 +96,6 @@ 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
|
||||
@@ -184,19 +177,12 @@ public:
|
||||
using IntraProcessManagerSharedPtr =
|
||||
std::shared_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
|
||||
|
||||
/// Implementation utility function that creates a typed mapped ring buffer.
|
||||
RCLCPP_PUBLIC
|
||||
virtual
|
||||
mapped_ring_buffer::MappedRingBufferBase::SharedPtr
|
||||
make_mapped_ring_buffer(size_t size) const;
|
||||
|
||||
/// Implementation utility function used to setup intra process publishing after creation.
|
||||
RCLCPP_PUBLIC
|
||||
void
|
||||
setup_intra_process(
|
||||
uint64_t intra_process_publisher_id,
|
||||
IntraProcessManagerSharedPtr ipm,
|
||||
const rcl_publisher_options_t & intra_process_options);
|
||||
IntraProcessManagerSharedPtr ipm);
|
||||
|
||||
protected:
|
||||
template<typename EventCallbackT>
|
||||
@@ -216,7 +202,6 @@ protected:
|
||||
std::shared_ptr<rcl_node_t> 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<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
|
||||
|
||||
@@ -227,7 +212,6 @@ protected:
|
||||
uint64_t intra_process_publisher_id_;
|
||||
|
||||
rmw_gid_t rmw_gid_;
|
||||
rmw_gid_t intra_process_rmw_gid_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -167,16 +167,15 @@ 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) {
|
||||
if (service) {
|
||||
service_handles_.push_back(service->get_service_handle());
|
||||
return false;
|
||||
});
|
||||
}
|
||||
});
|
||||
group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
|
||||
client_handles_.push_back(client->get_client_handle());
|
||||
return false;
|
||||
@@ -262,11 +261,6 @@ 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 +276,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);
|
||||
|
||||
@@ -29,13 +29,10 @@
|
||||
#include "rcl/error_handling.h"
|
||||
#include "rcl/subscription.h"
|
||||
|
||||
#include "rcl_interfaces/msg/intra_process_message.hpp"
|
||||
|
||||
#include "rclcpp/any_subscription_callback.hpp"
|
||||
#include "rclcpp/detail/resolve_use_intra_process.hpp"
|
||||
#include "rclcpp/exceptions.hpp"
|
||||
#include "rclcpp/expand_topic_or_service_name.hpp"
|
||||
#include "rclcpp/intra_process_manager.hpp"
|
||||
#include "rclcpp/logging.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/message_memory_strategy.hpp"
|
||||
@@ -189,117 +186,13 @@ 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)
|
||||
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<rcl_subscription_t>
|
||||
get_intra_process_subscription_handle() const
|
||||
{
|
||||
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<CallbackMessageT, AllocatorT>(
|
||||
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<CallbackMessageT, AllocatorT>(
|
||||
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<CallbackMessageT, AllocatorT> any_callback_;
|
||||
|
||||
@@ -21,8 +21,6 @@
|
||||
|
||||
#include "rcl/subscription.h"
|
||||
|
||||
#include "rcl_interfaces/msg/intra_process_message.hpp"
|
||||
|
||||
#include "rmw/rmw.h"
|
||||
|
||||
#include "rclcpp/any_subscription_callback.hpp"
|
||||
@@ -89,10 +87,6 @@ public:
|
||||
const std::shared_ptr<rcl_subscription_t>
|
||||
get_subscription_handle() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
virtual const std::shared_ptr<rcl_subscription_t>
|
||||
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
|
||||
@@ -152,13 +146,6 @@ public:
|
||||
void
|
||||
return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & 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;
|
||||
@@ -181,8 +168,11 @@ 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);
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
uint64_t
|
||||
get_intra_process_id() const;
|
||||
|
||||
protected:
|
||||
template<typename EventCallbackT>
|
||||
@@ -199,6 +189,10 @@ protected:
|
||||
event_handlers_.emplace_back(handler);
|
||||
}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
|
||||
|
||||
std::shared_ptr<rcl_node_t> node_handle_;
|
||||
std::shared_ptr<rcl_subscription_t> subscription_handle_;
|
||||
std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
|
||||
|
||||
@@ -26,6 +26,7 @@
|
||||
|
||||
#include "rclcpp/subscription.hpp"
|
||||
#include "rclcpp/subscription_traits.hpp"
|
||||
#include "rclcpp/intra_process_buffer_type.hpp"
|
||||
#include "rclcpp/intra_process_manager.hpp"
|
||||
#include "rclcpp/node_interfaces/node_base_interface.hpp"
|
||||
#include "rclcpp/visibility_control.hpp"
|
||||
|
||||
157
rclcpp/include/rclcpp/subscription_intra_process.hpp
Normal file
157
rclcpp/include/rclcpp/subscription_intra_process.hpp
Normal file
@@ -0,0 +1,157 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_
|
||||
#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_
|
||||
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
|
||||
#include "rclcpp/buffers/intra_process_buffer.hpp"
|
||||
#include "rclcpp/create_intra_process_buffer.hpp"
|
||||
#include "rclcpp/subscription.hpp"
|
||||
#include "rclcpp/subscription_intra_process_base.hpp"
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
template<
|
||||
typename MessageT,
|
||||
typename Alloc = std::allocator<void>,
|
||||
typename Deleter = std::default_delete<MessageT>,
|
||||
typename CallbackMessageT = MessageT>
|
||||
class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
|
||||
|
||||
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||
using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
|
||||
using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
|
||||
|
||||
using BufferUniquePtr =
|
||||
typename intra_process_buffer::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr;
|
||||
|
||||
SubscriptionIntraProcess(
|
||||
AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
|
||||
std::shared_ptr<Alloc> 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<MessageT, CallbackMessageT>::value) {
|
||||
throw std::runtime_error("SubscriptionIntraProcess wrong callback type");
|
||||
}
|
||||
|
||||
// Create the intra-process buffer.
|
||||
buffer_ = rclcpp::create_intra_process_buffer<MessageT, Alloc, Deleter>(
|
||||
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<CallbackMessageT>();
|
||||
}
|
||||
|
||||
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 T>
|
||||
typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
|
||||
execute_impl()
|
||||
{
|
||||
throw std::runtime_error("Subscription intra-process can't handle serialized messages");
|
||||
}
|
||||
|
||||
template<class T>
|
||||
typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::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<CallbackMessageT, Alloc> any_callback_;
|
||||
BufferUniquePtr buffer_;
|
||||
};
|
||||
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_HPP_
|
||||
84
rclcpp/include/rclcpp/subscription_intra_process_base.hpp
Normal file
84
rclcpp/include/rclcpp/subscription_intra_process_base.hpp
Normal file
@@ -0,0 +1,84 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
|
||||
#define RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
|
||||
|
||||
#include <rmw/rmw.h>
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
#include "rcl/error_handling.h"
|
||||
|
||||
#include "rclcpp/type_support_decl.hpp"
|
||||
#include "rclcpp/waitable.hpp"
|
||||
|
||||
namespace rclcpp
|
||||
{
|
||||
|
||||
class SubscriptionIntraProcessBase : public rclcpp::Waitable
|
||||
{
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
|
||||
: topic_name_(topic_name), qos_profile_(qos_profile)
|
||||
{}
|
||||
|
||||
virtual ~SubscriptionIntraProcessBase() = default;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
size_t
|
||||
get_number_of_ready_guard_conditions() {return 1;}
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
bool
|
||||
add_to_wait_set(rcl_wait_set_t * wait_set);
|
||||
|
||||
virtual bool
|
||||
is_ready(rcl_wait_set_t * wait_set) = 0;
|
||||
|
||||
virtual void
|
||||
execute() = 0;
|
||||
|
||||
virtual bool
|
||||
use_take_shared_method() const = 0;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
const char *
|
||||
get_topic_name() const;
|
||||
|
||||
RCLCPP_PUBLIC
|
||||
rmw_qos_profile_t
|
||||
get_actual_qos() const;
|
||||
|
||||
protected:
|
||||
std::recursive_mutex reentrant_mutex_;
|
||||
rcl_guard_condition_t gc_;
|
||||
|
||||
private:
|
||||
virtual void
|
||||
trigger_guard_condition() = 0;
|
||||
|
||||
std::string topic_name_;
|
||||
rmw_qos_profile_t qos_profile_;
|
||||
};
|
||||
} // namespace rclcpp
|
||||
|
||||
#endif // RCLCPP__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
|
||||
@@ -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<rclcpp::detail::RMWImplementationSpecificSubscriptionPayload>
|
||||
rmw_implementation_payload = nullptr;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -349,30 +346,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)
|
||||
@@ -539,7 +512,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
|
||||
|
||||
@@ -21,9 +21,7 @@ namespace intra_process_manager
|
||||
|
||||
static std::atomic<uint64_t> _next_unique_id {1};
|
||||
|
||||
IntraProcessManager::IntraProcessManager(
|
||||
rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl)
|
||||
: impl_(impl)
|
||||
IntraProcessManager::IntraProcessManager()
|
||||
{}
|
||||
|
||||
IntraProcessManager::~IntraProcessManager()
|
||||
@@ -31,50 +29,133 @@ IntraProcessManager::~IntraProcessManager()
|
||||
|
||||
uint64_t
|
||||
IntraProcessManager::add_publisher(
|
||||
rclcpp::PublisherBase::SharedPtr publisher,
|
||||
size_t buffer_size)
|
||||
rclcpp::PublisherBase::SharedPtr publisher)
|
||||
{
|
||||
std::unique_lock<std::shared_timed_mutex> lock(mutex_);
|
||||
|
||||
auto id = IntraProcessManager::get_next_unique_id();
|
||||
size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
|
||||
auto mrb = publisher->make_mapped_ring_buffer(size);
|
||||
impl_->add_publisher(id, publisher, mrb, size);
|
||||
if (!mrb) {
|
||||
throw std::runtime_error("failed to create a mapped ring buffer");
|
||||
|
||||
publishers_[id].publisher = publisher;
|
||||
publishers_[id].topic_name = publisher->get_topic_name();
|
||||
publishers_[id].qos = publisher->get_actual_qos();
|
||||
|
||||
// Initialize the subscriptions storage for this publisher.
|
||||
pub_to_subs_[id] = SplittedSubscriptions();
|
||||
|
||||
// create an entry for the publisher id and populate with already existing subscriptions
|
||||
for (auto & pair : subscriptions_) {
|
||||
if (can_communicate(publishers_[id], pair.second)) {
|
||||
insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method);
|
||||
}
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
uint64_t
|
||||
IntraProcessManager::add_subscription(
|
||||
rclcpp::SubscriptionBase::SharedPtr subscription)
|
||||
SubscriptionIntraProcessBase::SharedPtr subscription)
|
||||
{
|
||||
std::unique_lock<std::shared_timed_mutex> 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<std::shared_timed_mutex> 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<std::shared_timed_mutex> 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<std::shared_timed_mutex> 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<std::shared_timed_mutex> 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<std::shared_timed_mutex> lock(mutex_);
|
||||
|
||||
auto subscription_it = subscriptions_.find(intra_process_subscription_id);
|
||||
if (subscription_it == subscriptions_.end()) {
|
||||
return nullptr;
|
||||
} else {
|
||||
return subscription_it->second.subscription;
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t
|
||||
@@ -99,5 +180,44 @@ IntraProcessManager::get_next_unique_id()
|
||||
return next_id;
|
||||
}
|
||||
|
||||
void
|
||||
IntraProcessManager::insert_sub_id_for_pub(
|
||||
uint64_t sub_id,
|
||||
uint64_t pub_id,
|
||||
bool use_take_shared_method)
|
||||
{
|
||||
if (use_take_shared_method) {
|
||||
pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id);
|
||||
} else {
|
||||
pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
IntraProcessManager::can_communicate(
|
||||
PublisherInfo pub_info,
|
||||
SubscriptionInfo sub_info) const
|
||||
{
|
||||
// publisher and subscription must be on the same topic
|
||||
if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO(alsora): the following checks for qos compatibility should be provided by the RMW
|
||||
// a reliable subscription can't be connected with a best effort publisher
|
||||
if (sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE &&
|
||||
pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// a publisher and a subscription with different durability can't communicate
|
||||
if (sub_info.qos.durability != pub_info.qos.durability) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace intra_process_manager
|
||||
} // namespace rclcpp
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -82,7 +82,8 @@ NodeTopics::create_subscription(
|
||||
void
|
||||
NodeTopics::add_subscription(
|
||||
rclcpp::SubscriptionBase::SharedPtr subscription,
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group)
|
||||
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group,
|
||||
bool use_intra_process)
|
||||
{
|
||||
// Assign to a group.
|
||||
if (callback_group) {
|
||||
@@ -99,6 +100,20 @@ NodeTopics::add_subscription(
|
||||
callback_group->add_waitable(subscription_event);
|
||||
}
|
||||
|
||||
if (use_intra_process) {
|
||||
// Get the intra process manager for this context.
|
||||
auto context = node_base_->get_context();
|
||||
auto ipm =
|
||||
context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
|
||||
|
||||
// Use the id to retrieve the subscription intra-process from the intra-process manager.
|
||||
auto subscription_intra_process =
|
||||
ipm->get_subscription_intra_process(subscription->get_intra_process_id());
|
||||
|
||||
// Add to the callback group to be notified about intra-process msgs.
|
||||
callback_group->add_waitable(subscription_intra_process);
|
||||
}
|
||||
|
||||
// Notify the executor that a new subscription was created using the parent Node.
|
||||
{
|
||||
auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
|
||||
|
||||
@@ -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()
|
||||
{
|
||||
@@ -240,81 +226,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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -58,10 +58,6 @@ SubscriptionBase::SubscriptionBase(
|
||||
new rcl_subscription_t, custom_deletor);
|
||||
*subscription_handle_.get() = rcl_get_zero_initialized_subscription();
|
||||
|
||||
intra_process_subscription_handle_ = std::shared_ptr<rcl_subscription_t>(
|
||||
new rcl_subscription_t, custom_deletor);
|
||||
*intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription();
|
||||
|
||||
rcl_ret_t ret = rcl_subscription_init(
|
||||
subscription_handle_.get(),
|
||||
node_handle_.get(),
|
||||
@@ -117,12 +113,6 @@ SubscriptionBase::get_subscription_handle() const
|
||||
return subscription_handle_;
|
||||
}
|
||||
|
||||
const std::shared_ptr<rcl_subscription_t>
|
||||
SubscriptionBase::get_intra_process_subscription_handle() const
|
||||
{
|
||||
return intra_process_subscription_handle_;
|
||||
}
|
||||
|
||||
const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
|
||||
SubscriptionBase::get_event_handlers() const
|
||||
{
|
||||
@@ -172,31 +162,30 @@ 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;
|
||||
}
|
||||
|
||||
uint64_t
|
||||
SubscriptionBase::get_intra_process_id() const
|
||||
{
|
||||
return intra_process_subscription_id_;
|
||||
}
|
||||
|
||||
bool
|
||||
SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const
|
||||
{
|
||||
if (!use_intra_process_) {
|
||||
return false;
|
||||
}
|
||||
auto ipm = weak_ipm_.lock();
|
||||
if (!ipm) {
|
||||
throw std::runtime_error(
|
||||
"intra process publisher check called "
|
||||
"after destruction of intra process manager");
|
||||
}
|
||||
return ipm->matches_any_publishers(sender_gid);
|
||||
}
|
||||
|
||||
38
rclcpp/src/rclcpp/subscription_intra_process_base.cpp
Normal file
38
rclcpp/src/rclcpp/subscription_intra_process_base.cpp
Normal file
@@ -0,0 +1,38 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "rclcpp/subscription_intra_process_base.hpp"
|
||||
|
||||
using rclcpp::SubscriptionIntraProcessBase;
|
||||
|
||||
bool
|
||||
SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> 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_;
|
||||
}
|
||||
@@ -249,6 +249,13 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
|
||||
{rclcpp::ParameterEventsFilter::EventType::NEW,
|
||||
rclcpp::ParameterEventsFilter::EventType::CHANGED});
|
||||
for (auto & it : filter.get_events()) {
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
|
||||
RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
|
||||
continue;
|
||||
}
|
||||
>>>>>>> basic ipc implementation from alsora/new_ipc_proposal
|
||||
if (it.second->value.bool_value) {
|
||||
parameter_state_ = SET_TRUE;
|
||||
enable_ros_time();
|
||||
|
||||
246
rclcpp/test/test_intra_process_buffer.cpp
Normal file
246
rclcpp/test/test_intra_process_buffer.cpp
Normal file
@@ -0,0 +1,246 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
/*
|
||||
Construtctor
|
||||
*/
|
||||
TEST(TestIntraProcessBuffer, constructor) {
|
||||
using MessageT = char;
|
||||
using Alloc = std::allocator<void>;
|
||||
using Deleter = std::default_delete<MessageT>;
|
||||
using SharedMessageT = std::shared_ptr<const MessageT>;
|
||||
using UniqueMessageT = std::unique_ptr<MessageT, Deleter>;
|
||||
using SharedIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, SharedMessageT>;
|
||||
using UniqueIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, UniqueMessageT>;
|
||||
|
||||
auto shared_buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<SharedMessageT>>(2);
|
||||
|
||||
auto shared_intra_process_buffer =
|
||||
SharedIntraProcessBufferT(std::move(shared_buffer_impl));
|
||||
|
||||
EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method());
|
||||
|
||||
auto unique_buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<UniqueMessageT>>(2);
|
||||
|
||||
auto unique_intra_process_buffer =
|
||||
UniqueIntraProcessBufferT(std::move(unique_buffer_impl));
|
||||
|
||||
EXPECT_EQ(false, unique_intra_process_buffer.use_take_shared_method());
|
||||
}
|
||||
|
||||
/*
|
||||
Add data to an intra-process buffer with an implementations that stores shared_ptr
|
||||
Messages are extracted using the same data as the implementation, i.e. shared_ptr
|
||||
- Add shared_ptr no copies are expected
|
||||
- Add unique_ptr no copies are expected
|
||||
*/
|
||||
TEST(TestIntraProcessBuffer, shared_buffer_add) {
|
||||
using MessageT = char;
|
||||
using Alloc = std::allocator<void>;
|
||||
using Deleter = std::default_delete<MessageT>;
|
||||
using SharedMessageT = std::shared_ptr<const MessageT>;
|
||||
using SharedIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, SharedMessageT>;
|
||||
|
||||
auto buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<SharedMessageT>>(2);
|
||||
|
||||
auto intra_process_buffer =
|
||||
SharedIntraProcessBufferT(std::move(buffer_impl));
|
||||
|
||||
auto original_shared_msg = std::make_shared<char>('a');
|
||||
auto original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<char>('b');
|
||||
original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<void>;
|
||||
using Deleter = std::default_delete<MessageT>;
|
||||
using UniqueMessageT = std::unique_ptr<MessageT, Deleter>;
|
||||
using UniqueIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, UniqueMessageT>;
|
||||
|
||||
auto buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<UniqueMessageT>>(2);
|
||||
|
||||
auto intra_process_buffer =
|
||||
UniqueIntraProcessBufferT(std::move(buffer_impl));
|
||||
|
||||
auto original_shared_msg = std::make_shared<char>('a');
|
||||
auto original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<char>('b');
|
||||
original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<void>;
|
||||
using Deleter = std::default_delete<MessageT>;
|
||||
using SharedMessageT = std::shared_ptr<const MessageT>;
|
||||
using UniqueMessageT = std::unique_ptr<MessageT, Deleter>;
|
||||
using SharedIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, SharedMessageT>;
|
||||
|
||||
auto buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<SharedMessageT>>(2);
|
||||
|
||||
auto intra_process_buffer =
|
||||
SharedIntraProcessBufferT(std::move(buffer_impl));
|
||||
|
||||
auto original_shared_msg = std::make_shared<char>('a');
|
||||
auto original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<char>('b');
|
||||
original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<void>;
|
||||
using Deleter = std::default_delete<MessageT>;
|
||||
using SharedMessageT = std::shared_ptr<const MessageT>;
|
||||
using UniqueMessageT = std::unique_ptr<MessageT, Deleter>;
|
||||
using UniqueIntraProcessBufferT =
|
||||
rclcpp::intra_process_buffer::TypedIntraProcessBuffer<MessageT, Alloc, Deleter, UniqueMessageT>;
|
||||
|
||||
auto buffer_impl =
|
||||
std::make_unique<rclcpp::intra_process_buffer::RingBufferImplementation<UniqueMessageT>>(2);
|
||||
|
||||
auto intra_process_buffer =
|
||||
UniqueIntraProcessBufferT(std::move(buffer_impl));
|
||||
|
||||
auto original_unique_msg = std::make_unique<char>('a');
|
||||
auto original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(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<char>('b');
|
||||
original_message_pointer = reinterpret_cast<std::uintptr_t>(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<std::uintptr_t>(popped_unique_msg.get());
|
||||
|
||||
EXPECT_EQ(original_value, *popped_unique_msg);
|
||||
EXPECT_EQ(original_message_pointer, popped_message_pointer);
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 <memory>
|
||||
#include <utility>
|
||||
|
||||
#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<char> mrb(0), std::invalid_argument);
|
||||
// Getting or popping an empty buffer should result in a nullptr.
|
||||
rclcpp::mapped_ring_buffer::MappedRingBuffer<char> mrb(1);
|
||||
|
||||
std::unique_ptr<char> unique;
|
||||
mrb.get(1, unique);
|
||||
EXPECT_EQ(nullptr, unique);
|
||||
|
||||
mrb.pop(1, unique);
|
||||
EXPECT_EQ(nullptr, unique);
|
||||
|
||||
std::shared_ptr<const char> 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<char> mrb(2);
|
||||
// Pass in value with temporary object
|
||||
mrb.push_and_replace(1, std::shared_ptr<const char>(new char('a')));
|
||||
|
||||
std::shared_ptr<const char> 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<char> mrb(2);
|
||||
// Pass in value with temporary object
|
||||
mrb.push_and_replace(1, std::shared_ptr<const char>(new char('a')));
|
||||
|
||||
std::unique_ptr<char> 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<char> mrb(2);
|
||||
std::shared_ptr<const char> expected(new char('a'));
|
||||
|
||||
EXPECT_FALSE(mrb.push_and_replace(1, expected));
|
||||
EXPECT_EQ(2, expected.use_count());
|
||||
|
||||
std::shared_ptr<const char> 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<char> mrb(2);
|
||||
std::shared_ptr<const char> 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<char> 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<char> mrb(2);
|
||||
std::unique_ptr<char> expected(new char('a'));
|
||||
const char * expected_orig = expected.get();
|
||||
|
||||
EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected)));
|
||||
|
||||
std::unique_ptr<char> 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<char> mrb(2);
|
||||
std::unique_ptr<char> expected(new char('a'));
|
||||
const char * expected_orig = expected.get();
|
||||
|
||||
EXPECT_FALSE(mrb.push_and_replace(1, std::move(expected)));
|
||||
|
||||
std::shared_ptr<const char> 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<char> mrb(2);
|
||||
|
||||
std::shared_ptr<const char> 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<char> 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);
|
||||
}
|
||||
}
|
||||
@@ -159,15 +159,11 @@ static std::vector<TestParameters> invalid_qos_profiles()
|
||||
{
|
||||
std::vector<TestParameters> 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()),
|
||||
|
||||
80
rclcpp/test/test_ring_buffer_implementation.cpp
Normal file
80
rclcpp/test/test_ring_buffer_implementation.cpp
Normal file
@@ -0,0 +1,80 @@
|
||||
// Copyright 2019 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include "rclcpp/buffers/buffer_implementation_base.hpp"
|
||||
#include "rclcpp/buffers/ring_buffer_implementation.hpp"
|
||||
|
||||
/*
|
||||
Construtctor
|
||||
*/
|
||||
TEST(TestRingBufferImplementation, constructor) {
|
||||
// Cannot create a buffer of size zero.
|
||||
EXPECT_THROW(
|
||||
rclcpp::intra_process_buffer::RingBufferImplementation<char> rb(0), std::invalid_argument);
|
||||
|
||||
rclcpp::intra_process_buffer::RingBufferImplementation<char> rb(1);
|
||||
|
||||
EXPECT_EQ(false, rb.has_data());
|
||||
EXPECT_EQ(false, rb.is_full());
|
||||
}
|
||||
|
||||
/*
|
||||
Basic usage
|
||||
- insert data and check that it has data
|
||||
- extract data
|
||||
- overwrite old data writing over the buffer capacity
|
||||
*/
|
||||
TEST(TestRingBufferImplementation, basic_usage) {
|
||||
rclcpp::intra_process_buffer::RingBufferImplementation<char> 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());
|
||||
}
|
||||
@@ -16,6 +16,7 @@
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#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<rclcpp::Node>("test_subscription", "/ns");
|
||||
node = std::make_shared<rclcpp::Node>("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<TestParameters>
|
||||
{};
|
||||
|
||||
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<IntraProcessMessage>("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<IntraProcessMessage>(
|
||||
"topic",
|
||||
qos,
|
||||
callback);},
|
||||
std::invalid_argument);
|
||||
}
|
||||
}
|
||||
|
||||
static std::vector<TestParameters> invalid_qos_profiles()
|
||||
{
|
||||
std::vector<TestParameters> 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());
|
||||
|
||||
Reference in New Issue
Block a user