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; 
       }

ROS中的diagnostics模塊

（1） diagnostics的消息

（2）消息發佈 diagnostic_updater

（3）diagnostics數據的分析與整合

（4）diagnostics數據的查看工具

（5）diagnostics數據的轉換工具

激光雷達幀頭協議說明---以Velodyne64爲例

Windows下Python的Setuptools模塊採用的編譯器

Window下配置Deep_Sort_Pytorch

對偶（duality）的含義

OpenCV中的卡爾曼濾波方法

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結