topic_status.hpp

Go to the documentation of this file.

#pragma once

#include <limits>
#include <memory>
#include <string>
#include <type_traits>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/update_functions.h>
#include <ros/message_event.h>
#include <ros/message_traits.h>

#include <cras_cpp_common/diag_utils/topic_status_param.hpp>
#include <cras_cpp_common/message_utils.hpp>
#include <cras_cpp_common/time_utils.hpp>

namespace cras
{

template <typename Message, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
class TopicStatus : public ::diagnostic_updater::CompositeDiagnosticTask
{
public:
  template <typename M = Message, ::std::enable_if_t<!::ros::message_traits::HasHeader<M>::value, bool> = true>
  TopicStatus(const ::std::string& name, const ::cras::TopicStatusParam<Message>& params) :
    ::diagnostic_updater::CompositeDiagnosticTask(name), origParams(params)
  {
    this->freqTask = ::std::make_unique<::diagnostic_updater::FrequencyStatus>(this->origParams);
    this->addTask(this->freqTask.get());
  }

  template <typename M = Message, ::std::enable_if_t<!::ros::message_traits::HasHeader<M>::value, bool> = true>
  explicit TopicStatus(const ::std::string& name,
    const double minRate = 0.0, const double maxRate = ::std::numeric_limits<double>::infinity(),
    const double rateTolerance = 0.1, const int rateWindowSize = 5) :
      TopicStatus(name, ::cras::TopicStatusParam<Message>(minRate, maxRate, rateTolerance, rateWindowSize))
  {
  }

  template <typename M = Message, ::std::enable_if_t<::ros::message_traits::HasHeader<M>::value, bool> = true>
  TopicStatus(const ::std::string& name, const ::cras::TopicStatusParam<Message>& params) :
    ::diagnostic_updater::CompositeDiagnosticTask(name), origParams(params)
  {
    this->freqTask = ::std::make_unique<::diagnostic_updater::FrequencyStatus>(this->origParams);
    this->stampTask = ::std::make_unique<::diagnostic_updater::SlowTimeStampStatus>(this->origParams);

    this->addTask(this->freqTask.get());
    this->addTask(this->stampTask.get());
  }

  template <typename M = Message, ::std::enable_if_t<::ros::message_traits::HasHeader<M>::value, bool> = true>
  explicit TopicStatus(const ::std::string& name,
    const double minRate = 0.0, const double maxRate = ::std::numeric_limits<double>::infinity(),
    const double rateTolerance = 0.1, const int rateWindowSize = 5,
    const double minDelay = -1.0, const double maxDelay = 5.0) :
      TopicStatus(name, ::cras::TopicStatusParam<Message>(
        minRate, maxRate, rateTolerance, rateWindowSize, minDelay, maxDelay))
  {
  }

  TopicStatus(const ::std::string& name, const ::cras::SimpleTopicStatusParam<Message>& params) :
      TopicStatus(name, ::cras::TopicStatusParam<Message>(params))
  {
  }

  ~TopicStatus() override
  {
  }

  virtual void tick(const ::ros::Time& stamp)
  {
    this->freqTask->tick();
    if (this->stampTask != nullptr)
      this->stampTask->tick(stamp);
  }

  virtual void tick(const Message& message)
  {
    this->freqTask->tick();
    if (this->stampTask != nullptr && ::ros::message_traits::HasHeader<::cras::BaseMessage<Message>>::value)
      this->stampTask->tick(*::ros::message_traits::TimeStamp<::cras::BaseMessage<Message>>::pointer(message));
  }

  virtual void tick(const typename Message::Ptr& message)
  {
    this->tick(*message);
  }

  virtual void tick(const typename Message::ConstPtr& message)
  {
    this->tick(*message);
  }

  virtual void tick(const ::ros::MessageEvent<Message>& event)
  {
    this->tick(*event.getConstMessage());
  }

  ::ros::Rate getExpectedRate() const
  {
    return ::cras::safeRate(this->origParams.getExpectedRate());
  }

  ::ros::Rate getMinRate() const
  {
    return ::cras::safeRate(*this->origParams.min_freq_);
  }

  ::ros::Rate getMaxRate() const
  {
    return ::cras::safeRate(*this->origParams.max_freq_);
  }

  double getRateTolerance() const
  {
    return this->origParams.tolerance_;
  }

  int getRateWindowSize() const
  {
    return this->origParams.window_size_;
  }

  template <typename M = Message, typename = ::std::enable_if_t<::ros::message_traits::HasHeader<M>::value>>
  ::ros::Duration getMinDelay() const
  {
    return ::ros::Duration(this->origParams.min_acceptable_);
  }

  template <typename M = Message, typename = ::std::enable_if_t<::ros::message_traits::HasHeader<M>::value>>
  ::ros::Duration getMaxDelay() const
  {
    return ::ros::Duration(this->origParams.max_acceptable_);
  }

protected:
  ::std::unique_ptr<::diagnostic_updater::FrequencyStatus> freqTask;

  ::std::unique_ptr<::diagnostic_updater::SlowTimeStampStatus> stampTask;

  ::cras::TopicStatusParam<Message> origParams;
};

template <typename Message>
using TopicStatusPtr = ::std::shared_ptr<::cras::TopicStatus<Message>>;

}