Program Listing for File subscriber.hpp

↰ Return to documentation for file (include/message_filters/subscriber.hpp)

// Copyright 2009, Willow Garage, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the Willow Garage nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#ifndef MESSAGE_FILTERS__SUBSCRIBER_HPP_
#define MESSAGE_FILTERS__SUBSCRIBER_HPP_

#include <memory>
#include <stdexcept>
#include <string>
#include <type_traits>

#include <rclcpp/rclcpp.hpp>

#include "message_filters/connection.hpp"
#include "message_filters/simple_filter.hpp"

namespace message_filters
{

template<class NodeType = rclcpp::Node>
class SubscriberBase
{
public:
  typedef std::shared_ptr<NodeType> NodePtr;

  virtual ~SubscriberBase() = default;
  virtual void subscribe(
    NodePtr node, const std::string & topic,
    const rclcpp::QoS & qos) = 0;

  virtual void subscribe(
    NodeType * node, const std::string & topic,
    const rclcpp::QoS & qos) = 0;

  virtual void subscribe(
    NodePtr node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options)
  {
    this->subscribe(node.get(), topic, qos, options);
  }

  virtual void subscribe(
    NodeType * node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options) = 0;

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  virtual void subscribe(
    NodePtr node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  virtual void subscribe(
    NodeType * node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  virtual void subscribe(
    NodePtr node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options)
  {
    this->subscribe(
      node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options);
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  virtual void subscribe(
    NodeType * node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options) = 0;

  virtual void subscribe() = 0;
  virtual void unsubscribe() = 0;
};
template<typename T>
using SubscriberBasePtr = std::shared_ptr<SubscriberBase<T>>;

template<typename M, bool is_adapter = rclcpp::is_type_adapter<M>::value>
struct message_type;

template<typename M>
struct message_type<M, true>
{
  using type = typename M::custom_type;
};

template<typename M>
struct message_type<M, false>
{
  using type = M;
};

template<typename M>
using message_type_t = typename message_type<M>::type;

template<class M, class NodeType = rclcpp::Node>
class Subscriber
  : public SubscriberBase<NodeType>,
  public SimpleFilter<message_type_t<M>>
{
public:
  typedef std::shared_ptr<NodeType> NodePtr;
  typedef message_type_t<M> MessageType;
  typedef MessageEvent<MessageType const> EventType;
  Subscriber(
    NodePtr node, const std::string & topic,
    const rclcpp::QoS & qos)
  {
    subscribe(node, topic, qos);
  }

  Subscriber(
    NodeType * node, const std::string & topic,
    const rclcpp::QoS & qos)
  {
    subscribe(node, topic, qos);
  }

  Subscriber(
    NodePtr node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options)
  {
    subscribe(node.get(), topic, qos, options);
  }

  Subscriber(
    NodeType * node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options)
  {
    subscribe(node, topic, qos, options);
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  Subscriber(
    NodePtr node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default)
  {
    subscribe(node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)));
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  Subscriber(
    NodeType * node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default)
  {
    subscribe(node, topic, qos);
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  Subscriber(
    NodePtr node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options)
  {
    subscribe(node.get(), topic, qos, options);
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  Subscriber(
    NodeType * node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options)
  {
    subscribe(node, topic, qos, options);
  }

  Subscriber() = default;

  ~Subscriber()
  {
    unsubscribe();
  }

  void subscribe(
    NodePtr node, const std::string & topic,
    const rclcpp::QoS & qos) override
  {
    subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions());
  }

  void subscribe(
    NodeType * node, const std::string & topic,
    const rclcpp::QoS & qos) override
  {
    subscribe(node, topic, qos, rclcpp::SubscriptionOptions());
  }

  void subscribe(
    NodePtr node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options) override
  {
    subscribe(node.get(), topic, qos, options);
    node_raw_ = nullptr;
    node_shared_ = node;
  }

  void subscribe(
    NodeType * node,
    const std::string & topic,
    const rclcpp::QoS & qos,
    rclcpp::SubscriptionOptions options) override
  {
    unsubscribe();

    if (!topic.empty()) {
      topic_ = topic;
      qos_ = qos;
      options_ = options;
      sub_ = node->template create_subscription<M>(
        topic, qos,
        [this](const std::shared_ptr<MessageType const> msg) {
          this->cb(EventType(msg));
        }, options);

      node_raw_ = node;
    }
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  void subscribe(
    NodePtr node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default) override
  {
    subscribe(
      node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)),
      rclcpp::SubscriptionOptions());
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  void subscribe(
    NodeType * node, const std::string & topic,
    const rmw_qos_profile_t qos = rmw_qos_profile_default) override
  {
    subscribe(
      node, topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)),
      rclcpp::SubscriptionOptions());
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  void subscribe(
    NodePtr node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options) override
  {
    subscribe(node.get(), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos)), options);
    node_raw_ = nullptr;
    node_shared_ = node;
  }

  [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]]
  void subscribe(
    NodeType * node,
    const std::string & topic,
    const rmw_qos_profile_t qos,
    rclcpp::SubscriptionOptions options) override
  {
    unsubscribe();

    if (!topic.empty()) {
      topic_ = topic;
      rclcpp::QoS rclcpp_qos(rclcpp::QoSInitialization::from_rmw(qos));
      qos_ = rclcpp_qos;
      options_ = options;
      sub_ = node->template create_subscription<M>(
        topic, rclcpp_qos,
        [this](const std::shared_ptr<MessageType const> msg) {
          this->cb(EventType(msg));
        }, options);

      node_raw_ = node;
    }
  }

  void subscribe() override
  {
    if (!topic_.empty()) {
      if (node_raw_ != nullptr) {
        subscribe(node_raw_, topic_, qos_, options_);
      } else if (node_shared_ != nullptr) {
        subscribe(node_shared_, topic_, qos_, options_);
      }
    }
  }

  void unsubscribe() override
  {
    sub_.reset();
  }

  std::string getTopic() const
  {
    return this->topic_;
  }

  const typename rclcpp::Subscription<M>::SharedPtr getSubscriber() const {return sub_;}

  template<typename F>
  void connectInput(F & f)
  {
    (void)f;
  }

  void add(const EventType & e)
  {
    (void)e;
  }

private:
  void cb(const EventType & e)
  {
    this->signalMessage(e);
  }

  typename rclcpp::Subscription<M>::SharedPtr sub_;

  NodePtr node_shared_;
  NodeType * node_raw_ {nullptr};

  std::string topic_;
  rclcpp::QoS qos_ = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
  rclcpp::SubscriptionOptions options_;
};

}  // namespace message_filters

#endif  // MESSAGE_FILTERS__SUBSCRIBER_HPP_