[wip] Make diagnostics more reasonable

nodes/mtnode.py

@@ -12,6 +12,9 @@
     FluidPressure, Temperature, TimeReference
 from geometry_msgs.msg import TwistStamped, PointStamped
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
+from diagnostic_updater import Updater, TopicDiagnostic,\
+    FrequencyStatusParam, TimeStampStatusParam
+
 import time
 import datetime
 import calendar
@@ -51,7 +54,6 @@ def matrix_from_diagonal(diagonal):
 
 
 class XSensDriver(object):
-
     def __init__(self):
         device = get_param('~device', 'auto')
         baudrate = get_param('~baudrate', 0)
@@ -90,7 +92,6 @@ def __init__(self):
             self.mt.SetNoRotation(no_rotation_duration)
 
         self.frame_id = get_param('~frame_id', '/base_imu')
-
         self.frame_local = get_param('~frame_local', 'ENU')
 
         self.angular_velocity_covariance = matrix_from_diagonal(
@@ -103,17 +104,36 @@ def __init__(self):
             get_param_list("~orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)])
         )
 
-        self.diag_msg = DiagnosticArray()
         self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
                                            message='No status information')
         self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
                                          message='No status information')
         self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
                                          message='No status information')
-        self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
+
+        self.device_info = {
+            'product_code': self.mt.GetProductCode(),
+            'device_id': self.mt.GetDeviceID(),
+            'firmware_rev': self.mt.GetFirmwareRev()
+        }
+
+        self.device_info.update(self.mt.GetConfiguration())
+        self.updater = Updater()
+        self.updater.setHardwareID(str(self.mt.GetDeviceID()))
+
+        self.updater.add("Self Test", self.diagnostic_self_test)
+        self.updater.add("XKF", self.diagnostic_xkf)
+        self.updater.add("GPS Fix", self.diagnostic_gps)
+        self.updater.add("Device Info", self.diagnostic_device)
+
+        self.imu_freq = TopicDiagnostic("imu/data", self.updater,
+                                        FrequencyStatusParam({'min': 0, 'max': 100}, 0.1, 10),
+                                        TimeStampStatusParam())
+        self.mag_freq = TopicDiagnostic("imu/mag", self.updater,
+                                        FrequencyStatusParam({'min': 0, 'max': 100}, 0.1, 10),
+                                        TimeStampStatusParam())
 
         # publishers created at first use to reduce topic clutter
-        self.diag_pub = None
         self.imu_pub = None
         self.raw_gps_pub = None
         self.vel_pub = None
@@ -172,6 +192,20 @@ def spin(self):
         except (select.error, OSError, serial.serialutil.SerialException):
             pass
 
+    def diagnostic_self_test(self, stat):
+        stat.summary(self.stest_stat.level, self.stest_stat.message)
+
+    def diagnostic_xkf(self, stat):
+        stat.summary(self.xkf_stat.level, self.xkf_stat.message)
+
+    def diagnostic_gps(self, stat):
+        stat.summary(self.gps_stat.level, self.gps_stat.message)
+
+    def diagnostic_device(self, stat):
+        stat.summary(DiagnosticStatus.OK, "Device Ok")
+        for (key, value) in self.device_info.iteritems():
+            stat.add(key, value)
+
     def spin_once(self):
         '''Read data from device and publishes ROS messages.'''
         def convert_coords(x, y, z, source, dest=self.frame_local):
@@ -563,7 +597,7 @@ def fill_from_Angular_Velocity(o):
             block.'''
             try:
                 dqw, dqx, dqy, dqz = (o['Delta q0'], o['Delta q1'],
-                    o['Delta q2'], o['Delta q3'])
+                                      o['Delta q2'], o['Delta q3'])
                 now = rospy.Time.now()
                 if self.last_delta_q_time is None:
                     self.last_delta_q_time = now
@@ -723,6 +757,7 @@ def find_handler_name(name):
             self.imu_msg.header = self.h
             if self.imu_pub is None:
                 self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
+            self.imu_freq.tick(self.h.stamp)
             self.imu_pub.publish(self.imu_msg)
         if self.pub_raw_gps:
             self.raw_gps_msg.header = self.h
@@ -745,6 +780,7 @@ def find_handler_name(name):
             if self.mag_pub is None:
                 self.mag_pub = rospy.Publisher('imu/mag', MagneticField,
                                                queue_size=10)
+            self.mag_freq.tick(self.h.stamp)
             self.mag_pub.publish(self.mag_msg)
         if self.pub_temp:
             self.temp_msg.header = self.h
@@ -774,14 +810,10 @@ def find_handler_name(name):
                 self.ecef_pub = rospy.Publisher('ecef', PointStamped,
                                                 queue_size=10)
             self.ecef_pub.publish(self.ecef_msg)
-        if self.pub_diag:
-            self.diag_msg.header = self.h
-            if self.diag_pub is None:
-                self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray,
-                                                queue_size=10)
-            self.diag_pub.publish(self.diag_msg)
+
         # publish string representation
         self.str_pub.publish(str(data))
+        self.updater.update()
 
 
 def main():

package.xml

@@ -15,6 +15,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>diagnostic_msgs</build_depend>
+  <build_depend>diagnostic_updater</build_depend>
 
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
@@ -23,6 +24,7 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>diagnostic_msgs</run_depend>
   <run_depend>python-serial</run_depend>
+  <run_depend>diagnostic_updater</run_depend>
 </package>