Merge pull request #1417 from Kanazawanaoaki/add-diagnos-listener

Add diagnostics_ethercat_summarizer.py for packet drop confirmation

Author: k-okada

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/diagnostics_ethercat_summarizer.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+
+import rospy
+
+from diagnostic_msgs.msg import DiagnosticArray
+
+
+def callback(data):
+    flag = False
+    for e in data.status:
+        #EtherCAT Device
+        for v in e.values:
+            if v.key =='Drops':
+                flag = True
+                print(e.name + "Drops: {}".format(v.value))
+        #EtherCAT Master
+        if e.name == 'EtherCAT Master':
+            for v in e.values:
+                if v.key =='Dropped Packets':
+                    print(e.name + " Dropped Packets: {}".format(v.value))
+                if v.key =='RX Late Packet':
+                    print(e.name + " RX Late Packet: {}".format(v.value))
+
+    if flag:
+        print("--------------------------------------------")
+
+
+def listener():
+    rospy.init_node('diagnostics_ethercat_summarizer', anonymous=True)
+    rospy.Subscriber('/diagnostics', DiagnosticArray, callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()