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 
67  {
68  public:
72  DiagnosticTask(const std::string name) : name_(name)
73  {}
74 
78  const std::string &getName()
79  {
80  return name_;
81  }
82 
86  virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
87 
91  virtual ~DiagnosticTask()
92  {}
93 
94  private:
95  const std::string name_;
96  };
97 
107  template <class T>
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 
188  {
189  tasks_.push_back(t);
190  }
191 
192  private:
193  std::vector<DiagnosticTask *> tasks_;
194  };
195 
205  {
206  protected:
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_;
231  TaskFunction fn_;
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, _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, _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:
333  {}
334  std::vector<DiagnosticTaskInternal> tasks_;
335 
336  protected:
341  {
342  boost::mutex::scoped_lock lock(lock_);
343  tasks_.push_back(task);
344  addedTaskCallback(task);
345  }
346  };
347 
364  {
365  public:
366  bool verbose_;
367 
374  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)
375  {
376  setup();
377  }
378 
383  void update()
384  {
385  ros::Time now_time = ros::Time::now();
386  if (now_time < next_time_) {
387  // @todo put this back in after fix of #2157 update_diagnostic_period(); // Will be checked in force_update otherwise.
388  return;
389  }
390 
391  force_update();
392  }
393 
401  {
402  update_diagnostic_period();
403 
404  next_time_ = ros::Time::now() + ros::Duration().fromSec(period_);
405 
406  if (node_handle_.ok())
407  {
408  bool warn_nohwid = hwid_.empty();
409 
410  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
411 
412  boost::mutex::scoped_lock lock(lock_); // Make sure no adds happen while we are processing here.
413  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
414  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
415  iter != tasks.end(); iter++)
416  {
418 
419  status.name = iter->getName();
420  status.level = 2;
421  status.message = "No message was set";
422  status.hardware_id = hwid_;
423 
424  iter->run(status);
425 
426  status_vec.push_back(status);
427 
428  if (status.level)
429  warn_nohwid = false;
430 
431  if (verbose_ && status.level)
432  ROS_WARN("Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
433  }
434 
435  if (warn_nohwid && !warn_nohwid_done_)
436  {
437  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.");
438  warn_nohwid_done_ = true;
439  }
440 
441  publish(status_vec);
442  }
443  }
444 
449  double getPeriod()
450  {
451  return period_;
452  }
453 
454  // Destructor has trouble because the node is already shut down.
455  /*~Updater()
456  {
457  // Create a new node handle and publisher because the existing one is
458  // probably shut down at this stage.
459 
460  ros::NodeHandle newnh;
461  node_handle_ = newnh;
462  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
463  broadcast(2, "Node shut down");
464  }*/
465 
477  void broadcast(int lvl, const std::string msg)
478  {
479  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
480 
481  const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
482  for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
483  iter != tasks.end(); iter++)
484  {
486 
487  status.name = iter->getName();
488  status.summary(lvl, msg);
489 
490  status_vec.push_back(status);
491  }
492 
493  publish(status_vec);
494  }
495 
496  void setHardwareIDf(const char *format, ...)
497  {
498  va_list va;
499  char buff[1000]; // @todo This could be done more elegantly.
500  va_start(va, format);
501  if (vsnprintf(buff, 1000, format, va) >= 1000)
502  ROS_DEBUG("Really long string in diagnostic_updater::setHardwareIDf.");
503  hwid_ = std::string(buff);
504  va_end(va);
505  }
506 
507  void setHardwareID(const std::string &hwid)
508  {
509  hwid_ = hwid;
510  }
511 
512  private:
518  {
519  double old_period = period_;
520  private_node_handle_.getParamCached("diagnostic_period", period_);
521  next_time_ += ros::Duration(period_ - old_period); // Update next_time_
522  }
523 
527  void publish(diagnostic_msgs::DiagnosticStatus &stat)
528  {
529  std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
530  status_vec.push_back(stat);
531  publish(status_vec);
532  }
533 
537  void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
538  {
539  for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator
540  iter = status_vec.begin(); iter != status_vec.end(); iter++)
541  {
542  iter->name =
543  node_name_.substr(1) + std::string(": ") + iter->name;
544  }
545  diagnostic_msgs::DiagnosticArray msg;
546  msg.status = status_vec;
547  msg.header.stamp = ros::Time::now(); // Add timestamp for ROS 0.10
548  publisher_.publish(msg);
549  }
550 
554  void setup()
555  {
556  publisher_ = node_handle_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
557 
558  period_ = 1.0;
559  next_time_ = ros::Time::now() + ros::Duration(period_);
560  update_diagnostic_period();
561 
562  verbose_ = false;
563  warn_nohwid_done_ = false;
564  }
565 
571  {
573  stat.name = task.getName();
574  stat.summary(0, "Node starting up");
575  publish(stat);
576  }
577 
581 
583 
584  double period_;
585  std::string hwid_;
586  std::string node_name_;
588  };
589 
590 }
591 
592 #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 Thu Oct 8 2020 03:19:34