ROS中的diagnostics模块

（关于该模块的参考资料较少，下面整理的仅是个人的理解，有不恰当的地方欢迎指正，更具体的内容参考ROS官网说明）
diagnostics意为诊断，该模块旨在从系统中收集各种状态信息和硬件信息，以进行分析，故障排除和记录。工具链中包含用于收集，发布，分析和查看diagnostics数据的工具。主要内容有：

（1） diagnostics的消息

诊断工具链围绕/diagnostics主题构建。在此主题上，系统通过diagnostic_msgs/DiagnosticArray类型的消息（定义如下）发布设备的各种状态，每个状态都关联相应的诊断任务。

Header header #for timestamp
DiagnosticStatus[] status # an array of components being reported on

其中DiagnosticStatus具体信息包括：

byte level               # level of operation enumerated above
string name              # a description of the test/component reporting
string message           # a description of the status
string hardware_id       # 硬件ID
KeyValue[] values        # an array of values associated with the status

level 的值设定：

byte OK=0
byte WARN=1
byte ERROR=2
byte STALE=3

（2）消息发布 diagnostic_updater

diagnostic_updater是diagnostics模块的核心，用于：

在设备驱动程序上发布传感器数据主题的状态
报告硬件设备已关闭
当值超出范围时报告错误，例如温度

API：

diagnostic_updater::DiagnosticStatusWrapper

diagnostic_updater::DiagnosticStatusWrapper 类用于处理与diagnostic_msgs/DiagnosticStatus消息相关的操作如设置DiagnosticStatus中各个字段的信息，具体可以参考文章后面的例子，其成员函数有：

void     add (const std::string &key, const T &val)# Add a key-value pair.
void    add (const std::string &key, const bool &b)# For bool, diagnostic value is "True" or "False".
void    addf (const std::string &key, const char *format,...)# Add a key-value pair using a format string.
void   clear ()# Clear the key-value pairs.
........

diagnostic_updater::Updater

diagnostic_updater::Updater 类本质上是一个诊断任务的向量，通过该类可以添加或删除diagnostic任务、以及确定任务更新频率等，其用法可以参看文章最后的例子。该类中的每一个任务均通过DiagnosticStatusWrapper维护一个DiagnosticStatus状态。

diagnostic_updater::DiagnosedPublisher

对于一个标准的ros：Publisher，这个类允许将ros：Publisher和topicDiagnosis组合在一起。如下：

   ros::Publisher pub_;pub_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud", 100);void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);{.........
}diagnostic_updater::DiagnosedPublisher<sensor_msgs::PointCloud2>* diagnosticPub_;diagnosticPub_ = new diagnostic_updater::DiagnosedPublisher<sensor_msgs::PointCloud2>(pub_, *diagnostics_,diagnostic_updater::FrequencyStatusParam(&expected_frequency_, &expected_frequency_, 0.1, 10),diagnostic_updater::TimeStampStatusParam(-1, 1.3 * 1.0 / 12.5));diagnostics_->setHardwareID("none");  diagnostics_->add("device info", this, &produce_diagnostics);

发布时调用

   sensor_msgs::PointCloud2 msg；diagnosticPub_->publish(msg);

（3）diagnostics数据的分析与整合

diagnostic_aggregator包含一个ros节点aggregator_node，主要用于在运行时对诊断消息进行分类和分析。该节点的订阅发布消息如下：

Subscribed Topics
/diagnostics (diagnostic_msgs/DiagnosticArray)
Published Topics
/diagnostics_agg(diagnostic_msgs/DiagnosticArray)
/diagnostics_toplevel_state (diagnostic_msgs/DiagnosticStatus)

如果机器人有电机驱动器、SICK激光扫描仪、Videre摄像机和几个电池的诊断信息，诊断输入可能有状态名称：

Left Wheel
Right Wheel
SICK Frequency
SICK Temperature
SICK Connection Status
Stereo Left Camera
Stereo Right Camera
Stereo Analysis
Stereo Connection Status
Battery 1 Level
Battery 2 Level
Battery 3 Level
Battery 4 Level
Voltage Status

使用diagnostic aggregator, 我们可以将这些诊断消息以一种易于理解的形式组织起来，以便在robot_monitor中显示。即将类别的名称放在状态名称的前面，然后用"/"分割。

My Robot/Wheels/Left
My Robot/Wheels/Right
My Robot/SICK/Frequency
My Robot/SICK/Temperature
My Robot/SICK/Connection Status
My Robot/Stereo/Left Camera
My Robot/Stereo/Right Camera
My Robot/Stereo/Analysis
My Robot/Stereo/Connection Status
My Robot/Power System/Battery 1 Level
My Robot/Power System/Battery 2 Level
My Robot/Power System/Battery 3 Level
My Robot/Power System/Battery 4 Level
My Robot/Power System/Voltage Status

使用上述输入，机器人监视器将数据分类如下:

My Robot-- Wheels-- Left-- Right-- SICK-- etc ...-- Stereo-- Power System

（4）diagnostics数据的查看工具

rqt_robot_monitor 用于直观的查看 diagnostic_aggregator发布的消息，作用类似于rqt_bag.

（5）diagnostics数据的转换工具

diagnostic_analysis工具可以将diagnostics产生的数据bag文件转换为CSV文件，以便用现成的电子表格软件绘制或查看。

参考：
http://wiki.ros.org/diagnostics
http://wiki.ros.org/diagnostic_aggregator#Example
http://wiki.ros.org/diagnostic_updater?distro=melodic

 #include <diagnostic_updater/diagnostic_updater.h>#include <std_msgs/Bool.h>#include <diagnostic_updater/publisher.h>// 解决与windows.h中宏 ERROR定义的冲突#ifdef ERROR#undef ERROR#endifdouble time_to_launch;任务函数void dummy_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){// summary 和summaryf 函数用于设置 level 与message，不同点在于后者可以添加变量的输出如%f 。if (time_to_launch < 10)stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Buckle your seat belt. Launch in %f seconds!", time_to_launch);elsestat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Launch is in a long time. Have a soda.");// add and addf 函数用于增加 与该状态有关的key-value 对， 另外，add可以自动实现字符串的格式转换。 addf 支持格式化输出如%fstat.add("Diagnostic Name", "dummy");stat.add("Time to Launch", time_to_launch);stat.addf("Geeky thing to say", "The square of the time to launch %f is %f",   time_to_launch, time_to_launch * time_to_launch);}/用类的方法添加任务
class DummyClass{ public:  void  produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is a silly updater.");stat.add("Stupidicity of this updater", 1000.);} };class DummyTask : public diagnostic_updater::DiagnosticTask{public:DummyTask() : DiagnosticTask("Updater Derived from DiagnosticTask"){}void run(diagnostic_updater::DiagnosticStatusWrapper &stat){stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "This is another silly updater.");stat.add("Stupidicity of this updater", 2000.);}};任务函数的组合示例    void check_lower_bound(diagnostic_updater::DiagnosticStatusWrapper &stat){if (time_to_launch > 5)stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Lower-bound OK");elsestat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Too low");stat.add("Low-Side Margin", time_to_launch - 5);}void check_upper_bound(diagnostic_updater::DiagnosticStatusWrapper &stat){if (time_to_launch < 10)stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Upper-bound OK");elsestat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too high");stat.add("Top-Side Margin", 10 - time_to_launch);}int main(int argc, char **argv){ros::init(argc, argv, "diagnostic_updater_example");ros::NodeHandle nh;//Updater类发布 /diagnostics消息, 并且有一个~diagnostic_period 参数设置发布频率diagnostic_updater::Updater updater;//关联ID，以下两种方法均可设置ID ,不设置时建议填入none,否则会警告updater.setHardwareID("none"); updater.setHardwareIDf("Device-%i-%i", 27, 46);// 将diagnostic任务添加到Updater中。它们将在调用Updater（）函数时运行。//本质上, updater.add将传入内容转换为 DiagnosticTask。 //以下是两种添加的方法，任务的名称对应于任务的DiagnosticStatus的名称updater.add("Function updater", dummy_diagnostic);DummyClass dc;updater.add("Method updater", &dc&DummyClass::produce_diagnostics);//另外， FunctionDiagnosticTask 可以通过函数创建DiagnosticTask，//并且可以通过CompositeDiagnosticTask合并任务的输出，将 CompositeDiagnosticTask添加到Updater。//运行时总体名称将是子任务的名称, i.e., "Bound check". diagnostic_updater::FunctionDiagnosticTask lower("Lower-bound check",boost::bind(&check_lower_bound, _1));diagnostic_updater::FunctionDiagnosticTask upper("Upper-bound check",boost::bind(&check_upper_bound, _1));diagnostic_updater::CompositeDiagnosticTask bounds("Bound check");bounds.addTask(&lower);bounds.addTask(&upper);updater.add(bounds);// 如果节点/处于特殊状态，则可以广播消息到所有的 DiagnosticStatusupdater.broadcast(0, "Doing important initialization stuff.");ros::Publisher pub1 = nh.advertise<std_msgs::Bool>("topic1", 1);ros::Duration(2).sleep(); // 循环时间控制， 通过主函数中的ros::Duration(0.1).sleep()实现；// FrequencyStatus和TimestampStatus 是分别监视事件频率和时间戳的Diagnostic任务。// 对于没有header的消息，可以通过 HeaderlessTopicDiagnostic 处理FrequencyStatus （不能处理TimestampStatus）//或TopicDiagnostic可以处理FrequencyStatus 和TimestampStatus任务double min_freq = 0.5;double max_freq = 2;diagnostic_updater::HeaderlessTopicDiagnostic pub1_freq("topic1", updater, diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10)); // 注意TopicDiagnostic, HeaderlessDiagnosedPublisher,HeaderlessDiagnosedPublisher 以及 DiagnosedPublisher //都派生自CompositeDiagnosticTask， 因此你可以通过addTask添加自己的任务。pub1_freq.addTask(&lower); // **(This wouldn't work if lower was stateful).**    // 每次更新pub1_freq时，lower也将得到更新，其输出将与pub1_freq的输出合并。// 如果知道节点状态改变，可以强制立即更新updater.force_update();       //删除任务if (!updater.removeByName("Bound check"))ROS_ERROR("The Bound check task was not found when trying to remove it.");while (nh.ok()){std_msgs::Bool msg;ros::Duration(0.1).sleep();// 对pub1的调用必须伴随对pub1_freq的调用，以保持最新的统计数据。msg.data = false;pub1.publish(msg);pub1_freq.tick();//可以在程序空闲时随时调用updater。函数本身将限制更新速度。updater.update();}return 0; }