diagnostic_updater.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #ifndef DIAGNOSTICUPDATER_HH
36 #define DIAGNOSTICUPDATER_HH
37 
38 #include <stdexcept>
39 #include <vector>
40 #include <string>
41 
42 #include "ros/node_handle.h"
43 #include "ros/this_node.h"
44 
45 #include "diagnostic_msgs/DiagnosticArray.h"
47 
48 #include <boost/thread.hpp>
49 
51 {
52 
53  typedef boost::function<void(DiagnosticStatusWrapper&)> TaskFunction;
54  typedef boost::function<void(diagnostic_msgs::DiagnosticStatus&)> UnwrappedTaskFunction;
55 
66  {
67  public:
71  DiagnosticTask(const std::string name) : name_(name)
72  {}
73 
77  const std::string &getName()
78  {
79  return name_;
80  }
81 
85  virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
86 
90  virtual ~DiagnosticTask()
91  {}
92 
93  private:
94  const std::string name_;
95  };
96 
106  template <class T>
108  {
109  public:
118  GenericFunctionDiagnosticTask(const std::string &name, boost::function<void(T&)> fn) :
119  DiagnosticTask(name), fn_(fn)
120  {}
121 
122  virtual void run(DiagnosticStatusWrapper &stat)
123  {
124  fn_(stat);
125  }
126 
127  private:
128  const std::string name_;
129  const TaskFunction fn_;
130  };
131 
134 
147  {
148  public:
152  CompositeDiagnosticTask(const std::string name) : DiagnosticTask(name)
153  {}
154 
158  virtual void run(DiagnosticStatusWrapper &stat)
159  {
160  DiagnosticStatusWrapper combined_summary;
161  DiagnosticStatusWrapper original_summary;
162 
163  original_summary.summary(stat);
164 
165  for (std::vector<DiagnosticTask *>::iterator i = tasks_.begin();
166  i != tasks_.end(); i++)
167  {
168  // Put the summary that was passed in.
169  stat.summary(original_summary);
170  // Let the next task add entries and put its summary.
171  (*i)->run(stat);
172  // Merge the new summary into the combined summary.
173  combined_summary.mergeSummary(stat);
174  }
175 
176  // Copy the combined summary into the output.
177  stat.summary(combined_summary);
178  }
179 
187  {
188  tasks_.push_back(t);
189  }
190 
191  private:
192  std::vector<DiagnosticTask *> tasks_;
193  };
194 
204  {
205  protected:
211  {
212  public:
213  DiagnosticTaskInternal(const std::string name, TaskFunction f) :
214  name_(name), fn_(f)
215  {}
216 
218  {
219  stat.name = name_;
220  fn_(stat);
221  }
222 
223  const std::string &getName() const
224  {
225  return name_;
226  }
227 
228  private:
229  std::string name_;
230  TaskFunction fn_;
231  };
232 
233  boost::mutex lock_;
234 
238  const std::vector<DiagnosticTaskInternal> &getTasks()
239  {
240  return tasks_;
241  }
242 
243  public:
256  void add(const std::string &name, TaskFunction f)
257  {
258  DiagnosticTaskInternal int_task(name, f);
259  addInternal(int_task);
260  }
261 
270  void add(DiagnosticTask &task)
271  {
272  TaskFunction f = boost::bind(&DiagnosticTask::run, &task, _1);
273  add(task.getName(), f);
274  }
275 
289  template <class T>
290  void add(const std::string name, T *c, void (T::*f)(diagnostic_updater::DiagnosticStatusWrapper&))
291  {
292  DiagnosticTaskInternal int_task(name, boost::bind(f, c, _1));
293  addInternal(int_task);
294  }
295 
296 
308  bool removeByName(const std::string name)
309  {
310  boost::mutex::scoped_lock lock(lock_);
311  for (std::vector<DiagnosticTaskInternal>::iterator iter = tasks_.begin();
312  iter != tasks_.end(); iter++)
313  {
314  if (iter->getName() == name)
315  {
316  tasks_.erase(iter);
317  return true;
318  }
319 
321  }
322  return false;
323  }
324 
325  private:
332  {}
333  std::vector<DiagnosticTaskInternal> tasks_;
334 
335  protected:
340  {
341  boost::mutex::scoped_lock lock(lock_);
342  tasks_.push_back(task);
343  addedTaskCallback(task);
344  }
345  };
346 
363  {
364  public:
365  bool verbose_;
366 
373  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)
374  {
375  setup();
376  }
377 
382  void update()
383  {
384  ros::Time now_time = ros::Time::now();
385  if (now_time < next_time_) {
386  // @todo put this back in after fix of #2157 update_diagnostic_period(); // Will be checked in force_update otherwise.
387  return;
388  }
389 
390  force_update();
391  }
392 
400  {
401  update_diagnostic_period();
402 
403  next_time_ = ros::Time::now() + ros::Duration().fromSec(period_);
404 
405  if (node_handle_.ok())
406  {
407  bool warn_nohwid = hwid_.empty();
408 
409  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
410 
411  boost::mutex::scoped_lock lock(lock_); // Make sure no adds happen while we are processing here.
412  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
413  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
414  iter != tasks.end(); iter++)
415  {
417 
418  status.name = iter->getName();
419  status.level = 2;
420  status.message = "No message was set";
421  status.hardware_id = hwid_;
422 
423  iter->run(status);
424 
425  status_vec.push_back(status);
426 
427  if (status.level)
428  warn_nohwid = false;
429 
430  if (verbose_ && status.level)
431  ROS_WARN("Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
432  }
433 
434  if (warn_nohwid && !warn_nohwid_done_)
435  {
436  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.");
437  warn_nohwid_done_ = true;
438  }
439 
440  publish(status_vec);
441  }
442  }
443 
448  double getPeriod()
449  {
450  return period_;
451  }
452 
453  // Destructor has trouble because the node is already shut down.
454  /*~Updater()
455  {
456  // Create a new node handle and publisher because the existing one is
457  // probably shut down at this stage.
458 
459  ros::NodeHandle newnh;
460  node_handle_ = newnh;
461  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
462  broadcast(2, "Node shut down");
463  }*/
464 
476  void broadcast(int lvl, const std::string msg)
477  {
478  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
479 
480  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
481  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
482  iter != tasks.end(); iter++)
483  {
485 
486  status.name = iter->getName();
487  status.summary(lvl, msg);
488 
489  status_vec.push_back(status);
490  }
491 
492  publish(status_vec);
493  }
494 
495  void setHardwareIDf(const char *format, ...)
496  {
497  va_list va;
498  char buff[1000]; // @todo This could be done more elegantly.
499  va_start(va, format);
500  if (vsnprintf(buff, 1000, format, va) >= 1000)
501  ROS_DEBUG("Really long string in diagnostic_updater::setHardwareIDf.");
502  hwid_ = std::string(buff);
503  va_end(va);
504  }
505 
506  void setHardwareID(const std::string &hwid)
507  {
508  hwid_ = hwid;
509  }
510 
511  private:
517  {
518  double old_period = period_;
519  private_node_handle_.getParamCached("diagnostic_period", period_);
520  next_time_ += ros::Duration(period_ - old_period); // Update next_time_
521  }
522 
526  void publish(diagnostic_msgs::DiagnosticStatus &stat)
527  {
528  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
529  status_vec.push_back(stat);
530  publish(status_vec);
531  }
532 
536  void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
537  {
538  for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator
539  iter = status_vec.begin(); iter != status_vec.end(); iter++)
540  {
541  iter->name =
542  node_name_.substr(1) + std::string(": ") + iter->name;
543  }
544  diagnostic_msgs::DiagnosticArray msg;
545  msg.status = status_vec;
546  msg.header.stamp = ros::Time::now(); // Add timestamp for ROS 0.10
547  publisher_.publish(msg);
548  }
549 
553  void setup()
554  {
555  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
556 
557  period_ = 1.0;
558  next_time_ = ros::Time::now() + ros::Duration(period_);
559  update_diagnostic_period();
560 
561  verbose_ = false;
562  warn_nohwid_done_ = false;
563  }
564 
570  {
572  stat.name = task.getName();
573  stat.summary(0, "Node starting up");
574  publish(stat);
575  }
576 
580 
582 
583  double period_;
584  std::string hwid_;
585  std::string node_name_;
587  };
588 
589 };
590 
591 #endif
void addInternal(DiagnosticTaskInternal &task)
Updater(ros::NodeHandle h=ros::NodeHandle(), ros::NodeHandle ph=ros::NodeHandle("~"), std::string node_name=ros::this_node::getName())
Constructs an updater class.
std::vector< DiagnosticTaskInternal > tasks_
f
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
void setHardwareID(const std::string &hwid)
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)=0
Fills out this Task&#39;s DiagnosticStatusWrapper.
void add(const std::string &name, TaskFunction f)
Add a DiagnosticTask embodied by a name and function to the DiagnosticTaskVector. ...
ROSCPP_DECL const std::string & getName()
Author: Blaise Gassend.
boost::function< void(diagnostic_msgs::DiagnosticStatus &)> UnwrappedTaskFunction
#define ROS_WARN(...)
Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
virtual void addedTaskCallback(DiagnosticTaskInternal &task)
void add(const std::string name, T *c, void(T::*f)(diagnostic_updater::DiagnosticStatusWrapper &))
Add a DiagnosticTask embodied by a name and method to the DiagnosticTaskVector.
std::vector< DiagnosticTask * > tasks_
double getPeriod()
Returns the interval between updates.
virtual void addedTaskCallback(DiagnosticTaskInternal &)
void add(DiagnosticTask &task)
Add a DiagnosticTask to the DiagnosticTaskVector.
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) const
Duration & fromSec(double t)
void update()
Causes the diagnostics to update if the inter-update interval has been exceeded.
GenericFunctionDiagnosticTask< diagnostic_msgs::DiagnosticStatus > UnwrappedFunctionDiagnosticTask
bool removeByName(const std::string name)
Remove a task based on its name.
GenericFunctionDiagnosticTask(const std::string &name, boost::function< void(T &)> fn)
a DiagnosticTask based on a boost::function.
void mergeSummary(unsigned char lvl, const std::string s)
Merges a level and message with the existing ones.
const std::string & getName()
Returns the name of the DiagnosticTask.
DiagnosticTask(const std::string name)
Constructs a DiagnosticTask setting its name in the process.
void force_update()
Forces the diagnostics to update.
Class used to represent a diagnostic task internally in DiagnosticTaskVector.
boost::function< void(DiagnosticStatusWrapper &)> TaskFunction
CompositeDiagnosticTask(const std::string name)
Constructs a CompositeDiagnosticTask with the given name.
static Time now()
virtual void run(DiagnosticStatusWrapper &stat)
Fills out this Task&#39;s DiagnosticStatusWrapper.
DiagnosticTask is an abstract base class for collecting diagnostic data.
virtual void run(DiagnosticStatusWrapper &stat)
Runs each child and merges their outputs.
void broadcast(int lvl, const std::string msg)
Output a message on all the known DiagnosticStatus.
const std::vector< DiagnosticTaskInternal > & getTasks()
Returns the vector of tasks.
void setHardwareIDf(const char *format,...)
void publish(std::vector< diagnostic_msgs::DiagnosticStatus > &status_vec)
GenericFunctionDiagnosticTask< DiagnosticStatusWrapper > FunctionDiagnosticTask
void publish(diagnostic_msgs::DiagnosticStatus &stat)
Merges CompositeDiagnosticTask into a single DiagnosticTask.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...
void addTask(DiagnosticTask *t)
Adds a child CompositeDiagnosticTask.
#define ROS_DEBUG(...)


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Wed Mar 27 2019 03:02:22