subscription.hpp

Go to the documentation of this file.

// Copyright 2014 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__SUBSCRIPTION_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_

#include <rmw/error_handling.h>
#include <rmw/rmw.h>

#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>

#include "rcl/error_handling.h"
#include "rcl/subscription.h"

#include "rcl_interfaces/msg/intra_process_message.hpp"

#include "rclcpp/exceptions.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/any_subscription_callback.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rclcpp/visibility_control.hpp"

namespace rclcpp
{

namespace node_interfaces
{
class NodeTopicsInterface;
}  // namespace node_interfaces

class SubscriptionBase
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase)

  

  RCLCPP_PUBLIC
  SubscriptionBase(
    std::shared_ptr<rcl_node_t> node_handle,
    const rosidl_message_type_support_t & type_support_handle,
    const std::string & topic_name,
    const rcl_subscription_options_t & subscription_options);

  RCLCPP_PUBLIC
  virtual ~SubscriptionBase();

  RCLCPP_PUBLIC
  const char *
  get_topic_name() const;

  RCLCPP_PUBLIC
  rcl_subscription_t *
  get_subscription_handle();

  RCLCPP_PUBLIC
  const rcl_subscription_t *
  get_subscription_handle() const;

  RCLCPP_PUBLIC
  virtual const rcl_subscription_t *
  get_intra_process_subscription_handle() const;


  virtual std::shared_ptr<void>
  create_message() = 0;

  virtual void
  handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;


  virtual void
  return_message(std::shared_ptr<void> & message) = 0;

  virtual void
  handle_intra_process_message(
    rcl_interfaces::msg::IntraProcessMessage & ipm,
    const rmw_message_info_t & message_info) = 0;

protected:
  rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
  rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
  std::shared_ptr<rcl_node_t> node_handle_;

private:
  RCLCPP_DISABLE_COPY(SubscriptionBase)
};

template<typename MessageT, typename Alloc = std::allocator<void>>
class Subscription : public SubscriptionBase
{
  friend class rclcpp::node_interfaces::NodeTopicsInterface;

public:
  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
  using MessageAlloc = typename MessageAllocTraits::allocator_type;
  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;

  RCLCPP_SMART_PTR_DEFINITIONS(Subscription)

  

  Subscription(
    std::shared_ptr<rcl_node_t> node_handle,
    const std::string & topic_name,
    const rcl_subscription_options_t & subscription_options,
    AnySubscriptionCallback<MessageT, Alloc> callback,
    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
    memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
    Alloc>::create_default())
  : SubscriptionBase(
      node_handle,
      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
      topic_name,
      subscription_options),
    any_callback_(callback),
    message_memory_strategy_(memory_strategy),
    get_intra_process_message_callback_(nullptr),
    matches_any_intra_process_publishers_(nullptr)
  {}


  void set_message_memory_strategy(
    typename message_memory_strategy::MessageMemoryStrategy<MessageT,
    Alloc>::SharedPtr message_memory_strategy)
  {
    message_memory_strategy_ = message_memory_strategy;
  }
  std::shared_ptr<void> create_message()
  {
    /* The default message memory strategy provides a dynamically allocated message on each call to
     * create_message, though alternative memory strategies that re-use a preallocated message may be
     * used (see rclcpp/strategies/message_pool_memory_strategy.hpp).
     */
    return message_memory_strategy_->borrow_message();
  }

  void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
  {
    if (matches_any_intra_process_publishers_) {
      if (matches_any_intra_process_publishers_(&message_info.publisher_gid)) {
        // In this case, the message will be delivered via intra process and
        // we should ignore this copy of the message.
        return;
      }
    }
    auto typed_message = std::static_pointer_cast<MessageT>(message);
    any_callback_.dispatch(typed_message, message_info);
  }


  void return_message(std::shared_ptr<void> & message)
  {
    auto typed_message = std::static_pointer_cast<MessageT>(message);
    message_memory_strategy_->return_message(typed_message);
  }

  void handle_intra_process_message(
    rcl_interfaces::msg::IntraProcessMessage & ipm,
    const rmw_message_info_t & message_info)
  {
    if (!get_intra_process_message_callback_) {
      // throw std::runtime_error(
      //   "handle_intra_process_message called before setup_intra_process");
      // TODO(wjwwood): for now, this could mean that intra process was just not enabled.
      // However, this can only really happen if this node has it disabled, but the other doesn't.
      return;
    }
    MessageUniquePtr msg;
    get_intra_process_message_callback_(
      ipm.publisher_id,
      ipm.message_sequence,
      intra_process_subscription_id_,
      msg);
    if (!msg) {
      // This either occurred because the publisher no longer exists or the
      // message requested is no longer being stored.
      // TODO(wjwwood): should we notify someone of this? log error, log warning?
      return;
    }
    any_callback_.dispatch_intra_process(msg, message_info);
  }

  using GetMessageCallbackType =
      std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
  using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;

  void setup_intra_process(
    uint64_t intra_process_subscription_id,
    GetMessageCallbackType get_message_callback,
    MatchesAnyPublishersCallbackType matches_any_publisher_callback,
    const rcl_subscription_options_t & intra_process_options)
  {
    std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
    rcl_ret_t ret = rcl_subscription_init(
      &intra_process_subscription_handle_,
      node_handle_.get(),
      rclcpp::type_support::get_intra_process_message_msg_type_support(),
      intra_process_topic_name.c_str(),
      &intra_process_options);
    if (ret != RCL_RET_OK) {
      if (ret == RCL_RET_TOPIC_NAME_INVALID) {
        auto rcl_node_handle = node_handle_.get();
        // this will throw on any validation problem
        rcl_reset_error();
        expand_topic_or_service_name(
          intra_process_topic_name,
          rcl_node_get_name(rcl_node_handle),
          rcl_node_get_namespace(rcl_node_handle));
      }

      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
    }

    intra_process_subscription_id_ = intra_process_subscription_id;
    get_intra_process_message_callback_ = get_message_callback;
    matches_any_intra_process_publishers_ = matches_any_publisher_callback;
  }

  const rcl_subscription_t *
  get_intra_process_subscription_handle() const
  {
    if (!get_intra_process_message_callback_) {
      return nullptr;
    }
    return &intra_process_subscription_handle_;
  }

private:
  RCLCPP_DISABLE_COPY(Subscription)

  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
  typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
    message_memory_strategy_;

  GetMessageCallbackType get_intra_process_message_callback_;
  MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
  uint64_t intra_process_subscription_id_;
};

}  // namespace rclcpp

#endif  // RCLCPP__SUBSCRIPTION_HPP_