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
 #endif

 double 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);
         else
           stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Launch is in a long time. Have a soda.");
         // add and addf 函数用于增加 与该状态有关的key-value 对， 另外，add可以自动实现字符串的格式转换。 addf 支持格式化输出如%f
        stat.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");
        else
         stat.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");
        else
         stat.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);

    // 如果节点/处于特殊状态，则可以广播消息到所有的 DiagnosticStatus
         updater.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; 
       }