loaned_message.hpp

// 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__LOANED_MESSAGE_HPP_
#define RCLCPP__LOANED_MESSAGE_HPP_
 
#include <memory>
#include <utility>
 
#include "rclcpp/allocator/allocator_common.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/publisher_base.hpp"
 
#include "rcl/allocator.h"
#include "rcl/publisher.h"
 
namespace rclcpp
{
 
template<typename MessageT, typename AllocatorT = std::allocator<void>>
class LoanedMessage
{
public:
  using MessageAllocatorTraits = rclcpp::allocator::AllocRebind<MessageT, AllocatorT>;
  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
 
 
  LoanedMessage(
    const rclcpp::PublisherBase & pub,
    MessageAllocator allocator)
  : pub_(pub),
    message_(nullptr),
    message_allocator_(std::move(allocator))
  {
    if (pub_.can_loan_messages()) {
      void * message_ptr = nullptr;
      auto ret = rcl_borrow_loaned_message(
        pub_.get_publisher_handle().get(),
        rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
        &message_ptr);
      if (RCL_RET_OK != ret) {
        rclcpp::exceptions::throw_from_rcl_error(ret);
      }
      message_ = static_cast<MessageT *>(message_ptr);
    } else {
      RCLCPP_INFO_ONCE(
        rclcpp::get_logger("rclcpp"),
        "Currently used middleware can't loan messages. Local allocator will be used.");
      message_ = message_allocator_.allocate(1);
      new (message_) MessageT();
    }
  }
 
  LoanedMessage(LoanedMessage<MessageT> && other)
  : pub_(std::move(other.pub_)),
    message_(std::move(other.message_)),
    message_allocator_(std::move(other.message_allocator_))
  {
    other.message_ = nullptr;
  }
 
 
  virtual ~LoanedMessage()
  {
    auto error_logger = rclcpp::get_logger("LoanedMessage");
 
    if (message_ == nullptr) {
      return;
    }
 
    if (pub_.can_loan_messages()) {
      // return allocated memory to the middleware
      auto ret =
        rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_);
      if (ret != RCL_RET_OK) {
        RCLCPP_ERROR(
          error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str);
        rcl_reset_error();
      }
    } else {
      // call destructor before deallocating
      message_->~MessageT();
      message_allocator_.deallocate(message_, 1);
    }
    message_ = nullptr;
  }
 
 
  bool is_valid() const
  {
    return message_ != nullptr;
  }
 
 
  MessageT & get() const
  {
    return *message_;
  }
 
 
  std::unique_ptr<MessageT, std::function<void(MessageT *)>>
  release()
  {
    auto msg = message_;
    message_ = nullptr;
 
    if (pub_.can_loan_messages()) {
      return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(msg, [](MessageT *) {});
    }
 
    return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(
      msg,
      [allocator = message_allocator_](MessageT * msg_ptr) mutable {
        // call destructor before deallocating
        msg_ptr->~MessageT();
        allocator.deallocate(msg_ptr, 1);
      });
  }
 
protected:
  const rclcpp::PublisherBase & pub_;
 
  MessageT * message_;
 
  MessageAllocator message_allocator_;
 
  LoanedMessage(const LoanedMessage<MessageT> & other) = delete;
};
 
}  // namespace rclcpp
 
#endif  // RCLCPP__LOANED_MESSAGE_HPP_