roch_diagnostics.h

Go to the documentation of this file.

#ifndef roch_BASE_roch_DIAGNOSTICS_H
#define roch_BASE_roch_DIAGNOSTICS_H

#include "ros/ros.h"
#include "diagnostic_updater/diagnostic_updater.h"
#include "roch_base/core_wrapper.h"
#include "roch_msgs/RochStatus.h"

namespace roch_base
{

  class rochSoftwareDiagnosticTask :
    public diagnostic_updater::DiagnosticTask
  {
  public:
    explicit rochSoftwareDiagnosticTask(roch_msgs::RochStatus &msg, double target_control_freq);

    void run(diagnostic_updater::DiagnosticStatusWrapper &stat);

    void updateControlFrequency(double frequency);

  private:
    void reset();

    double control_freq_, target_control_freq_;
    roch_msgs::RochStatus &msg_;
  };

  template<typename T>
  class rochHardwareDiagnosticTask :
    public diagnostic_updater::DiagnosticTask
  {
  public:
    explicit rochHardwareDiagnosticTask(roch_msgs::RochStatus &msg);

    void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
    {
      typename core::Channel<T>::Ptr latest = core::Channel<T>::requestData(1.0);
      if (latest)
      {
        update(stat, latest);
      }
    }

    void update(diagnostic_updater::DiagnosticStatusWrapper &stat, typename core::Channel<T>::Ptr &status);

  private:
    roch_msgs::RochStatus &msg_;
  };

  template<>
  rochHardwareDiagnosticTask<sawyer::DataSystemStatus>::rochHardwareDiagnosticTask(roch_msgs::RochStatus &msg);

  template<>
  rochHardwareDiagnosticTask<sawyer::DataPowerSystem>::rochHardwareDiagnosticTask(roch_msgs::RochStatus &msg);

  template<>
  rochHardwareDiagnosticTask<sawyer::DataSafetySystemStatus>::rochHardwareDiagnosticTask(
    roch_msgs::RochStatus &msg);

  template<>
  void rochHardwareDiagnosticTask<sawyer::DataSystemStatus>::update(
    diagnostic_updater::DiagnosticStatusWrapper &stat,
    core::Channel<sawyer::DataSystemStatus>::Ptr &status);

  template<>
  void rochHardwareDiagnosticTask<sawyer::DataPowerSystem>::update(
    diagnostic_updater::DiagnosticStatusWrapper &stat,
    core::Channel<sawyer::DataPowerSystem>::Ptr &status);

  template<>
  void rochHardwareDiagnosticTask<sawyer::DataSafetySystemStatus>::update(
    diagnostic_updater::DiagnosticStatusWrapper &stat,
    core::Channel<sawyer::DataSafetySystemStatus>::Ptr &status);

}  // namespace roch_base
#endif  // roch_BASE_roch_DIAGNOSTICS_H