galactic/include/diagnostic_updater/diagnostic_updater.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
#ifndef DIAGNOSTIC_UPDATER__DIAGNOSTIC_UPDATER_HPP_
#define DIAGNOSTIC_UPDATER__DIAGNOSTIC_UPDATER_HPP_
 
#include <functional>  // for bind()
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
 
#include "builtin_interfaces/msg/time.hpp"
 
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
 
#include "diagnostic_updater/diagnostic_status_wrapper.hpp"
 
#include "rcl/time.h"
 
#include "rclcpp/create_timer.hpp"
#include "rclcpp/rclcpp.hpp"
 
namespace diagnostic_updater
{
 
typedef std::function<void (DiagnosticStatusWrapper &)> TaskFunction;
typedef std::function<void (diagnostic_msgs::msg::DiagnosticStatus &)>
  UnwrappedTaskFunction;
 
class DiagnosticTask
{
public:
  explicit DiagnosticTask(const std::string name)
  : name_(name) {}
 
  const std::string & getName() {return name_;}
 
  virtual void run(diagnostic_updater::DiagnosticStatusWrapper & stat) = 0;
 
  virtual ~DiagnosticTask() {}
 
private:
  const std::string name_;
};
 
template<class T>
class GenericFunctionDiagnosticTask : public DiagnosticTask
{
public:
  GenericFunctionDiagnosticTask(
    const std::string & name,
    std::function<void(T &)> fn)
  : DiagnosticTask(name), fn_(fn) {}
 
  virtual void run(DiagnosticStatusWrapper & stat) {fn_(stat);}
 
private:
  const std::string name_;
  const TaskFunction fn_;
};
 
typedef GenericFunctionDiagnosticTask<diagnostic_msgs::msg::DiagnosticStatus>
  UnwrappedFunctionDiagnosticTask;
typedef GenericFunctionDiagnosticTask<DiagnosticStatusWrapper>
  FunctionDiagnosticTask;
 
class CompositeDiagnosticTask : public DiagnosticTask
{
public:
  explicit CompositeDiagnosticTask(const std::string name)
  : DiagnosticTask(name) {}
 
  virtual void run(DiagnosticStatusWrapper & stat)
  {
    DiagnosticStatusWrapper combined_summary;
    DiagnosticStatusWrapper original_summary;
 
    original_summary.summary(stat);
 
    for (std::vector<DiagnosticTask *>::iterator i = tasks_.begin();
      i != tasks_.end(); i++)
    {
      // Put the summary that was passed in.
      stat.summary(original_summary);
      // Let the next task add entries and put its summary.
      (*i)->run(stat);
      // Merge the new summary into the combined summary.
      combined_summary.mergeSummary(stat);
    }
 
    // Copy the combined summary into the output.
    stat.summary(combined_summary);
  }
 
  void addTask(DiagnosticTask * t) {tasks_.push_back(t);}
 
private:
  std::vector<DiagnosticTask *> tasks_;
};
 
class DiagnosticTaskVector
{
protected:
  class DiagnosticTaskInternal
  {
public:
    DiagnosticTaskInternal(const std::string name, TaskFunction f)
    : name_(name), fn_(f) {}
 
    void run(diagnostic_updater::DiagnosticStatusWrapper & stat) const
    {
      stat.name = name_;
      fn_(stat);
    }
 
    const std::string & getName() const {return name_;}
 
private:
    std::string name_;
    TaskFunction fn_;
  };
 
  std::mutex lock_;
 
  const std::vector<DiagnosticTaskInternal> & getTasks() {return tasks_;}
 
public:
  virtual ~DiagnosticTaskVector() {}
 
  void add(const std::string & name, TaskFunction f)
  {
    DiagnosticTaskInternal int_task(name, f);
    addInternal(int_task);
  }
 
  void add(DiagnosticTask & task)
  {
    TaskFunction f = std::bind(&DiagnosticTask::run, &task, std::placeholders::_1);
    add(task.getName(), f);
  }
 
  template<class T>
  void add(
    const std::string name, T * c,
    void (T::* f)(diagnostic_updater::DiagnosticStatusWrapper &))
  {
    DiagnosticTaskInternal int_task(name, std::bind(f, c, std::placeholders::_1));
    addInternal(int_task);
  }
 
  bool removeByName(const std::string name)
  {
    std::unique_lock<std::mutex> lock(lock_);
    for (std::vector<DiagnosticTaskInternal>::iterator iter = tasks_.begin();
      iter != tasks_.end(); iter++)
    {
      if (iter->getName() == name) {
        tasks_.erase(iter);
        return true;
      }
 
      diagnostic_updater::DiagnosticStatusWrapper status;
    }
    return false;
  }
 
private:
  virtual void addedTaskCallback(DiagnosticTaskInternal &) {}
  std::vector<DiagnosticTaskInternal> tasks_;
 
protected:
  void addInternal(DiagnosticTaskInternal & task)
  {
    std::unique_lock<std::mutex> lock(lock_);
    tasks_.push_back(task);
    addedTaskCallback(task);
  }
};
 
class Updater : public DiagnosticTaskVector
{
public:
  bool verbose_;
 
  template<class NodeT>
  explicit Updater(NodeT node, double period = 1.0)
  : Updater(
      node->get_node_base_interface(),
      node->get_node_logging_interface(),
      node->get_node_parameters_interface(),
      node->get_node_timers_interface(),
      node->get_node_topics_interface(),
      period)
  {}
 
  Updater(
    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> base_interface,
    std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> logging_interface,
    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
    std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> timers_interface,
    std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> topics_interface,
    double period = 1.0)
  : verbose_(false),
    base_interface_(base_interface),
    timers_interface_(timers_interface),
    clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME)),
    period_(rclcpp::Duration::from_nanoseconds(period * 1e9)),
    publisher_(
      rclcpp::create_publisher<diagnostic_msgs::msg::DiagnosticArray>(
        topics_interface, "/diagnostics", 1)),
    logger_(logging_interface->get_logger()),
    node_name_(base_interface->get_name()),
    warn_nohwid_done_(false)
  {
    period = parameters_interface->declare_parameter(
      "diagnostic_updater.period", rclcpp::ParameterValue(period)).get<double>();
    period_ = rclcpp::Duration::from_nanoseconds(period * 1e9);
 
    reset_timer();
  }
 
  auto getPeriod() const {return period_;}
 
  void setPeriod(rclcpp::Duration period)
  {
    period_ = period;
    reset_timer();
  }
 
  void setPeriod(double period)
  {
    setPeriod(rclcpp::Duration::from_nanoseconds(period * 1e9));
  }
 
  void force_update()
  {
    update();
  }
 
  void broadcast(int lvl, const std::string msg)
  {
    std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
 
    const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
    for (std::vector<DiagnosticTaskInternal>::const_iterator iter =
      tasks.begin();
      iter != tasks.end(); iter++)
    {
      diagnostic_updater::DiagnosticStatusWrapper status;
 
      status.name = iter->getName();
      status.summary(lvl, msg);
 
      status_vec.push_back(status);
    }
 
    publish(status_vec);
  }
 
  void setHardwareIDf(const char * format, ...)
  {
    va_list va;
    const int kBufferSize = 1000;
    char buff[kBufferSize];  // @todo This could be done more elegantly.
    va_start(va, format);
    if (vsnprintf(buff, kBufferSize, format, va) >= kBufferSize) {
      RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf.");
    }
    hwid_ = std::string(buff);
    va_end(va);
  }
 
  void setHardwareID(const std::string & hwid) {hwid_ = hwid;}
 
private:
  void reset_timer()
  {
    update_timer_ = rclcpp::create_timer(
      base_interface_,
      timers_interface_,
      clock_,
      period_,
      std::bind(&Updater::update, this));
  }
 
  void update()
  {
    if (rclcpp::ok()) {
      bool warn_nohwid = hwid_.empty();
 
      std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
 
      std::unique_lock<std::mutex> lock(
        lock_);    // Make sure no adds happen while we are processing here.
      const std::vector<DiagnosticTaskInternal> & tasks = getTasks();
      for (std::vector<DiagnosticTaskInternal>::const_iterator iter =
        tasks.begin();
        iter != tasks.end(); iter++)
      {
        diagnostic_updater::DiagnosticStatusWrapper status;
 
        status.name = iter->getName();
        status.level = 2;
        status.message = "No message was set";
        status.hardware_id = hwid_;
 
        iter->run(status);
 
        status_vec.push_back(status);
 
        if (status.level) {
          warn_nohwid = false;
        }
 
        if (verbose_ && status.level) {
          RCLCPP_WARN(
            logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'",
            status.name.c_str(), status.level, status.message.c_str());
        }
      }
 
      if (warn_nohwid && !warn_nohwid_done_) {
        std::string error_msg = "diagnostic_updater: No HW_ID was set.";
        error_msg += " This is probably a bug. Please report it.";
        error_msg += " For devices that do not have a HW_ID, set this value to 'none'.";
        error_msg += " This warning only occurs once all diagnostics are OK.";
        error_msg += " It is okay to wait until the device is open before calling setHardwareID.";
        RCLCPP_WARN(logger_, "%s", error_msg.c_str());
        warn_nohwid_done_ = true;
      }
 
      publish(status_vec);
    }
  }
 
  // TODO(Karsten1987) Follow up PR for eloquent
  // void update_diagnostic_period()
  // {
  //   // rcl_duration_value_t old_period = period_;
  //   // next_time_ = next_time_ +
  //   //   rclcpp::Duration(period_ - old_period);             // Update next_time_
  // }
 
  void publish(diagnostic_msgs::msg::DiagnosticStatus & stat)
  {
    std::vector<diagnostic_msgs::msg::DiagnosticStatus> status_vec;
    status_vec.push_back(stat);
    publish(status_vec);
  }
 
  void publish(std::vector<diagnostic_msgs::msg::DiagnosticStatus> & status_vec)
  {
    for (std::vector<diagnostic_msgs::msg::DiagnosticStatus>::iterator iter =
      status_vec.begin();
      iter != status_vec.end(); iter++)
    {
      iter->name = node_name_ + std::string(": ") + iter->name;
    }
    diagnostic_msgs::msg::DiagnosticArray msg;
    msg.status = status_vec;
    msg.header.stamp = rclcpp::Clock().now();
    publisher_->publish(msg);
  }
 
  virtual void addedTaskCallback(DiagnosticTaskInternal & task)
  {
    DiagnosticStatusWrapper stat;
    stat.name = task.getName();
    stat.summary(0, "Node starting up");
    publish(stat);
  }
 
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_;
  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_;
  rclcpp::Clock::SharedPtr clock_;
  rclcpp::Duration period_;
  rclcpp::TimerBase::SharedPtr update_timer_;
  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr publisher_;
  rclcpp::Logger logger_;
 
  std::string hwid_;
  std::string node_name_;
  bool warn_nohwid_done_;
};
}   // namespace diagnostic_updater
 
#endif  // DIAGNOSTIC_UPDATER__DIAGNOSTIC_UPDATER_HPP_