node_handle_with_diagnostics.h

Go to the documentation of this file.

#pragma once

#include <memory>
#include <string>
#include <type_traits>
#include <utility>

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

#include <diagnostic_updater/diagnostic_updater.h>
#include <ros/advertise_options.h>
#include <ros/datatypes.h>
#include <ros/forwards.h>
#include <ros/message_traits.h>
#include <ros/node_handle.h>
#include <ros/subscribe_options.h>
#include <ros/transport_hints.h>

#include <cras_cpp_common/diag_utils/diagnosed_pub_sub.hpp>
#include <cras_cpp_common/diag_utils/topic_status_param.hpp>
#include <cras_cpp_common/log_utils/node.h>
#include <cras_cpp_common/message_utils.hpp>
#include <cras_cpp_common/node_utils/param_helper.h>
#include <cras_cpp_common/param_utils/bound_param_helper.hpp>
#include <cras_cpp_common/param_utils/get_param_adapters/node_handle.hpp>

namespace cras
{

class NodeHandleWithDiagnostics : public ::cras::NodeParamHelper
{
public:
  explicit NodeHandleWithDiagnostics(const ::std::string& ns = "", const ::ros::M_string& remappings = {});

  NodeHandleWithDiagnostics(const ::ros::NodeHandle& parent, const ::std::string& ns);

  NodeHandleWithDiagnostics(const ::ros::NodeHandle& parent, const ::std::string& ns,
    const ::ros::M_string& remappings);

protected:
  virtual ::std::string prefixDiagNamespace(const ::std::string& ns) const;

  virtual ::cras::BoundParamHelperPtr getDiagParams(const ::std::string& diagNs, const ::std::string& topic) const;

public:
  // ADVERTISE METHODS //

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, const size_t queueSize, const bool latch = false)
  {
    auto pub = this->template advertise<Message>(topic, queueSize, latch);
    auto result = ::std::make_unique<::cras::DiagnosedPublisher<Message>>(
      pub, this->getDiagParams(diagNamespace, topic), defaultDiagParams);
    result->attach(updater);
    return result;
  }

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
    ::ros::AdvertiseOptions& options)
  {
    const auto topic = options.topic;  // make copy of topic before advertise has a chance to change it
    auto pub = this->advertise(options);
    auto result = ::std::make_unique<::cras::DiagnosedPublisher<Message>>(
      pub, this->getDiagParams(diagNamespace, topic), defaultDiagParams);
    result->attach(updater);
    return result;
  }

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, const size_t queueSize, const bool latch = false)
  {
    return this->template advertiseDiagnosed<Message>(updater, ::cras::SimpleTopicStatusParam<Message>(), diagNamespace,
      topic, queueSize, latch);
  }

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    ::ros::AdvertiseOptions& options)
  {
    return this->template advertiseDiagnosed<Message>(updater, ::cras::SimpleTopicStatusParam<Message>(), diagNamespace,
      options);
  }

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, const size_t queueSize, const bool latch = false)
  {
    return this->template advertiseDiagnosed<Message>(updater, "", topic, queueSize, latch);
  }

  template <class Message>
  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>>
  advertiseDiagnosed(::diagnostic_updater::Updater& updater, ::ros::AdvertiseOptions& options)
  {
    return this->template advertiseDiagnosed<Message>(updater, "", options);
  }

  // SUBSCRIBE METHODS //

  template <typename M, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(*cb)(M), ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template <typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(const ::boost::shared_ptr<Message>&)>& cb, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>(new ::cras::DiagnosedSubscriber<Message>(
      *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template <typename C, typename = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<C>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(C)>& cb, ::ros::VoidConstPtr obj = {}, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, obj, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), T* obj, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, obj, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, T* obj, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, obj, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M), const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, obj, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>(
      new ::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>(
        *this, this->getDiagParams(diagNamespace, topic), defaultDiagParams, topic, queue_size, cb, obj, hints));
    result->attach(updater);
    return ::std::move(result);
  }

  template<typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
    ::ros::SubscribeOptions& options)
  {
    auto result = ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>(new ::cras::DiagnosedSubscriber<Message>(
      *this, this->getDiagParams(diagNamespace, options.topic), defaultDiagParams, options));
    result->attach(updater);
    return ::std::move(result);
  }

  template <typename M, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(*cb)(M), ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>(),
      diagNamespace, topic, queue_size, cb, hints);
  }

  template <typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(const ::boost::shared_ptr<Message>&)>& cb, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<Message>(updater, ::cras::SimpleTopicStatusParam<Message>(), diagNamespace,
      topic, queue_size, cb, hints);
  }

  template <typename C, typename = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(C)>& cb, ::ros::VoidConstPtr obj = {}, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<C>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<C>>(),
      diagNamespace, topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), T* obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>(),
      diagNamespace, topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, T* obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>(),
      diagNamespace, topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M), const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>(),
      diagNamespace, topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>(),
      diagNamespace, topic, queue_size, cb, obj, hints);
  }

  template<typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, const ::std::string& diagNamespace,
    ::ros::SubscribeOptions& options)
  {
    return this->template subscribeDiagnosed<Message>(updater, ::cras::SimpleTopicStatusParam<Message>(),
      diagNamespace, options);
  }

  template <typename M, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size, void(*cb)(M), ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, "", topic, queue_size, cb, hints);
  }

  template <typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(const ::boost::shared_ptr<Message>&)>& cb, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<Message>(updater, "", topic, queue_size, cb, hints);
  }

  template <typename C, typename = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size,
    const boost::function<void(C)>& cb, ::ros::VoidConstPtr obj = {}, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<C>(updater, "", topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), T* obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, "", topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, T* obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, "", topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M), const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, "", topic, queue_size, cb, obj, hints);
  }

  template<typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater,
    const ::std::string& topic, uint32_t queue_size,
    void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {})
  {
    return this->template subscribeDiagnosed<M>(updater, "", topic, queue_size, cb, obj, hints);
  }

  template<typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
  subscribeDiagnosed(::diagnostic_updater::Updater& updater, ::ros::SubscribeOptions& options)
  {
    return this->template subscribeDiagnosed<Message>(updater, "", options);
  }

protected:
  ::ros::NodeHandle parentNh {};
};

}