rebind allocator in mapped_ring_buffer
This commit is contained in:
parent
0cd13608f7
commit
ea21d9263a
11 changed files with 143 additions and 81 deletions
|
@ -12,8 +12,8 @@
|
||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
#ifndef RCLCPP_RCLCPP_ALLOCATOR_COMMON_HPP_
|
#ifndef RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
|
||||||
#define RCLCPP_RCLCPP_ALLOCATOR_COMMON_HPP_
|
#define RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
|
||||||
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
|
|
|
@ -50,7 +50,8 @@ public:
|
||||||
void operator()(U * ptr)
|
void operator()(U * ptr)
|
||||||
{
|
{
|
||||||
std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
|
std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
|
||||||
std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, sizeof(U));
|
// TODO: figure out if we're guaranteed to be destroying only 1 item here
|
||||||
|
std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, 1);
|
||||||
ptr = NULL;
|
ptr = NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -33,9 +33,10 @@ namespace any_subscription_callback
|
||||||
template<typename MessageT, typename Alloc>
|
template<typename MessageT, typename Alloc>
|
||||||
class AnySubscriptionCallback
|
class AnySubscriptionCallback
|
||||||
{
|
{
|
||||||
using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
|
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||||
using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
|
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||||
using UniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
|
||||||
|
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||||
|
|
||||||
using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
|
using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
|
||||||
using SharedPtrWithInfoCallback =
|
using SharedPtrWithInfoCallback =
|
||||||
|
@ -43,8 +44,8 @@ class AnySubscriptionCallback
|
||||||
using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
|
using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
|
||||||
using ConstSharedPtrWithInfoCallback =
|
using ConstSharedPtrWithInfoCallback =
|
||||||
std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
|
std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
|
||||||
using UniquePtrCallback = std::function<void(UniquePtr)>;
|
using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
|
||||||
using UniquePtrWithInfoCallback = std::function<void(UniquePtr, const rmw_message_info_t &)>;
|
using UniquePtrWithInfoCallback = std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
|
||||||
|
|
||||||
SharedPtrCallback shared_ptr_callback_;
|
SharedPtrCallback shared_ptr_callback_;
|
||||||
SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
|
SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
|
||||||
|
@ -59,8 +60,8 @@ public:
|
||||||
const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr),
|
const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr),
|
||||||
unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr)
|
unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr)
|
||||||
{
|
{
|
||||||
message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
|
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
|
||||||
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
|
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
|
AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
|
||||||
|
@ -162,20 +163,20 @@ public:
|
||||||
} else if (const_shared_ptr_with_info_callback_) {
|
} else if (const_shared_ptr_with_info_callback_) {
|
||||||
const_shared_ptr_with_info_callback_(message, message_info);
|
const_shared_ptr_with_info_callback_(message, message_info);
|
||||||
} else if (unique_ptr_callback_) {
|
} else if (unique_ptr_callback_) {
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr, *message);
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
|
||||||
unique_ptr_callback_(UniquePtr(ptr, message_deleter_));
|
unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_));
|
||||||
} else if (unique_ptr_with_info_callback_) {
|
} else if (unique_ptr_with_info_callback_) {
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr, *message);
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
|
||||||
unique_ptr_with_info_callback_(UniquePtr(ptr, message_deleter_), message_info);
|
unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info);
|
||||||
} else {
|
} else {
|
||||||
throw std::runtime_error("unexpected message without any callback set");
|
throw std::runtime_error("unexpected message without any callback set");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void dispatch_intra_process(
|
void dispatch_intra_process(
|
||||||
UniquePtr & message, const rmw_message_info_t & message_info)
|
MessageUniquePtr & message, const rmw_message_info_t & message_info)
|
||||||
{
|
{
|
||||||
(void)message_info;
|
(void)message_info;
|
||||||
if (shared_ptr_callback_) {
|
if (shared_ptr_callback_) {
|
||||||
|
@ -200,7 +201,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
typename MessageAlloc::allocator_type * message_allocator_;
|
std::shared_ptr<MessageAlloc> message_allocator_;
|
||||||
MessageDeleter message_deleter_;
|
MessageDeleter message_deleter_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -201,9 +201,10 @@ public:
|
||||||
throw std::invalid_argument("the calculated buffer size is too large");
|
throw std::invalid_argument("the calculated buffer size is too large");
|
||||||
}
|
}
|
||||||
publishers_[id].sequence_number.store(0);
|
publishers_[id].sequence_number.store(0);
|
||||||
using Deleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
|
publishers_[id].buffer =
|
||||||
publishers_[id].buffer = mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter>::make_shared(
|
mapped_ring_buffer::MappedRingBuffer<MessageT,
|
||||||
size);
|
typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
|
||||||
|
size, publisher->get_allocator());
|
||||||
publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
|
publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
|
||||||
return id;
|
return id;
|
||||||
}
|
}
|
||||||
|
@ -249,11 +250,13 @@ public:
|
||||||
* \param message the message that is being stored.
|
* \param message the message that is being stored.
|
||||||
* \return the message sequence number.
|
* \return the message sequence number.
|
||||||
*/
|
*/
|
||||||
template<typename MessageT, typename Deleter = std::default_delete<MessageT>>
|
template<typename MessageT, typename Alloc = std::allocator<void>>
|
||||||
uint64_t
|
uint64_t
|
||||||
store_intra_process_message(
|
store_intra_process_message(
|
||||||
uint64_t intra_process_publisher_id,
|
uint64_t intra_process_publisher_id,
|
||||||
std::unique_ptr<MessageT, Deleter> & message)
|
std::unique_ptr<MessageT,
|
||||||
|
typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
|
||||||
|
MessageT>> & message)
|
||||||
{
|
{
|
||||||
auto it = publishers_.find(intra_process_publisher_id);
|
auto it = publishers_.find(intra_process_publisher_id);
|
||||||
if (it == publishers_.end()) {
|
if (it == publishers_.end()) {
|
||||||
|
@ -263,7 +266,8 @@ public:
|
||||||
// Calculate the next message sequence number.
|
// Calculate the next message sequence number.
|
||||||
uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
|
uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
|
||||||
// Insert the message into the ring buffer using the message_seq to identify it.
|
// Insert the message into the ring buffer using the message_seq to identify it.
|
||||||
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter> TypedMRB;
|
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||||
|
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
|
||||||
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info.buffer);
|
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info.buffer);
|
||||||
bool did_replace = typed_buffer->push_and_replace(message_seq, message);
|
bool did_replace = typed_buffer->push_and_replace(message_seq, message);
|
||||||
// TODO(wjwwood): do something when a message was displaced. log debug?
|
// TODO(wjwwood): do something when a message was displaced. log debug?
|
||||||
|
@ -323,13 +327,15 @@ public:
|
||||||
* \param requesting_subscriptions_intra_process_id the subscription's id.
|
* \param requesting_subscriptions_intra_process_id the subscription's id.
|
||||||
* \param message the message typed unique_ptr used to return the message.
|
* \param message the message typed unique_ptr used to return the message.
|
||||||
*/
|
*/
|
||||||
template<typename MessageT, typename Deleter = std::default_delete<MessageT>>
|
template<typename MessageT, typename Alloc = std::allocator<void>>
|
||||||
void
|
void
|
||||||
take_intra_process_message(
|
take_intra_process_message(
|
||||||
uint64_t intra_process_publisher_id,
|
uint64_t intra_process_publisher_id,
|
||||||
uint64_t message_sequence_number,
|
uint64_t message_sequence_number,
|
||||||
uint64_t requesting_subscriptions_intra_process_id,
|
uint64_t requesting_subscriptions_intra_process_id,
|
||||||
std::unique_ptr<MessageT, Deleter> & message)
|
std::unique_ptr<MessageT,
|
||||||
|
typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
|
||||||
|
MessageT>> & message)
|
||||||
{
|
{
|
||||||
message = nullptr;
|
message = nullptr;
|
||||||
PublisherInfo * info;
|
PublisherInfo * info;
|
||||||
|
@ -361,7 +367,8 @@ public:
|
||||||
}
|
}
|
||||||
target_subs->erase(it);
|
target_subs->erase(it);
|
||||||
}
|
}
|
||||||
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter> TypedMRB;
|
using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
|
||||||
|
typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
|
||||||
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info->buffer);
|
typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info->buffer);
|
||||||
// Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
|
// Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
|
||||||
if (target_subs->size()) {
|
if (target_subs->size()) {
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
#ifndef RCLCPP_RCLCPP_RING_BUFFER_HPP_
|
#ifndef RCLCPP_RCLCPP_RING_BUFFER_HPP_
|
||||||
#define RCLCPP_RCLCPP_RING_BUFFER_HPP_
|
#define RCLCPP_RCLCPP_RING_BUFFER_HPP_
|
||||||
|
|
||||||
|
#include <rclcpp/allocator/allocator_common.hpp>
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
@ -50,25 +51,35 @@ public:
|
||||||
* there is no guarantee on which value is returned if a key is used multiple
|
* there is no guarantee on which value is returned if a key is used multiple
|
||||||
* times.
|
* times.
|
||||||
*/
|
*/
|
||||||
template<typename T, typename Deleter = std::default_delete<T>>
|
template<typename T, typename Alloc = std::allocator<void>>
|
||||||
class MappedRingBuffer : public MappedRingBufferBase
|
class MappedRingBuffer : public MappedRingBufferBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Deleter>);
|
RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>);
|
||||||
using ElemUniquePtr = std::unique_ptr<T, Deleter>;
|
using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
|
||||||
|
using ElemAlloc = typename ElemAllocTraits::allocator_type;
|
||||||
|
using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
|
||||||
|
|
||||||
|
using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
|
||||||
|
|
||||||
/// Constructor.
|
/// Constructor.
|
||||||
/* The constructor will allocate memory while reserving space.
|
/* The constructor will allocate memory while reserving space.
|
||||||
*
|
*
|
||||||
* \param size size of the ring buffer; must be positive and non-zero.
|
* \param size size of the ring buffer; must be positive and non-zero.
|
||||||
*/
|
*/
|
||||||
MappedRingBuffer(size_t size)
|
MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
|
||||||
: elements_(size), head_(0)
|
: elements_(size), head_(0)
|
||||||
{
|
{
|
||||||
if (size == 0) {
|
if (size == 0) {
|
||||||
throw std::invalid_argument("size must be a positive, non-zero value");
|
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() {}
|
virtual ~MappedRingBuffer() {}
|
||||||
|
|
||||||
/// Return a copy of the value stored in the ring buffer at the given key.
|
/// Return a copy of the value stored in the ring buffer at the given key.
|
||||||
|
@ -88,7 +99,9 @@ public:
|
||||||
auto it = get_iterator_of_key(key);
|
auto it = get_iterator_of_key(key);
|
||||||
value = nullptr;
|
value = nullptr;
|
||||||
if (it != elements_.end() && it->in_use) {
|
if (it != elements_.end() && it->in_use) {
|
||||||
value = ElemUniquePtr(new T(*it->value));
|
auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
|
||||||
|
ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
|
||||||
|
value = ElemUniquePtr(ptr);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -118,7 +131,9 @@ public:
|
||||||
value = nullptr;
|
value = nullptr;
|
||||||
if (it != elements_.end() && it->in_use) {
|
if (it != elements_.end() && it->in_use) {
|
||||||
// Make a copy.
|
// Make a copy.
|
||||||
auto copy = ElemUniquePtr(new T(*it->value));
|
auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
|
||||||
|
ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
|
||||||
|
auto copy = ElemUniquePtr(ptr);
|
||||||
// Return the original.
|
// Return the original.
|
||||||
value.swap(it->value);
|
value.swap(it->value);
|
||||||
// Store the copy.
|
// Store the copy.
|
||||||
|
@ -184,7 +199,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Deleter>);
|
RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
|
||||||
|
|
||||||
struct element
|
struct element
|
||||||
{
|
{
|
||||||
|
@ -193,7 +208,9 @@ private:
|
||||||
bool in_use;
|
bool in_use;
|
||||||
};
|
};
|
||||||
|
|
||||||
typename std::vector<element>::iterator
|
using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<element>;
|
||||||
|
|
||||||
|
typename std::vector<element, VectorAlloc>::iterator
|
||||||
get_iterator_of_key(uint64_t key)
|
get_iterator_of_key(uint64_t key)
|
||||||
{
|
{
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
|
@ -204,8 +221,9 @@ private:
|
||||||
return it;
|
return it;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<element> elements_;
|
std::vector<element, VectorAlloc> elements_;
|
||||||
size_t head_;
|
size_t head_;
|
||||||
|
std::shared_ptr<ElemAlloc> allocator_;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -34,17 +34,18 @@ class MessageMemoryStrategy
|
||||||
public:
|
public:
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
|
RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
|
||||||
|
|
||||||
using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
|
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||||
using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
|
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||||
|
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
|
||||||
|
|
||||||
MessageMemoryStrategy()
|
MessageMemoryStrategy()
|
||||||
{
|
{
|
||||||
message_allocator_ = new typename MessageAlloc::allocator_type();
|
message_allocator_ = std::make_shared<MessageAlloc>();
|
||||||
}
|
}
|
||||||
|
|
||||||
MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
|
MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
|
||||||
{
|
{
|
||||||
message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
|
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Default factory method
|
/// Default factory method
|
||||||
|
@ -57,9 +58,10 @@ public:
|
||||||
// \return Shared pointer to the new message.
|
// \return Shared pointer to the new message.
|
||||||
virtual std::shared_ptr<MessageT> borrow_message()
|
virtual std::shared_ptr<MessageT> borrow_message()
|
||||||
{
|
{
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr);
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr);
|
||||||
return std::shared_ptr<MessageT>(ptr, message_deleter_);
|
return std::shared_ptr<MessageT>(ptr, message_deleter_);
|
||||||
|
//return std::allocate_shared<MessageT, typename MessageAlloc::allocator_type>(*message_allocator_);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Release ownership of the message, which will deallocate it if it has no more owners.
|
/// Release ownership of the message, which will deallocate it if it has no more owners.
|
||||||
|
@ -69,7 +71,7 @@ public:
|
||||||
msg.reset();
|
msg.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
typename MessageAlloc::allocator_type * message_allocator_;
|
std::shared_ptr<MessageAlloc> message_allocator_;
|
||||||
MessageDeleter message_deleter_;
|
MessageDeleter message_deleter_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -187,8 +187,9 @@ Node::create_publisher(
|
||||||
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
|
"' is incompatible from the publisher type '" + message_type_info.name() + "'");
|
||||||
}
|
}
|
||||||
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
|
MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
|
||||||
std::unique_ptr<MessageT> unique_msg(typed_message_ptr);
|
using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
|
||||||
uint64_t message_seq = ipm->store_intra_process_message(publisher_id, unique_msg);
|
std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
|
||||||
|
uint64_t message_seq = ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
|
||||||
return message_seq;
|
return message_seq;
|
||||||
};
|
};
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
|
@ -293,7 +294,7 @@ Node::create_subscription(
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"intra process take called after destruction of intra process manager");
|
"intra process take called after destruction of intra process manager");
|
||||||
}
|
}
|
||||||
ipm->take_intra_process_message(publisher_id, message_sequence, subscription_id, message);
|
ipm->take_intra_process_message<MessageT, Alloc>(publisher_id, message_sequence, subscription_id, message);
|
||||||
},
|
},
|
||||||
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
|
[weak_ipm](const rmw_gid_t * sender_gid) -> bool {
|
||||||
auto ipm = weak_ipm.lock();
|
auto ipm = weak_ipm.lock();
|
||||||
|
|
|
@ -211,8 +211,9 @@ class Publisher : public PublisherBase
|
||||||
friend rclcpp::node::Node;
|
friend rclcpp::node::Node;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
|
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||||
using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
|
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||||
|
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
|
||||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||||
|
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
|
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
|
||||||
|
@ -225,8 +226,8 @@ public:
|
||||||
std::shared_ptr<Alloc> allocator)
|
std::shared_ptr<Alloc> allocator)
|
||||||
: PublisherBase(node_handle, publisher_handle, topic, queue_size)
|
: PublisherBase(node_handle, publisher_handle, topic, queue_size)
|
||||||
{
|
{
|
||||||
message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
|
message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
|
||||||
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
|
allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -237,7 +238,6 @@ public:
|
||||||
*/
|
*/
|
||||||
void
|
void
|
||||||
publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
|
publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
|
||||||
//publish(MessageUniquePtr & msg)
|
|
||||||
{
|
{
|
||||||
this->do_inter_process_publish(msg.get());
|
this->do_inter_process_publish(msg.get());
|
||||||
if (store_intra_process_message_) {
|
if (store_intra_process_message_) {
|
||||||
|
@ -285,8 +285,8 @@ public:
|
||||||
// The intra process manager should probably also be able to store
|
// The intra process manager should probably also be able to store
|
||||||
// shared_ptr's and do the "smart" thing based on other intra process
|
// shared_ptr's and do the "smart" thing based on other intra process
|
||||||
// subscriptions. For now call the other publish().
|
// subscriptions. For now call the other publish().
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
|
||||||
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
||||||
return this->publish(unique_msg);
|
return this->publish(unique_msg);
|
||||||
}
|
}
|
||||||
|
@ -304,8 +304,8 @@ public:
|
||||||
// The intra process manager should probably also be able to store
|
// The intra process manager should probably also be able to store
|
||||||
// shared_ptr's and do the "smart" thing based on other intra process
|
// shared_ptr's and do the "smart" thing based on other intra process
|
||||||
// subscriptions. For now call the other publish().
|
// subscriptions. For now call the other publish().
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
|
||||||
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
||||||
return this->publish(unique_msg);
|
return this->publish(unique_msg);
|
||||||
}
|
}
|
||||||
|
@ -319,12 +319,17 @@ public:
|
||||||
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.
|
// Otherwise we have to allocate memory in a unique_ptr and pass it along.
|
||||||
auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
|
auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
|
||||||
MessageAlloc::construct(*message_allocator_, ptr, msg);
|
MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
|
||||||
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
MessageUniquePtr unique_msg(ptr, message_deleter_);
|
||||||
return this->publish(unique_msg);
|
return this->publish(unique_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<MessageAlloc> get_allocator() const
|
||||||
|
{
|
||||||
|
return message_allocator_;
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void
|
void
|
||||||
do_inter_process_publish(const MessageT * msg)
|
do_inter_process_publish(const MessageT * msg)
|
||||||
|
@ -338,7 +343,7 @@ protected:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
typename MessageAlloc::allocator_type * message_allocator_;
|
std::shared_ptr<MessageAlloc> message_allocator_;
|
||||||
|
|
||||||
MessageDeleter message_deleter_;
|
MessageDeleter message_deleter_;
|
||||||
|
|
||||||
|
|
|
@ -35,21 +35,22 @@ namespace allocator_memory_strategy
|
||||||
* come through.
|
* come through.
|
||||||
*/
|
*/
|
||||||
template<typename Alloc>
|
template<typename Alloc>
|
||||||
class AllocatorMemoryStrategy : public MemoryStrategy
|
class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
|
||||||
{
|
{
|
||||||
|
|
||||||
public:
|
public:
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
|
RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
|
||||||
|
|
||||||
using ExecAlloc = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
|
using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
|
||||||
using ExecDeleter =
|
using ExecAlloc = typename ExecAllocTraits::allocator_type;
|
||||||
allocator::Deleter<typename ExecAlloc::allocator_type, executor::AnyExecutable>;
|
using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
|
||||||
using VoidAlloc = allocator::AllocRebind<void *, Alloc>;
|
using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
|
||||||
|
using VoidAlloc = typename VoidAllocTraits::allocator_type;
|
||||||
|
|
||||||
AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
|
AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
|
||||||
{
|
{
|
||||||
executable_allocator_ = new ExecAlloc(*allocator.get());
|
executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
|
||||||
allocator_ = new VoidAlloc(*allocator.get());
|
allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
|
/// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
|
||||||
|
@ -137,8 +138,8 @@ public:
|
||||||
virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
|
virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
|
||||||
{
|
{
|
||||||
//return std::make_shared<executor::AnyExecutable>();
|
//return std::make_shared<executor::AnyExecutable>();
|
||||||
auto ptr = ExecAlloc::allocate(*executable_allocator_, 1);
|
auto ptr = ExecAllocTraits::allocate(*executable_allocator_.get(), 1);
|
||||||
ExecAlloc::construct(*executable_allocator_);
|
ExecAllocTraits::construct(*executable_allocator_.get(), ptr);
|
||||||
return std::shared_ptr<executor::AnyExecutable>(ptr, executable_deleter_);
|
return std::shared_ptr<executor::AnyExecutable>(ptr, executable_deleter_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -152,7 +153,9 @@ public:
|
||||||
if (size == 0) {
|
if (size == 0) {
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
return VoidAlloc::allocate(*allocator_, size);
|
auto ptr = VoidAllocTraits::allocate(*allocator_.get(), size);
|
||||||
|
alloc_map[ptr] = size;
|
||||||
|
return ptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Implementation of a general-purpose free.
|
/// Implementation of a general-purpose free.
|
||||||
|
@ -161,22 +164,34 @@ public:
|
||||||
*/
|
*/
|
||||||
virtual void free(void * ptr)
|
virtual void free(void * ptr)
|
||||||
{
|
{
|
||||||
VoidAlloc::deallocate(*allocator, ptr);
|
if (alloc_map.count(ptr) == 0) {
|
||||||
|
// do nothing, the pointer is not in the alloc'd map
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
VoidAllocTraits::deallocate(*allocator_.get(), &ptr, alloc_map[ptr]);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr, Alloc> subs;
|
template<typename U>
|
||||||
std::vector<rclcpp::service::ServiceBase::SharedPtr, Alloc> services;
|
using VectorRebind = typename std::allocator_traits<Alloc>::template rebind_alloc<U>;
|
||||||
std::vector<rclcpp::client::ClientBase::SharedPtr, Alloc> clients;
|
|
||||||
|
|
||||||
std::vector<void *, Alloc> subscription_handles;
|
std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr,
|
||||||
std::vector<void *, Alloc> service_handles;
|
VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr>> subs;
|
||||||
std::vector<void *, Alloc> client_handles;
|
std::vector<rclcpp::service::ServiceBase::SharedPtr,
|
||||||
|
VectorRebind<rclcpp::service::ServiceBase::SharedPtr>> services;
|
||||||
|
std::vector<rclcpp::client::ClientBase::SharedPtr,
|
||||||
|
VectorRebind<rclcpp::client::ClientBase::SharedPtr>> clients;
|
||||||
|
|
||||||
|
std::vector<void *, VoidAlloc> subscription_handles;
|
||||||
|
std::vector<void *, VoidAlloc> service_handles;
|
||||||
|
std::vector<void *, VoidAlloc> client_handles;
|
||||||
std::array<void *, 2> guard_cond_handles;
|
std::array<void *, 2> guard_cond_handles;
|
||||||
|
|
||||||
|
std::unordered_map<void *, size_t> alloc_map;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
typename ExecAlloc::allocator_type * executable_allocator_;
|
std::shared_ptr<ExecAlloc> executable_allocator_;
|
||||||
ExecDeleter executable_deleter_;
|
ExecDeleter executable_deleter_;
|
||||||
typename VoidAlloc::allocator_type * allocator_;
|
std::shared_ptr<VoidAlloc> allocator_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} /* allocator_memory_strategy */
|
} /* allocator_memory_strategy */
|
||||||
|
|
|
@ -150,8 +150,9 @@ class Subscription : public SubscriptionBase
|
||||||
friend class rclcpp::node::Node;
|
friend class rclcpp::node::Node;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
|
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
|
||||||
using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
|
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||||
|
using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
|
||||||
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
|
||||||
|
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
|
RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
|
||||||
|
|
|
@ -59,10 +59,21 @@ template<typename T, typename Alloc = std::allocator<void>>
|
||||||
class Publisher : public PublisherBase
|
class Publisher : public PublisherBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
using MessageAlloc = allocator::AllocRebind<T, Alloc>;
|
using MessageAllocTraits = allocator::AllocRebind<T, Alloc>;
|
||||||
using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, T>;
|
using MessageAlloc = typename MessageAllocTraits::allocator_type;
|
||||||
|
using MessageDeleter = allocator::Deleter<MessageAlloc, T>;
|
||||||
|
using MessageUniquePtr = std::unique_ptr<T, MessageDeleter>;
|
||||||
|
std::shared_ptr<MessageAlloc> allocator_;
|
||||||
|
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
|
RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
|
||||||
|
|
||||||
|
Publisher() {
|
||||||
|
allocator_ = std::make_shared<MessageAlloc>();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::shared_ptr<MessageAlloc> get_allocator() {
|
||||||
|
return allocator_;
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue