Skip to content

Commit

Permalink
rebase ontop of master
Browse files Browse the repository at this point in the history
Signed-off-by: Karsten Knese <karsten@openrobotics.org>
  • Loading branch information
Karsten1987 committed Sep 28, 2019
1 parent 52dbe95 commit 45a6d70
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 30 deletions.
14 changes: 7 additions & 7 deletions rclcpp/include/rclcpp/loaned_message.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,21 +27,21 @@
namespace rclcpp
{

template<typename MessageT, typename Alloc>
template<typename MessageT, typename AllocatorT>
class Publisher;

template<typename MessageT, typename Alloc = std::allocator<void>>
template<typename MessageT, typename AllocatorT = std::allocator<void>>
class LoanedMessage
{
using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
using MessageAlloc = typename MessageAllocTraits::allocator_type;
using MessageAllocatorTraits = allocator::AllocRebind<MessageT, AllocatorT>;
using MessageAllocator = typename MessageAllocatorTraits::allocator_type;

protected:
const rclcpp::Publisher<MessageT, Alloc> * pub_;
const rclcpp::Publisher<MessageT, AllocatorT> * pub_;

std::unique_ptr<MessageT> message_;

const std::shared_ptr<MessageAlloc> message_allocator_;
const std::shared_ptr<MessageAllocator> message_allocator_;

/// Deleted copy constructor to preserve memory integrity
LoanedMessage(const LoanedMessage<MessageT> & other) = delete;
Expand All @@ -68,7 +68,7 @@ class LoanedMessage
* \param allocator Allocator instance in case middleware can not allocate messages
*/
LoanedMessage(
const rclcpp::Publisher<MessageT, Alloc> * pub,
const rclcpp::Publisher<MessageT, AllocatorT> * pub,
const std::shared_ptr<std::allocator<MessageT>> allocator)
: pub_(pub),
message_(nullptr),
Expand Down
37 changes: 14 additions & 23 deletions rclcpp/include/rclcpp/publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@
namespace rclcpp
{

template<typename MessageT, typename Alloc>
template<typename MessageT, typename AllocatorT>
class LoanedMessage;

/// A publisher publishes messages of any type to a topic.
Expand Down Expand Up @@ -150,40 +150,23 @@ class Publisher : public PublisherBase
*
* \return LoanedMessage containing memory for a ROS message of type MessageT
*/
rclcpp::LoanedMessage<MessageT, Alloc>
rclcpp::LoanedMessage<MessageT, AllocatorT>
loan_message()
{
return rclcpp::LoanedMessage<MessageT, Alloc>(this, this->get_allocator());
return rclcpp::LoanedMessage<MessageT, AllocatorT>(this, this->get_allocator());
}

/// Return and deallocate the memory for the loaned message.
/**
* \param loaned_message The LoanedMessage instance to be returned.
*/
void
return_loaned_message(rclcpp::LoanedMessage<MessageT, Alloc> && loaned_msg) const
return_loaned_message(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg) const
{
// scope exit
(void) loaned_msg;
}

/// Publish an instance of a LoanedMessage
/**
* When publishing a loaned message, the memory for this ROS instance will be deallocated
* after being published.
* The instance of the loaned message is no longer valid after this call.
*
* \param loaned_msg The LoanedMessage instance to be published.
*/
void
publish(std::unique_ptr<rclcpp::LoanedMessage<MessageT, Alloc>> loaned_msg)
{
if (!loaned_msg.is_valid()) {
throw std::runtime_error("loaned message is not valid");
}
return rcl_publish(&publisher_handle_, *loaned_msg->get(), nullptr, true);
}

/// Send a message to the topic for this publisher.
/**
* This function is templated on the input message type, MessageT.
Expand Down Expand Up @@ -243,13 +226,21 @@ class Publisher : public PublisherBase
return this->do_serialized_publish(&serialized_msg);
}

/// Publish an instance of a LoanedMessage
/**
* When publishing a loaned message, the memory for this ROS instance will be deallocated
* after being published.
* The instance of the loaned message is no longer valid after this call.
*
* \param loaned_msg The LoanedMessage instance to be published.
*/
void
publish(std::unique_ptr<rclcpp::LoanedMessage<MessageT, Alloc>> loaned_msg)
publish(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg)
{
if (!loaned_msg.is_valid()) {
throw std::runtime_error("loaned message is not valid");
}
return rcl_publish(&publisher_handle_, *loaned_msg->get(), nullptr, true);
return rcl_publish(&publisher_handle_, *loaned_msg.get(), nullptr, true);
}

std::shared_ptr<MessageAllocator>
Expand Down

0 comments on commit 45a6d70

Please sign in to comment.