Go to the documentation of this file.
35 #ifndef DIAGNOSTICUPDATER_HH
36 #define DIAGNOSTICUPDATER_HH
45 #include "diagnostic_msgs/DiagnosticArray.h"
46 #include "diagnostic_msgs/DiagnosticStatus.h"
49 #include <boost/thread.hpp>
95 const std::string
name_;
108 class GenericFunctionDiagnosticTask :
public DiagnosticTask
129 const std::string
name_;
164 original_summary.
summary(stat);
166 for (std::vector<DiagnosticTask *>::iterator i =
tasks_.begin();
170 stat.
summary(original_summary);
178 stat.
summary(combined_summary);
193 std::vector<DiagnosticTask *>
tasks_;
211 class DiagnosticTaskInternal
224 const std::string &
getName()
const
239 const std::vector<DiagnosticTaskInternal> &
getTasks()
293 DiagnosticTaskInternal int_task(name, boost::bind(f, c, boost::placeholders::_1));
311 boost::mutex::scoped_lock lock(
lock_);
312 for (std::vector<DiagnosticTaskInternal>::iterator iter =
tasks_.begin();
313 iter !=
tasks_.end(); iter++)
315 if (iter->getName() == name)
334 std::vector<DiagnosticTaskInternal>
tasks_;
342 boost::mutex::scoped_lock lock(
lock_);
363 class Updater :
public DiagnosticTaskVector
410 bool warn_nohwid =
hwid_.empty();
412 std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
414 boost::mutex::scoped_lock lock(
lock_);
415 const std::vector<DiagnosticTaskInternal> &tasks =
getTasks();
416 for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
417 iter != tasks.end(); iter++)
421 status.name = iter->getName();
423 status.message =
"No message was set";
424 status.hardware_id =
hwid_;
428 status_vec.push_back(status);
434 ROS_WARN(
"Non-zero diagnostic status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
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.");
481 std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
483 const std::vector<DiagnosticTaskInternal> &tasks =
getTasks();
484 for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
485 iter != tasks.end(); iter++)
489 status.name = iter->getName();
492 status_vec.push_back(status);
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);
529 void publish(diagnostic_msgs::DiagnosticStatus &stat)
531 std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
532 status_vec.push_back(stat);
539 void publish(std::vector<diagnostic_msgs::DiagnosticStatus> &status_vec)
541 for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator
542 iter = status_vec.begin(); iter != status_vec.end(); iter++)
545 node_name_.substr(1) + std::string(
": ") + iter->name;
547 diagnostic_msgs::DiagnosticArray
msg;
548 msg.status = status_vec;
575 stat.name = task.getName();
576 stat.
summary(0,
"Node starting up");
void addTask(DiagnosticTask *t)
Adds a child CompositeDiagnosticTask.
virtual void addedTaskCallback(DiagnosticTaskInternal &task)
std::vector< DiagnosticTask * > tasks_
Duration & fromSec(double t)
void setHardwareIDf(const char *format,...)
void force_update()
Forces the diagnostics to update.
DiagnosticTask(const std::string name)
Constructs a DiagnosticTask setting its name in the process.
boost::function< void(diagnostic_msgs::DiagnosticStatus &)> UnwrappedTaskFunction
void mergeSummary(unsigned char lvl, const std::string s)
Merges a level and message with the existing ones.
ros::NodeHandle node_handle_
const std::string & getName() const
virtual ~DiagnosticTask()
void publish(const boost::shared_ptr< M > &message) const
void broadcast(int lvl, const std::string msg)
Output a message on all the known DiagnosticStatus.
Publisher advertise(AdvertiseOptions &ops)
ros::NodeHandle private_node_handle_
GenericFunctionDiagnosticTask(const std::string &name, boost::function< void(T &)> fn)
DiagnosticTask is an abstract base class for collecting diagnostic data.
GenericFunctionDiagnosticTask< diagnostic_msgs::DiagnosticStatus > UnwrappedFunctionDiagnosticTask
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)=0
Fills out this Task's DiagnosticStatusWrapper.
Updater(ros::NodeHandle h=ros::NodeHandle(), ros::NodeHandle ph=ros::NodeHandle("~"), std::string node_name=ros::this_node::getName())
Constructs an updater class.
ros::Publisher publisher_
boost::function< void(DiagnosticStatusWrapper &)> TaskFunction
Merges CompositeDiagnosticTask into a single DiagnosticTask.
void setHardwareID(const std::string &hwid)
void update_diagnostic_period()
virtual void addedTaskCallback(DiagnosticTaskInternal &)
DiagnosticTaskInternal(const std::string name, TaskFunction f)
const ROSCPP_DECL std::string & getName()
void update()
Causes the diagnostics to update if the inter-update interval has been exceeded.
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
bool getParamCached(const std::string &key, bool &b) const
GenericFunctionDiagnosticTask< DiagnosticStatusWrapper > FunctionDiagnosticTask
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) const
const std::string & getName()
Returns the name of the DiagnosticTask.
void publish(diagnostic_msgs::DiagnosticStatus &stat)
Class used to represent a diagnostic task internally in DiagnosticTaskVector.
CompositeDiagnosticTask(const std::string name)
Constructs a CompositeDiagnosticTask with the given name.
a DiagnosticTask based on a boost::function.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update.
bool removeByName(const std::string name)
Remove a task based on its name.
void addInternal(DiagnosticTaskInternal &task)
std::vector< DiagnosticTaskInternal > tasks_
virtual void run(DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
virtual void run(DiagnosticStatusWrapper &stat)
Runs each child and merges their outputs.
const std::vector< DiagnosticTaskInternal > & getTasks()
Returns the vector of tasks.
double getPeriod()
Returns the interval between updates.
void add(const std::string &name, TaskFunction f)
Add a DiagnosticTask embodied by a name and function to the DiagnosticTaskVector.
diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen
, Jeremy Leibs, Blaise Gassend
autogenerated on Tue Nov 15 2022 03:17:19