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"
46 #include "diagnostic_msgs/DiagnosticStatus.h"
48 
49 #include <boost/thread.hpp>
50 
52 {
53 
54  typedef boost::function<void(DiagnosticStatusWrapper&)> TaskFunction;
55  typedef boost::function<void(diagnostic_msgs::DiagnosticStatus&)> UnwrappedTaskFunction;
56 
66  class DiagnosticTask
67  {
68  public:
72  DiagnosticTask(const std::string name) : name_(name)
73  {}
74 
78  const std::string &getName()
79  {
80  return name_;
81  }
82 
87 
91  virtual ~DiagnosticTask()
92  {}
93 
94  private:
95  const std::string name_;
96  };
97 
107  template <class T>
108  class GenericFunctionDiagnosticTask : public DiagnosticTask
109  {
110  public:
119  GenericFunctionDiagnosticTask(const std::string &name, boost::function<void(T&)> fn) :
120  DiagnosticTask(name), fn_(fn)
121  {}
122 
123  virtual void run(DiagnosticStatusWrapper &stat)
124  {
125  fn_(stat);
126  }
127 
128  private:
129  const std::string name_;
130  const TaskFunction fn_;
131  };
132 
135 
148  {
149  public:
153  CompositeDiagnosticTask(const std::string name) : DiagnosticTask(name)
154  {}
155 
159  virtual void run(DiagnosticStatusWrapper &stat)
160  {
161  DiagnosticStatusWrapper combined_summary;
162  DiagnosticStatusWrapper original_summary;
163 
164  original_summary.summary(stat);
165 
166  for (std::vector<DiagnosticTask *>::iterator i = tasks_.begin();
167  i != tasks_.end(); i++)
168  {
169  // Put the summary that was passed in.
170  stat.summary(original_summary);
171  // Let the next task add entries and put its summary.
172  (*i)->run(stat);
173  // Merge the new summary into the combined summary.
174  combined_summary.mergeSummary(stat);
175  }
176 
177  // Copy the combined summary into the output.
178  stat.summary(combined_summary);
179  }
180 
187  void addTask(DiagnosticTask *t)
188  {
189  tasks_.push_back(t);
190  }
191 
192  private:
193  std::vector<DiagnosticTask *> tasks_;
194  };
195 
205  {
206  protected:
211  class DiagnosticTaskInternal
212  {
213  public:
214  DiagnosticTaskInternal(const std::string name, TaskFunction f) :
215  name_(name), fn_(f)
216  {}
217 
219  {
220  stat.name = name_;
221  fn_(stat);
222  }
223 
224  const std::string &getName() const
225  {
226  return name_;
227  }
228 
229  private:
230  std::string name_;
232  };
233 
234  boost::mutex lock_;
235 
239  const std::vector<DiagnosticTaskInternal> &getTasks()
240  {
241  return tasks_;
242  }
243 
244  public:
257  void add(const std::string &name, TaskFunction f)
258  {
259  DiagnosticTaskInternal int_task(name, f);
260  addInternal(int_task);
261  }
262 
271  void add(DiagnosticTask &task)
272  {
273  TaskFunction f = boost::bind(&DiagnosticTask::run, &task, boost::placeholders::_1);
274  add(task.getName(), f);
275  }
276 
290  template <class T>
291  void add(const std::string name, T *c, void (T::*f)(diagnostic_updater::DiagnosticStatusWrapper&))
292  {
293  DiagnosticTaskInternal int_task(name, boost::bind(f, c, boost::placeholders::_1));
294  addInternal(int_task);
295  }
296 
297 
309  bool removeByName(const std::string name)
310  {
311  boost::mutex::scoped_lock lock(lock_);
312  for (std::vector<DiagnosticTaskInternal>::iterator iter = tasks_.begin();
313  iter != tasks_.end(); iter++)
314  {
315  if (iter->getName() == name)
316  {
317  tasks_.erase(iter);
318  return true;
319  }
320 
322  }
323  return false;
324  }
325 
326  private:
332  virtual void addedTaskCallback(DiagnosticTaskInternal &)
333  {}
334  std::vector<DiagnosticTaskInternal> tasks_;
335 
336  protected:
340  void addInternal(DiagnosticTaskInternal &task)
341  {
342  boost::mutex::scoped_lock lock(lock_);
343  tasks_.push_back(task);
344  addedTaskCallback(task);
345  }
346  };
347 
363  class Updater : public DiagnosticTaskVector
364  {
365  public:
366  bool verbose_;
367 
377  {
378  setup();
379  }
380 
385  void update()
386  {
387  ros::Time now_time = ros::Time::now();
388  if (now_time < next_time_) {
389  // @todo put this back in after fix of #2157 update_diagnostic_period(); // Will be checked in force_update otherwise.
390  return;
391  }
392 
393  force_update();
394  }
395 
402  void force_update()
403  {
405 
407 
409  {
410  bool warn_nohwid = hwid_.empty();
411 
412  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
413 
414  boost::mutex::scoped_lock lock(lock_); // Make sure no adds happen while we are processing here.
415  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
416  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
417  iter != tasks.end(); iter++)
418  {
420 
421  status.name = iter->getName();
422  status.level = 2;
423  status.message = "No message was set";
424  status.hardware_id = hwid_;
425 
426  iter->run(status);
427 
428  status_vec.push_back(status);
429 
430  if (status.level)
431  warn_nohwid = false;
432 
433  if (verbose_ && status.level)
434  ROS_WARN("Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
435  }
436 
437  if (warn_nohwid && !warn_nohwid_done_)
438  {
439  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.");
440  warn_nohwid_done_ = true;
441  }
442 
443  publish(status_vec);
444  }
445  }
446 
451  double getPeriod()
452  {
453  return period_;
454  }
455 
456  // Destructor has trouble because the node is already shut down.
457  /*~Updater()
458  {
459  // Create a new node handle and publisher because the existing one is
460  // probably shut down at this stage.
461 
462  ros::NodeHandle newnh;
463  node_handle_ = newnh;
464  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
465  broadcast(2, "Node shut down");
466  }*/
467 
479  void broadcast(int lvl, const std::string msg)
480  {
481  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
482 
483  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
484  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
485  iter != tasks.end(); iter++)
486  {
488 
489  status.name = iter->getName();
490  status.summary(lvl, msg);
491 
492  status_vec.push_back(status);
493  }
494 
495  publish(status_vec);
496  }
497 
498  void setHardwareIDf(const char *format, ...)
499  {
500  va_list va;
501  char buff[1000]; // @todo This could be done more elegantly.
502  va_start(va, format);
503  if (vsnprintf(buff, 1000, format, va) >= 1000)
504  ROS_DEBUG("Really long string in diagnostic_updater::setHardwareIDf.");
505  hwid_ = std::string(buff);
506  va_end(va);
507  }
508 
509  void setHardwareID(const std::string &hwid)
510  {
511  hwid_ = hwid;
512  }
513 
514  private:
520  {
521  double old_period = period_;
522  private_node_handle_.getParamCached("diagnostic_period", period_);
523  next_time_ += ros::Duration(period_ - old_period); // Update next_time_
524  }
525 
529  void publish(diagnostic_msgs::DiagnosticStatus &stat)
530  {
531  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
532  status_vec.push_back(stat);
533  publish(status_vec);
534  }
535 
539  void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
540  {
541  for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator
542  iter = status_vec.begin(); iter != status_vec.end(); iter++)
543  {
544  iter->name =
545  node_name_.substr(1) + std::string(": ") + iter->name;
546  }
547  diagnostic_msgs::DiagnosticArray msg;
548  msg.status = status_vec;
549  msg.header.stamp = ros::Time::now(); // Add timestamp for ROS 0.10
551  }
552 
556  void setup()
557  {
558  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
559 
560  period_ = 1.0;
563 
564  verbose_ = false;
565  warn_nohwid_done_ = false;
566  }
567 
572  virtual void addedTaskCallback(DiagnosticTaskInternal &task)
573  {
575  stat.name = task.getName();
576  stat.summary(0, "Node starting up");
577  publish(stat);
578  }
579 
583 
585 
586  double period_;
587  std::string hwid_;
588  std::string node_name_;
589  bool warn_nohwid_done_;
590  };
591 
592 }
593 
594 #endif
diagnostic_updater::CompositeDiagnosticTask::addTask
void addTask(DiagnosticTask *t)
Adds a child CompositeDiagnosticTask.
Definition: diagnostic_updater.h:219
diagnostic_updater::Updater::addedTaskCallback
virtual void addedTaskCallback(DiagnosticTaskInternal &task)
Definition: diagnostic_updater.h:604
node_handle.h
diagnostic_updater::DiagnosticTask::name_
const std::string name_
Definition: diagnostic_updater.h:127
diagnostic_updater::CompositeDiagnosticTask::tasks_
std::vector< DiagnosticTask * > tasks_
Definition: diagnostic_updater.h:225
DiagnosticStatusWrapper.h
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal::name_
std::string name_
Definition: diagnostic_updater.h:262
example.msg
msg
Definition: example.py:237
ros::Publisher
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
diagnostic_updater::DiagnosticTaskVector::lock_
boost::mutex lock_
Definition: diagnostic_updater.h:266
diagnostic_updater::Updater::setHardwareIDf
void setHardwareIDf(const char *format,...)
Definition: diagnostic_updater.h:530
diagnostic_updater::Updater::force_update
void force_update()
Forces the diagnostics to update.
Definition: diagnostic_updater.h:434
diagnostic_updater::DiagnosticTask::DiagnosticTask
DiagnosticTask(const std::string name)
Constructs a DiagnosticTask setting its name in the process.
Definition: diagnostic_updater.h:104
diagnostic_updater::UnwrappedTaskFunction
boost::function< void(diagnostic_msgs::DiagnosticStatus &)> UnwrappedTaskFunction
Definition: diagnostic_updater.h:87
diagnostic_updater::DiagnosticStatusWrapper::mergeSummary
void mergeSummary(unsigned char lvl, const std::string s)
Merges a level and message with the existing ones.
Definition: DiagnosticStatusWrapper.h:165
diagnostic_updater::Updater::node_handle_
ros::NodeHandle node_handle_
Definition: diagnostic_updater.h:613
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal::getName
const std::string & getName() const
Definition: diagnostic_updater.h:256
diagnostic_updater::Updater::node_name_
std::string node_name_
Definition: diagnostic_updater.h:620
diagnostic_updater::DiagnosticTaskVector
Internal use only.
Definition: diagnostic_updater.h:236
diagnostic_updater::GenericFunctionDiagnosticTask::fn_
const TaskFunction fn_
Definition: diagnostic_updater.h:162
diagnostic_updater::DiagnosticTask::~DiagnosticTask
virtual ~DiagnosticTask()
Definition: diagnostic_updater.h:123
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
diagnostic_updater::Updater::broadcast
void broadcast(int lvl, const std::string msg)
Output a message on all the known DiagnosticStatus.
Definition: diagnostic_updater.h:511
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
diagnostic_updater::Updater::private_node_handle_
ros::NodeHandle private_node_handle_
Definition: diagnostic_updater.h:612
f
f
diagnostic_updater::GenericFunctionDiagnosticTask::GenericFunctionDiagnosticTask
GenericFunctionDiagnosticTask(const std::string &name, boost::function< void(T &)> fn)
Definition: diagnostic_updater.h:151
diagnostic_updater::DiagnosticTask
DiagnosticTask is an abstract base class for collecting diagnostic data.
Definition: diagnostic_updater.h:98
diagnostic_updater::Updater::verbose_
bool verbose_
Definition: diagnostic_updater.h:398
diagnostic_updater::UnwrappedFunctionDiagnosticTask
GenericFunctionDiagnosticTask< diagnostic_msgs::DiagnosticStatus > UnwrappedFunctionDiagnosticTask
Definition: diagnostic_updater.h:165
diagnostic_updater::DiagnosticTask::run
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)=0
Fills out this Task's DiagnosticStatusWrapper.
ROS_DEBUG
#define ROS_DEBUG(...)
diagnostic_updater::Updater::Updater
Updater(ros::NodeHandle h=ros::NodeHandle(), ros::NodeHandle ph=ros::NodeHandle("~"), std::string node_name=ros::this_node::getName())
Constructs an updater class.
Definition: diagnostic_updater.h:408
diagnostic_updater
Author: Blaise Gassend.
Definition: diagnostic_updater.h:51
diagnostic_updater::Updater::publisher_
ros::Publisher publisher_
Definition: diagnostic_updater.h:614
diagnostic_updater::Updater::period_
double period_
Definition: diagnostic_updater.h:618
ROS_WARN
#define ROS_WARN(...)
diagnostic_updater::Updater::hwid_
std::string hwid_
Definition: diagnostic_updater.h:619
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal::fn_
TaskFunction fn_
Definition: diagnostic_updater.h:263
diagnostic_updater::TaskFunction
boost::function< void(DiagnosticStatusWrapper &)> TaskFunction
Definition: diagnostic_updater.h:86
diagnostic_updater::CompositeDiagnosticTask
Merges CompositeDiagnosticTask into a single DiagnosticTask.
Definition: diagnostic_updater.h:179
diagnostic_updater::Updater::setHardwareID
void setHardwareID(const std::string &hwid)
Definition: diagnostic_updater.h:541
diagnostic_updater::Updater::update_diagnostic_period
void update_diagnostic_period()
Definition: diagnostic_updater.h:551
diagnostic_updater::Updater::next_time_
ros::Time next_time_
Definition: diagnostic_updater.h:616
diagnostic_updater::DiagnosticTaskVector::addedTaskCallback
virtual void addedTaskCallback(DiagnosticTaskInternal &)
Definition: diagnostic_updater.h:364
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal::DiagnosticTaskInternal
DiagnosticTaskInternal(const std::string name, TaskFunction f)
Definition: diagnostic_updater.h:246
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
diagnostic_updater::Updater::update
void update()
Causes the diagnostics to update if the inter-update interval has been exceeded.
Definition: diagnostic_updater.h:417
ros::NodeHandle::ok
bool ok() const
this_node.h
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
Definition: DiagnosticStatusWrapper.h:140
ros::NodeHandle::getParamCached
bool getParamCached(const std::string &key, bool &b) const
ros::Time
diagnostic_updater::FunctionDiagnosticTask
GenericFunctionDiagnosticTask< DiagnosticStatusWrapper > FunctionDiagnosticTask
Definition: diagnostic_updater.h:166
diagnostic_updater::Updater::warn_nohwid_done_
bool warn_nohwid_done_
Definition: diagnostic_updater.h:621
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal::run
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) const
Definition: diagnostic_updater.h:250
diagnostic_updater::DiagnosticTask::getName
const std::string & getName()
Returns the name of the DiagnosticTask.
Definition: diagnostic_updater.h:110
diagnostic_updater::Updater::publish
void publish(diagnostic_msgs::DiagnosticStatus &stat)
Definition: diagnostic_updater.h:561
diagnostic_updater::GenericFunctionDiagnosticTask::name_
const std::string name_
Definition: diagnostic_updater.h:161
diagnostic_updater::DiagnosticTaskVector::DiagnosticTaskInternal
Class used to represent a diagnostic task internally in DiagnosticTaskVector.
Definition: diagnostic_updater.h:243
diagnostic_updater::CompositeDiagnosticTask::CompositeDiagnosticTask
CompositeDiagnosticTask(const std::string name)
Constructs a CompositeDiagnosticTask with the given name.
Definition: diagnostic_updater.h:185
diagnostic_updater::GenericFunctionDiagnosticTask
a DiagnosticTask based on a boost::function.
Definition: diagnostic_updater.h:140
diagnostic_updater::DiagnosticStatusWrapper
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update.
Definition: DiagnosticStatusWrapper.h:98
diagnostic_updater::DiagnosticTaskVector::removeByName
bool removeByName(const std::string name)
Remove a task based on its name.
Definition: diagnostic_updater.h:341
diagnostic_updater::DiagnosticTaskVector::addInternal
void addInternal(DiagnosticTaskInternal &task)
Definition: diagnostic_updater.h:372
diagnostic_updater::DiagnosticTaskVector::tasks_
std::vector< DiagnosticTaskInternal > tasks_
Definition: diagnostic_updater.h:366
diagnostic_updater::GenericFunctionDiagnosticTask::run
virtual void run(DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
Definition: diagnostic_updater.h:155
diagnostic_updater::CompositeDiagnosticTask::run
virtual void run(DiagnosticStatusWrapper &stat)
Runs each child and merges their outputs.
Definition: diagnostic_updater.h:191
diagnostic_updater::DiagnosticTaskVector::getTasks
const std::vector< DiagnosticTaskInternal > & getTasks()
Returns the vector of tasks.
Definition: diagnostic_updater.h:271
diagnostic_updater::Updater::getPeriod
double getPeriod()
Returns the interval between updates.
Definition: diagnostic_updater.h:483
diagnostic_updater::DiagnosticTaskVector::add
void add(const std::string &name, TaskFunction f)
Add a DiagnosticTask embodied by a name and function to the DiagnosticTaskVector.
Definition: diagnostic_updater.h:289
ros::Duration
diagnostic_updater::Updater::setup
void setup()
Definition: diagnostic_updater.h:588
ros::NodeHandle
ros::Time::now
static Time now()


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Tue Nov 15 2022 03:17:19