diagnostic_updater.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef DIAGNOSTICUPDATER_HH
00036 #define DIAGNOSTICUPDATER_HH
00037 
00038 #include <stdexcept>
00039 #include <vector>
00040 #include <string>
00041 
00042 #include "ros/node_handle.h"
00043 #include "ros/this_node.h"
00044 
00045 #include "diagnostic_msgs/DiagnosticArray.h"
00046 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00047 
00048 #include <boost/thread.hpp>
00049 
00050 namespace diagnostic_updater
00051 {
00052 
00053   typedef boost::function<void(DiagnosticStatusWrapper&)> TaskFunction;
00054   typedef boost::function<void(diagnostic_msgs::DiagnosticStatus&)> UnwrappedTaskFunction;
00055 
00065   class DiagnosticTask
00066   {
00067     public:
00071       DiagnosticTask(const std::string name) : name_(name)
00072       {}
00073 
00077       const std::string &getName()
00078       {
00079         return name_;
00080       }
00081 
00085       virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
00086 
00090       virtual ~DiagnosticTask()
00091       {}
00092 
00093     private:
00094       const std::string name_;
00095   };
00096 
00106   template <class T>
00107   class GenericFunctionDiagnosticTask : public DiagnosticTask
00108   {
00109     public:
00118       GenericFunctionDiagnosticTask(const std::string &name, boost::function<void(T&)> fn) : 
00119         DiagnosticTask(name), fn_(fn)
00120       {}
00121 
00122       virtual void run(DiagnosticStatusWrapper &stat)
00123       {
00124         fn_(stat);
00125       }
00126 
00127     private:
00128       const std::string name_;
00129       const TaskFunction fn_;
00130   };
00131 
00132   typedef GenericFunctionDiagnosticTask<diagnostic_msgs::DiagnosticStatus> UnwrappedFunctionDiagnosticTask;
00133   typedef GenericFunctionDiagnosticTask<DiagnosticStatusWrapper> FunctionDiagnosticTask;
00134 
00146   class CompositeDiagnosticTask : public DiagnosticTask
00147   {
00148     public:
00152       CompositeDiagnosticTask(const std::string name) : DiagnosticTask(name)
00153       {}
00154 
00158       virtual void run(DiagnosticStatusWrapper &stat)
00159       {
00160         DiagnosticStatusWrapper combined_summary;
00161         DiagnosticStatusWrapper original_summary;
00162 
00163         original_summary.summary(stat);
00164 
00165         for (std::vector<DiagnosticTask *>::iterator i = tasks_.begin();
00166             i != tasks_.end(); i++)
00167         {
00168           // Put the summary that was passed in.
00169           stat.summary(original_summary);
00170           // Let the next task add entries and put its summary.
00171           (*i)->run(stat); 
00172           // Merge the new summary into the combined summary.
00173           combined_summary.mergeSummary(stat);
00174         }
00175 
00176         // Copy the combined summary into the output.
00177         stat.summary(combined_summary);
00178       }
00179 
00186       void addTask(DiagnosticTask *t)
00187       {
00188         tasks_.push_back(t);
00189       }
00190 
00191     private:
00192       std::vector<DiagnosticTask *> tasks_;
00193   };
00194 
00203   class DiagnosticTaskVector
00204   {
00205     protected:
00210       class DiagnosticTaskInternal
00211       {
00212         public:
00213           DiagnosticTaskInternal(const std::string name, TaskFunction f) :
00214             name_(name), fn_(f)
00215         {}
00216 
00217           void run(diagnostic_updater::DiagnosticStatusWrapper &stat) const
00218           {
00219             stat.name = name_;
00220             fn_(stat);
00221           }
00222 
00223           const std::string &getName() const
00224           {
00225             return name_;
00226           }
00227 
00228         private:
00229           std::string name_;
00230           TaskFunction fn_;
00231       };
00232 
00233       boost::mutex lock_;
00234 
00238       const std::vector<DiagnosticTaskInternal> &getTasks()
00239       {
00240         return tasks_;
00241       }
00242 
00243     public:    
00256       void add(const std::string &name, TaskFunction f)
00257       {
00258         DiagnosticTaskInternal int_task(name, f);
00259         addInternal(int_task);
00260       }
00261 
00270       void add(DiagnosticTask &task)
00271       {
00272         TaskFunction f = boost::bind(&DiagnosticTask::run, &task, _1);
00273         add(task.getName(), f);
00274       }
00275 
00289       template <class T>
00290         void add(const std::string name, T *c, void (T::*f)(diagnostic_updater::DiagnosticStatusWrapper&))
00291         {
00292           DiagnosticTaskInternal int_task(name, boost::bind(f, c, _1));
00293           addInternal(int_task);
00294         }
00295 
00296 
00308          bool removeByName(const std::string name)
00309          {
00310            boost::mutex::scoped_lock lock(lock_); 
00311            for (std::vector<DiagnosticTaskInternal>::iterator iter = tasks_.begin();
00312                iter != tasks_.end(); iter++)
00313            {
00314              if (iter->getName() == name)
00315              {
00316                tasks_.erase(iter);
00317                return true;
00318              }
00319 
00320              diagnostic_updater::DiagnosticStatusWrapper status;
00321            }
00322            return false;
00323          }
00324 
00325     private:
00331       virtual void addedTaskCallback(DiagnosticTaskInternal &)
00332       {}
00333       std::vector<DiagnosticTaskInternal> tasks_;
00334 
00335     protected:
00339       void addInternal(DiagnosticTaskInternal &task)
00340       {
00341         boost::mutex::scoped_lock lock(lock_);
00342         tasks_.push_back(task); 
00343         addedTaskCallback(task);
00344       }
00345   };
00346 
00362   class Updater : public DiagnosticTaskVector
00363   {
00364     public:
00365       bool verbose_;
00366 
00373     Updater()
00374     {
00375       setup();
00376     }
00377 
00382       void update()
00383       {
00384         ros::Time now_time = ros::Time::now();
00385         if (now_time < next_time_) {
00386           // @todo put this back in after fix of #2157 update_diagnostic_period(); // Will be checked in force_update otherwise.
00387           return;
00388         }
00389 
00390         force_update();
00391       }
00392 
00399       void force_update()
00400       {
00401         update_diagnostic_period();
00402 
00403         next_time_ = ros::Time::now() + ros::Duration().fromSec(period_);
00404 
00405         if (node_handle_.ok())
00406         {
00407           bool warn_nohwid = hwid_.empty();
00408           
00409           std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
00410 
00411           boost::mutex::scoped_lock lock(lock_); // Make sure no adds happen while we are processing here.
00412           const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
00413           for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
00414               iter != tasks.end(); iter++)
00415           {
00416             diagnostic_updater::DiagnosticStatusWrapper status;
00417 
00418             status.name = iter->getName();
00419             status.level = 2;
00420             status.message = "No message was set";
00421             status.hardware_id = hwid_;
00422 
00423             iter->run(status);
00424 
00425             status_vec.push_back(status);
00426                                          
00427             if (status.level)
00428               warn_nohwid = false;
00429 
00430             if (verbose_ && status.level)
00431               ROS_WARN("Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
00432           }
00433 
00434           if (warn_nohwid && !warn_nohwid_done_)
00435           {
00436             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.");
00437             warn_nohwid_done_ = true;
00438           }
00439 
00440           publish(status_vec);
00441         }
00442       }
00443 
00448       double getPeriod()
00449       {
00450         return period_;
00451       }
00452 
00453       // Destructor has trouble because the node is already shut down.
00454       /*~Updater()
00455         {
00456       // Create a new node handle and publisher because the existing one is 
00457       // probably shut down at this stage.
00458 
00459       ros::NodeHandle newnh; 
00460       node_handle_ = newnh; 
00461       publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00462       broadcast(2, "Node shut down"); 
00463       }*/
00464 
00476       void broadcast(int lvl, const std::string msg)
00477       {
00478         std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
00479 
00480         const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
00481         for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
00482             iter != tasks.end(); iter++)
00483         {
00484           diagnostic_updater::DiagnosticStatusWrapper status;
00485 
00486           status.name = iter->getName();
00487           status.summary(lvl, msg);
00488 
00489           status_vec.push_back(status);
00490         }
00491 
00492         publish(status_vec);
00493       }
00494 
00495       void setHardwareIDf(const char *format, ...)
00496       {
00497         va_list va;
00498         char buff[1000]; // @todo This could be done more elegantly.
00499         va_start(va, format);
00500         if (vsnprintf(buff, 1000, format, va) >= 1000)
00501           ROS_DEBUG("Really long string in diagnostic_updater::setHardwareIDf.");
00502         hwid_ = std::string(buff);
00503         va_end(va);
00504       }
00505 
00506       void setHardwareID(const std::string &hwid)
00507       {
00508         hwid_ = hwid;
00509       }
00510 
00511     private:
00516       void update_diagnostic_period()
00517       {
00518         double old_period = period_;
00519         private_node_handle_.getParamCached("diagnostic_period", period_);
00520         next_time_ += ros::Duration(period_ - old_period); // Update next_time_
00521       }
00522 
00526       void publish(diagnostic_msgs::DiagnosticStatus &stat)
00527       {
00528         std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
00529         status_vec.push_back(stat);
00530         publish(status_vec);
00531       }
00532 
00536       void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
00537       {
00538         for  (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator 
00539             iter = status_vec.begin(); iter != status_vec.end(); iter++)
00540         {
00541           iter->name = 
00542             ros::this_node::getName().substr(1) + std::string(": ") + iter->name;
00543         }
00544         diagnostic_msgs::DiagnosticArray msg;
00545         msg.status = status_vec;
00546         msg.header.stamp = ros::Time::now(); // Add timestamp for ROS 0.10
00547         publisher_.publish(msg);
00548       }
00549 
00553       void setup()
00554       {
00555         publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00556         private_node_handle_ = ros::NodeHandle("~");
00557 
00558         period_ = 1.0;
00559         update_diagnostic_period();
00560         next_time_ = ros::Time::now();
00561 
00562         verbose_ = false;
00563         warn_nohwid_done_ = false;
00564       }
00565 
00570       virtual void addedTaskCallback(DiagnosticTaskInternal &task)
00571       {
00572         DiagnosticStatusWrapper stat;
00573         stat.name = task.getName();
00574         stat.summary(0, "Node starting up");
00575         publish(stat);
00576       }
00577 
00578       ros::NodeHandle private_node_handle_;
00579       ros::NodeHandle node_handle_;
00580       ros::Publisher publisher_;
00581 
00582       ros::Time next_time_;
00583 
00584       double period_;
00585       std::string hwid_;
00586       bool warn_nohwid_done_;
00587   };
00588 
00589 };
00590 
00591 #endif


diagnostic_updater
Author(s): Jeremy Leibs, Blaise Gassend, Brice Rebsamen
autogenerated on Fri Jan 3 2014 11:18:57