intra_process_manager.hpp

Go to the documentation of this file.

// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_
#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_

#include <rmw/types.h>

#include <algorithm>
#include <atomic>
#include <cstdint>
#include <exception>
#include <map>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <utility>
#include <set>

#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/intra_process_manager_impl.hpp"
#include "rclcpp/mapped_ring_buffer.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher_base.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{
namespace intra_process_manager
{


class IntraProcessManager
{
private:
  RCLCPP_DISABLE_COPY(IntraProcessManager)

public:
  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)

  RCLCPP_PUBLIC
  explicit IntraProcessManager(
    IntraProcessManagerImplBase::SharedPtr state = create_default_impl());

  RCLCPP_PUBLIC
  virtual ~IntraProcessManager();


  RCLCPP_PUBLIC
  uint64_t
  add_subscription(SubscriptionBase::SharedPtr subscription);


  RCLCPP_PUBLIC
  void
  remove_subscription(uint64_t intra_process_subscription_id);


  RCLCPP_PUBLIC
  uint64_t
  add_publisher(
    rclcpp::PublisherBase::SharedPtr publisher,
    size_t buffer_size = 0);


  RCLCPP_PUBLIC
  void
  remove_publisher(uint64_t intra_process_publisher_id);


  template<
    typename MessageT, typename Alloc = std::allocator<void>>
  uint64_t
  store_intra_process_message(
    uint64_t intra_process_publisher_id,
    std::shared_ptr<const MessageT> 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, message);
    // TODO(wjwwood): do something when a message was displaced. log debug?
    (void)did_replace;  // Avoid unused variable warning.

    impl_->store_intra_process_message(intra_process_publisher_id, message_seq);

    // Return the message sequence which is sent to the subscription.
    return message_seq;
  }

  template<
    typename MessageT, typename Alloc = std::allocator<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;
  }


  template<
    typename MessageT, typename Alloc = std::allocator<void>,
    typename Deleter = std::default_delete<MessageT>>
  void
  take_intra_process_message(
    uint64_t intra_process_publisher_id,
    uint64_t message_sequence_number,
    uint64_t requesting_subscriptions_intra_process_id,
    std::unique_ptr<MessageT, Deleter> & message)
  {
    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
    using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
    message = nullptr;

    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);
    }
  }

  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;

    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);
    }
  }

  RCLCPP_PUBLIC
  bool
  matches_any_publishers(const rmw_gid_t * id) const;

  RCLCPP_PUBLIC
  size_t
  get_subscription_count(uint64_t intra_process_publisher_id) const;

private:
  RCLCPP_PUBLIC
  static uint64_t
  get_next_unique_id();

  IntraProcessManagerImplBase::SharedPtr impl_;
  std::mutex take_mutex_;
};

}  // namespace intra_process_manager
}  // namespace rclcpp

#endif  // RCLCPP__INTRA_PROCESS_MANAGER_HPP_