diagnosed_pub_sub.hpp

Go to the documentation of this file.

#pragma once

#include <functional>
#include <memory>
#include <string>
#include <type_traits>

#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>

#include <diagnostic_updater/diagnostic_updater.h>
#include <ros/forwards.h>
#include <ros/message_event.h>
#include <ros/message_traits.h>
#include <ros/node_handle.h>
#include <ros/publisher.h>
#include <ros/rate.h>
#include <ros/subscriber.h>
#include <ros/subscribe_options.h>

#include <cras_cpp_common/diag_utils/topic_status.hpp>
#include <cras_cpp_common/diag_utils/topic_status_param.hpp>
#include <cras_cpp_common/message_utils.hpp>
#include <cras_cpp_common/param_utils/bound_param_helper.hpp>
#include <cras_cpp_common/type_utils.hpp>

namespace cras
{

template<class Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
class DiagnosedPubSub
{
public:
  explicit DiagnosedPubSub(const ::std::shared_ptr<::cras::TopicStatus<Message>>& diag);

  explicit DiagnosedPubSub(const ::std::string& name, const ::cras::TopicStatusParam<Message>& diagParams);

  DiagnosedPubSub(const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<Message>& defaultParams);

  explicit DiagnosedPubSub(const ::cras::BoundParamHelperPtr& params);

  virtual ~DiagnosedPubSub() = default;

  virtual void attach(::diagnostic_updater::Updater& updater);

  virtual ::std::shared_ptr<::cras::TopicStatus<Message>> getDiagnosticTask() const;

protected:
  template <class M = Message, ::std::enable_if_t<::ros::message_traits::HasHeader<M>::value, bool> EnableM = true>
  void addDelayParams(::cras::SimpleTopicStatusParam<Message>& topicStatusParam,
    const ::cras::BoundParamHelperPtr& params);

  template <class M = Message, ::std::enable_if_t<!::ros::message_traits::HasHeader<M>::value, bool> EnableM = true>
  void addDelayParams(::cras::SimpleTopicStatusParam<Message>& topicStatusParam,
    const ::cras::BoundParamHelperPtr& params);

  ::std::shared_ptr<::cras::TopicStatus<Message>> diag {nullptr};
};

template<class Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
class DiagnosedPublisher : public ::cras::DiagnosedPubSub<Message>
{
public:
  DiagnosedPublisher(const ::ros::Publisher &pub, const ::std::shared_ptr<::cras::TopicStatus<Message>>& diag);

  DiagnosedPublisher(const ::ros::Publisher &pub, const ::std::string& name,
    const ::cras::TopicStatusParam<Message>& diagParams);

  DiagnosedPublisher(const ::ros::Publisher &pub, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<Message>& defaultParams);

  DiagnosedPublisher(const ::ros::Publisher &pub, const ::cras::BoundParamHelperPtr& params);

  [[deprecated]]
  DiagnosedPublisher(const ::ros::Publisher &pub, ::diagnostic_updater::Updater& updater,
    const ::cras::BoundParamHelperPtr& params);

  [[deprecated]]
  DiagnosedPublisher(const ::ros::Publisher &pub, ::diagnostic_updater::Updater& updater,
    const ::cras::BoundParamHelperPtr& params, const ::ros::Rate& defaultRate);

  [[deprecated]]
  DiagnosedPublisher(const ::ros::Publisher &pub, ::diagnostic_updater::Updater& updater,
    const ::cras::BoundParamHelperPtr& params, const ::ros::Rate& defaultRate,
    const ::ros::Rate& defaultMinRate, const ::ros::Rate& defaultMaxRate);

  [[deprecated]]
  ::ros::Rate getDesiredRate() const;

  virtual void publish(const typename Message::Ptr& message);

  virtual void publish(const Message& message);

  const ::ros::Publisher& getPublisher() const;

  ::ros::Publisher& getPublisher();

  void setPublisher(const ::ros::Publisher& pub);

protected:
  ::ros::Publisher publisher;
};

template<class Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
class DiagnosedSubscriber : public ::cras::DiagnosedPubSub<Message>
{
public:
  template <typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size, void(*cb)(M), ::ros::TransportHints hints = {});

  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<Message>& defaultParams,
    const ::std::string& topic, uint32_t queue_size,
    const ::boost::function<void(const ::boost::shared_ptr<Message>&)>& cb, ::ros::TransportHints hints = {});

  template <typename C, typename EnableC = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<C>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size,
    const ::boost::function<void(C)>& cb, ::ros::VoidConstPtr obj = {}, ::ros::TransportHints hints = {});

  template<typename M, class T, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), T* obj, ::ros::TransportHints hints = {});

  template<typename M, class T, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, T* obj, ::ros::TransportHints hints = {});

  template<typename M, class T, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), const ::boost::shared_ptr<T>& obj,
    ::ros::TransportHints hints = {});

  template<typename M, class T, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultParams,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj,
    ::ros::TransportHints hints = {});

  DiagnosedSubscriber(::ros::NodeHandle nh, const ::cras::BoundParamHelperPtr& params,
    const ::cras::SimpleTopicStatusParam<Message>& defaultParams, ::ros::SubscribeOptions& options);

  const ::ros::Subscriber& getSubscriber() const;

  ::ros::Subscriber& getSubscriber();

protected:
  ::boost::function<void(const Message&)> addTick(const ::std::function<void(const Message&)>& fn);

  ::boost::function<void(const typename Message::Ptr&)> addTick(
    const ::std::function<void(const typename Message::Ptr&)>& fn);

  ::boost::function<void(const ::ros::MessageEvent<Message>&)> addTick(
    const ::std::function<void(const ::ros::MessageEvent<Message>&)>& fn);

  template <typename T, typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::boost::function<void(M)> addTick(const ::std::function<void(M)>& fn, T);

  template <typename T, typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::boost::function<void(M)> addTick(void(T::*fn)(M), T* obj);

  template <typename T, typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::boost::function<void(M)> addTick(void(T::*fn)(M), const ::boost::shared_ptr<T>& obj);

  template <typename T, typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::boost::function<void(M)> addTick(void(T::*fn)(M) const, T* obj);

  template <typename T, typename M, typename EnableM = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::boost::function<void(M)> addTick(void(T::*fn)(M) const, const ::boost::shared_ptr<T>& obj);

  ::ros::Subscriber subscriber;
};

// Class template deduction guides allow leaving out the type parameter when declaring a DiagnosedSubscriber. C++17 only
#if __cpp_deduction_guides

template <typename M, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>&, const ::std::string&, uint32_t, void(*)(M),
  ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<M>>;

template <typename C, typename = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<C>>&, const ::std::string&, uint32_t,
  const ::boost::function<void(C)>&, ::ros::VoidConstPtr = {}, ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<C>>;

template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>&, const ::std::string&, uint32_t,
  void(T::*)(M), T*, ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<M>>;

template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>&, const ::std::string&, uint32_t,
  void(T::*)(M) const, T*, ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<M>>;

template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>&, const ::std::string&, uint32_t,
  void(T::*)(M), const ::boost::shared_ptr<T>&, ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<M>>;

template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
DiagnosedSubscriber(::ros::NodeHandle, const ::cras::BoundParamHelperPtr&,
  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>&, const ::std::string&, uint32_t,
  void(T::*)(M) const, const ::boost::shared_ptr<T>&, ::ros::TransportHints = {})
    -> DiagnosedSubscriber<::cras::BaseMessage<M>>;

#endif
}

#include <cras_cpp_common/diag_utils/impl/diagnosed_pub_sub.hpp>