Program Listing for File diagnostic_updater.hpp

↰ Return to documentation for file (include/diagnostic_updater/diagnostic_updater.hpp)

/*********************************************************************
 * 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/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/node_interfaces/node_timers_interface.hpp"
#include "rclcpp/node_interfaces/node_topics_interface.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_clock_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::NodeClockInterface> clock_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);

  auto getPeriod() const {return period_;}

  void setPeriod(rclcpp::Duration period)
  {
    period_ = period;
    reset_timer();
  }

  void setPeriod(double period)
  {
    setPeriod(rclcpp::Duration::from_seconds(period));
  }

  void force_update()
  {
    update();
  }

  void broadcast(unsigned char lvl, const std::string msg);

  void setHardwareIDf(const char * format, ...);

  void setHardwareID(const std::string & hwid) {hwid_ = hwid;}

private:
  void reset_timer();

  void update();

  // 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);

  void publish(std::vector<diagnostic_msgs::msg::DiagnosticStatus> & status_vec);

  virtual void addedTaskCallback(DiagnosticTaskInternal & task);

  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_