diagnostic_updater.h

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 DIAGNOSTICUPDATER_HH
#define DIAGNOSTICUPDATER_HH

#include <stdexcept>
#include <vector>
#include <string>

#include "ros/node_handle.h"
#include "ros/this_node.h"

#include "diagnostic_msgs/DiagnosticArray.h"
#include "diagnostic_updater/DiagnosticStatusWrapper.h"

#include <boost/thread.hpp>

namespace diagnostic_updater
{

  typedef boost::function<void(DiagnosticStatusWrapper&)> TaskFunction;
  typedef boost::function<void(diagnostic_msgs::DiagnosticStatus&)> UnwrappedTaskFunction;

  class DiagnosticTask
  {
    public:
      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, boost::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::DiagnosticStatus> UnwrappedFunctionDiagnosticTask;
  typedef GenericFunctionDiagnosticTask<DiagnosticStatusWrapper> FunctionDiagnosticTask;

  class CompositeDiagnosticTask : public DiagnosticTask
  {
    public:
      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_;
      };

      boost::mutex lock_;

      const std::vector<DiagnosticTaskInternal> &getTasks()
      {
        return tasks_;
      }

    public:    
      void add(const std::string &name, TaskFunction f)
      {
        DiagnosticTaskInternal int_task(name, f);
        addInternal(int_task);
      }

      void add(DiagnosticTask &task)
      {
        TaskFunction f = boost::bind(&DiagnosticTask::run, &task, _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, boost::bind(f, c, _1));
          addInternal(int_task);
        }


         bool removeByName(const std::string name)
         {
           boost::mutex::scoped_lock 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)
      {
        boost::mutex::scoped_lock lock(lock_);
        tasks_.push_back(task); 
        addedTaskCallback(task);
      }
  };

  class Updater : public DiagnosticTaskVector
  {
    public:
      bool verbose_;

    Updater(ros::NodeHandle h = ros::NodeHandle(), ros::NodeHandle ph = ros::NodeHandle("~"), std::string node_name = ros::this_node::getName()) : private_node_handle_(ph), node_handle_(h), node_name_(node_name)
    {
      setup();
    }

      void update()
      {
        ros::Time now_time = ros::Time::now();
        if (now_time < next_time_) {
          // @todo put this back in after fix of #2157 update_diagnostic_period(); // Will be checked in force_update otherwise.
          return;
        }

        force_update();
      }

      void force_update()
      {
        update_diagnostic_period();

        next_time_ = ros::Time::now() + ros::Duration().fromSec(period_);

        if (node_handle_.ok())
        {
          bool warn_nohwid = hwid_.empty();
          
          std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;

          boost::mutex::scoped_lock 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)
              ROS_WARN("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_)
          {
            ROS_WARN("diagnostic_updater: No HW_ID was set. This is probably a bug. Please report it. For devices that do not have a HW_ID, set this value to 'none'. This warning only occurs once all diagnostics are OK so it is okay to wait until the device is open before calling setHardwareID.");
            warn_nohwid_done_ = true;
          }

          publish(status_vec);
        }
      }

      double getPeriod()
      {
        return period_;
      }

      // Destructor has trouble because the node is already shut down.
      /*~Updater()
        {
      // Create a new node handle and publisher because the existing one is 
      // probably shut down at this stage.

      ros::NodeHandle newnh; 
      node_handle_ = newnh; 
      publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
      broadcast(2, "Node shut down"); 
      }*/

      void broadcast(int lvl, const std::string msg)
      {
        std::vector<diagnostic_msgs::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;
        char buff[1000]; // @todo This could be done more elegantly.
        va_start(va, format);
        if (vsnprintf(buff, 1000, format, va) >= 1000)
          ROS_DEBUG("Really long string in diagnostic_updater::setHardwareIDf.");
        hwid_ = std::string(buff);
        va_end(va);
      }

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

    private:
      void update_diagnostic_period()
      {
        double old_period = period_;
        private_node_handle_.getParamCached("diagnostic_period", period_);
        next_time_ += ros::Duration(period_ - old_period); // Update next_time_
      }

      void publish(diagnostic_msgs::DiagnosticStatus &stat)
      {
        std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
        status_vec.push_back(stat);
        publish(status_vec);
      }

      void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
      {
        for  (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator 
            iter = status_vec.begin(); iter != status_vec.end(); iter++)
        {
          iter->name = 
            node_name_.substr(1) + std::string(": ") + iter->name;
        }
        diagnostic_msgs::DiagnosticArray msg;
        msg.status = status_vec;
        msg.header.stamp = ros::Time::now(); // Add timestamp for ROS 0.10
        publisher_.publish(msg);
      }

      void setup()
      {
        publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);

        period_ = 1.0;
        next_time_ = ros::Time::now() + ros::Duration(period_);
        update_diagnostic_period();

        verbose_ = false;
        warn_nohwid_done_ = false;
      }

      virtual void addedTaskCallback(DiagnosticTaskInternal &task)
      {
        DiagnosticStatusWrapper stat;
        stat.name = task.getName();
        stat.summary(0, "Node starting up");
        publish(stat);
      }

      ros::NodeHandle private_node_handle_;
      ros::NodeHandle node_handle_;
      ros::Publisher publisher_;

      ros::Time next_time_;

      double period_;
      std::string hwid_;
      std::string node_name_;
      bool warn_nohwid_done_;
  };

};

#endif