intra_process_manager.hpp

Go to the documentation of this file.

// 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__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_
#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_

#include <rmw/types.h>

#include <shared_mutex>

#include <algorithm>
#include <atomic>
#include <cstdint>
#include <exception>
#include <map>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "rclcpp/allocator/allocator_deleter.hpp"
#include "rclcpp/experimental/subscription_intra_process.hpp"
#include "rclcpp/experimental/subscription_intra_process_base.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/publisher_base.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{

namespace experimental
{


class IntraProcessManager
{
private:
  RCLCPP_DISABLE_COPY(IntraProcessManager)

public:
  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)

  RCLCPP_PUBLIC
  IntraProcessManager();

  RCLCPP_PUBLIC
  virtual ~IntraProcessManager();


  RCLCPP_PUBLIC
  uint64_t
  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);


  RCLCPP_PUBLIC
  void
  remove_subscription(uint64_t intra_process_subscription_id);


  RCLCPP_PUBLIC
  uint64_t
  add_publisher(rclcpp::PublisherBase::SharedPtr publisher);


  RCLCPP_PUBLIC
  void
  remove_publisher(uint64_t intra_process_publisher_id);


  template<
    typename MessageT,
    typename Alloc = std::allocator<void>,
    typename Deleter = std::default_delete<MessageT>>
  void
  do_intra_process_publish(
    uint64_t intra_process_publisher_id,
    std::unique_ptr<MessageT, Deleter> message,
    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
  {
    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;

    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;

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

      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
      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);

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

  template<
    typename MessageT,
    typename Alloc = std::allocator<void>,
    typename Deleter = std::default_delete<MessageT>>
  std::shared_ptr<const MessageT>
  do_intra_process_publish_and_return_shared(
    uint64_t intra_process_publisher_id,
    std::unique_ptr<MessageT, Deleter> message,
    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
  {
    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;

    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 nullptr;
    }
    const auto & sub_ids = publisher_it->second;

    if (sub_ids.take_ownership_subscriptions.empty()) {
      // If there are no owning, just convert to shared.
      std::shared_ptr<MessageT> shared_msg = std::move(message);
      if (!sub_ids.take_shared_subscriptions.empty()) {
        this->template add_shared_msg_to_buffers<MessageT>(shared_msg,
          sub_ids.take_shared_subscriptions);
      }
      return shared_msg;
    } else {
      // Construct a new shared pointer from the message for the buffers that
      // do not require ownership and to return.
      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);

      if (!sub_ids.take_shared_subscriptions.empty()) {
        this->template add_shared_msg_to_buffers<MessageT>(
          shared_msg,
          sub_ids.take_shared_subscriptions);
      }
      if (!sub_ids.take_ownership_subscriptions.empty()) {
        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
          std::move(message),
          sub_ids.take_ownership_subscriptions,
          allocator);
      }

      return shared_msg;
    }
  }

  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;

  RCLCPP_PUBLIC
  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
  get_subscription_intra_process(uint64_t intra_process_subscription_id);

private:
  struct SubscriptionInfo
  {
    SubscriptionInfo() = default;

    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription;
    rmw_qos_profile_t qos;
    const char * topic_name;
    bool use_take_shared_method;
  };

  struct PublisherInfo
  {
    PublisherInfo() = default;

    rclcpp::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();

  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<
        rclcpp::experimental::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<
        rclcpp::experimental::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 experimental
}  // namespace rclcpp

#endif  // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_