subscriber_handler.h

Go to the documentation of this file.

// clang: MatousFormat
#ifndef SUBRSCRIBER_HANDLER_H
#define SUBRSCRIBER_HANDLER_H
 
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/time_source.hpp>
 
#include <mrs_lib/timeout_manager.h>
 
namespace mrs_lib
{
 
  static const rclcpp::Duration no_timeout = rclcpp::Duration(0, 0);
 
  /* struct SubscriberHandlerOptions //{ */
 
  struct SubscriberHandlerOptions
  {
    SubscriberHandlerOptions(const rclcpp::Node::SharedPtr& node) : node(node)
    {
    }
    SubscriberHandlerOptions() = default;
 
    rclcpp::Node::SharedPtr node; 
    std::string node_name = {}; 
    std::string topic_name = {}; 
    std::shared_ptr<mrs_lib::TimeoutManager> timeout_manager = nullptr; 
    rclcpp::Duration no_message_timeout =
        mrs_lib::no_timeout; 
    std::function<void(const std::string& topic_name, const rclcpp::Time& last_msg)> timeout_callback =
        {}; 
    bool threadsafe = true; 
    bool autostart =
        true; 
    rclcpp::QoS qos = rclcpp::SystemDefaultsQoS();
 
    rclcpp::SubscriptionOptions subscription_options = rclcpp::SubscriptionOptions();
  };
 
  //}
 
  /* SubscriberHandler class //{ */
  template <typename MessageType>
  class SubscriberHandler
  {
  public:
    using message_type = MessageType;
 
    using timeout_callback_t = std::function<void(const std::string& topic_name, const rclcpp::Time& last_msg)>;
 
    using message_callback_t = std::function<void(typename MessageType::ConstSharedPtr)>;
 
  public:
    virtual typename MessageType::ConstSharedPtr getMsg()
    {
      assert(m_pimpl);
      return m_pimpl->getMsg();
    };
 
    virtual typename MessageType::ConstSharedPtr peekMsg() const
    {
      assert(m_pimpl);
      return m_pimpl->peekMsg();
    };
 
    virtual bool hasMsg() const
    {
      assert(m_pimpl);
      return m_pimpl->hasMsg();
    };
 
    virtual bool newMsg() const
    {
      assert(m_pimpl);
      return m_pimpl->newMsg();
    };
 
    virtual bool usedMsg() const
    {
      assert(m_pimpl);
      return m_pimpl->usedMsg();
    };
 
    virtual typename MessageType::ConstSharedPtr waitForNew(const rclcpp::Duration& timeout)
    {
      assert(m_pimpl);
      return m_pimpl->waitForNew(timeout);
    };
 
    virtual rclcpp::Time lastMsgTime() const
    {
      assert(m_pimpl);
      return m_pimpl->lastMsgTime();
    };
 
    virtual std::string topicName() const
    {
      assert(m_pimpl);
      return m_pimpl->topicName();
    };
 
    virtual std::string subscribedTopicName() const
    {
      assert(m_pimpl);
      return m_pimpl->m_topic_name;
    };
 
    virtual uint32_t getNumPublishers() const
    {
      assert(m_pimpl);
      return m_pimpl->getNumPublishers();
    };
 
    virtual void setNoMessageTimeout(const rclcpp::Duration& timeout)
    {
      assert(m_pimpl);
      return m_pimpl->setNoMessageTimeout(timeout);
    };
 
    virtual void start()
    {
      assert(m_pimpl);
      return m_pimpl->start();
    };
 
    virtual void stop()
    {
      assert(m_pimpl);
      return m_pimpl->stop();
    };
 
  public:
    SubscriberHandler(){};
 
    template <class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, const std::string& topic_name, Types... args)
        : SubscriberHandler(
              [options, topic_name]() {
                SubscriberHandlerOptions opts = options;
                opts.topic_name = topic_name;
                return opts;
              }(),
              args...)
    {
    }
 
    SubscriberHandler(const SubscriberHandlerOptions& options, const message_callback_t& message_callback = {})
    {
      if (options.threadsafe)
      {
        m_pimpl = std::make_unique<ImplThreadsafe>(options, message_callback);
      } else
      {
        m_pimpl = std::make_unique<Impl>(options, message_callback);
      }
      if (options.autostart)
        start();
    };
 
    template <class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, const timeout_callback_t& timeout_callback, Types... args)
        : SubscriberHandler(
              [options, timeout_callback]() {
                SubscriberHandlerOptions opts = options;
                opts.timeout_callback = timeout_callback;
                return opts;
              }(),
              args...)
    {
    }
 
    template <class ObjectType2, class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, mrs_lib::Task<> (ObjectType2::*const message_callback)(typename MessageType::ConstSharedPtr),
                      ObjectType2* const obj2, Types... args)
        : SubscriberHandler(options, message_callback == nullptr ? message_callback_t() : createNonReentrantCallback(message_callback, obj2), args...)
    {
      internal::require_callback_group_coro_compatible(options.subscription_options.callback_group);
    }
 
    template <class ObjectType1, class ObjectType2, class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, mrs_lib::Task<> (ObjectType2::*const message_callback)(typename MessageType::ConstSharedPtr),
                      ObjectType2* const obj2, void (ObjectType1::*const timeout_callback)(const std::string& topic_name, const rclcpp::Time& last_msg),
                      ObjectType1* const obj1, Types... args)
        : SubscriberHandler(
              [options, timeout_callback, obj1]() {
                SubscriberHandlerOptions opts = options;
                opts.timeout_callback =
                    timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
                return opts;
              }(),
              message_callback == nullptr ? message_callback_t() : createNonReentrantCallback(message_callback, obj2), args...)
    {
      internal::require_callback_group_coro_compatible(options.subscription_options.callback_group);
    }
 
    template <class ObjectType1, class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options,
                      void (ObjectType1::*const timeout_callback)(const std::string& topic_name, const rclcpp::Time& last_msg), ObjectType1* const obj1,
                      Types... args)
        : SubscriberHandler(
              [options, timeout_callback, obj1]() {
                SubscriberHandlerOptions opts = options;
                opts.timeout_callback =
                    timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
                return opts;
              }(),
              args...)
    {
    }
 
    template <class ObjectType2, class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, void (ObjectType2::*const message_callback)(typename MessageType::ConstSharedPtr),
                      ObjectType2* const obj2, Types... args)
        : SubscriberHandler(options, message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1), args...)
    {
    }
 
    template <class ObjectType1, class ObjectType2, class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, void (ObjectType2::*const message_callback)(typename MessageType::ConstSharedPtr),
                      ObjectType2* const obj2, void (ObjectType1::*const timeout_callback)(const std::string& topic_name, const rclcpp::Time& last_msg),
                      ObjectType1* const obj1, Types... args)
        : SubscriberHandler(
              [options, timeout_callback, obj1]() {
                SubscriberHandlerOptions opts = options;
                opts.timeout_callback =
                    timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
                return opts;
              }(),
              message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1), args...)
    {
    }
 
    template <class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, const rclcpp::Duration& no_message_timeout, Types... args)
        : SubscriberHandler(
              [options, no_message_timeout]() {
                SubscriberHandlerOptions opts = options;
                opts.no_message_timeout = no_message_timeout;
                return opts;
              }(),
              args...)
    {
    }
 
    template <class... Types>
    SubscriberHandler(const SubscriberHandlerOptions& options, std::shared_ptr<mrs_lib::TimeoutManager>& timeout_manager, Types... args)
        : SubscriberHandler(options, timeout_manager, args...)
    {
    }
 
    ~SubscriberHandler() = default;
    // delete copy constructor and assignment operator (forbid copying shandlers)
    SubscriberHandler(const SubscriberHandler&) = delete;
    SubscriberHandler& operator=(const SubscriberHandler&) = delete;
    // define only move constructor and assignemnt operator
    SubscriberHandler(SubscriberHandler&& other)
    {
      this->m_pimpl = std::move(other.m_pimpl);
      other.m_pimpl = nullptr;
    }
    SubscriberHandler& operator=(SubscriberHandler&& other)
    {
      this->m_pimpl = std::move(other.m_pimpl);
      other.m_pimpl = nullptr;
      return *this;
    }
 
  private:
    class Impl;
    class ImplThreadsafe;
    std::unique_ptr<Impl> m_pimpl;
 
  protected:
    template <typename C>
    static std::function<void(typename MessageType::ConstSharedPtr)> createNonReentrantCallback(Task<> (C::*method)(typename MessageType::ConstSharedPtr msg),
                                                                                                C* instance)
    {
      auto is_running = std::make_shared<std::atomic<bool>>(false);
 
      return [is_running, method, instance](typename MessageType::ConstSharedPtr msg) -> void {
        bool was_running = is_running->exchange(true);
 
        if (!was_running)
        {
          internal::start_task(
              [](std::shared_ptr<std::atomic<bool>> is_running, Task<> (C::*method)(typename MessageType::ConstSharedPtr msg), C* instance,
                 typename MessageType::ConstSharedPtr msg) -> mrs_lib::Task<void> {
                // Run the user specified callback. We co_await it, but we do not
                // co_return the boolean result since start_task expects Task<void>.
                co_await std::invoke(method, instance, msg);
 
                is_running->store(false);
              },
              is_running, method, instance, msg);
        }
      };
    }
  };
  //}
 
  template <typename SubscriberHandler>
  using message_type = typename SubscriberHandler::message_type;
 
  template <typename Class, class... Types>
  void construct_object(Class& object, Types... args)
  {
    object = Class(args...);
  }
} // namespace mrs_lib
 
#ifndef SUBSCRIBER_HANDLER_HPP
#include <mrs_lib/impl/subscriber_handler.hpp>
#endif // SUBSCRIBER_HANDLER_HPP
 
#endif // SUBRSCRIBER_HANDLER_H