Added diagnostics and default covariance values to base controller module

ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -22,18 +22,20 @@
 """
 
 import rospy
-import os
+import sys, os
 
 from math import sin, cos, pi
 from geometry_msgs.msg import Quaternion, Twist, Pose
 from nav_msgs.msg import Odometry
 from tf.broadcaster import TransformBroadcaster
-from sys import float_info
+from ros_arduino_python.diagnostics import DiagnosticsUpdater
+
  
 """ Class to receive Twist commands and publish Odometry data """
 class BaseController:
-    def __init__(self, arduino, base_frame):
+    def __init__(self, arduino, base_frame, name='base_controller'):
         self.arduino = arduino
+        self.name = name
         self.base_frame = base_frame
         self.rate = float(rospy.get_param("~base_controller_rate", 10))
         self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
@@ -41,7 +43,9 @@ class BaseController:
         self.odom_angular_scale_correction = rospy.get_param("~odom_angular_scale_correction", 1.0)
         self.use_imu_heading = rospy.get_param("~use_imu_heading", False)
         self.publish_odom_base_transform = rospy.get_param("~publish_odom_base_transform", True)
+
         self.stopped = False
+        self.current_speed = Twist()
                  
         pid_params = dict()
         pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
@@ -55,19 +59,28 @@ class BaseController:
         
         self.accel_limit = rospy.get_param('~accel_limit', 1.0)
         self.motors_reversed = rospy.get_param("~motors_reversed", False)
-        
+
+        # Default error threshold (percent) before getting a diagnostics warning
+        self.base_diagnotics_error_threshold = rospy.get_param("~base_diagnotics_error_threshold", 10)
+
+        # Diagnostics update rate
+        self.base_diagnotics_rate = rospy.get_param("~base_diagnotics_rate", 1.0)
+
+        # Create the diagnostics updater for the Arduino device
+        self.diagnostics = DiagnosticsUpdater(self, self.name, self.base_diagnotics_error_threshold, self.base_diagnotics_rate)
+
         # Set up PID parameters and check for missing values
         self.setup_pid(pid_params)
-            
+
         # How many encoder ticks are there per meter?
         self.ticks_per_meter = self.encoder_resolution * self.gear_reduction  / (self.wheel_diameter * pi)
-        
+
         # What is the maximum acceleration we will tolerate when changing wheel speeds?
         self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
-                
+
         # Track how often we get a bad encoder count (if any)
         self.bad_encoder_count = 0
-                        
+
         now = rospy.Time.now()    
         self.then = now # time for determining dx/dy
         self.t_delta = rospy.Duration(1.0 / self.rate)
@@ -87,14 +100,14 @@ class BaseController:
 
         # Subscriptions
         rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
-        
+
         # Clear any old odometry info
         self.arduino.reset_encoders()
-        
+
         # Set up the odometry broadcaster
         self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
         self.odomBroadcaster = TransformBroadcaster()
-        
+
         rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
         rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
         
@@ -119,16 +132,22 @@ class BaseController:
         self.Ki = pid_params['Ki']
         self.Ko = pid_params['Ko']
         
-        self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
+        if self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko):
-        rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko))
+            rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko))
+        else:
+            rospy.logerr("Updating PID parameters failed!")
 
     def poll(self):
         now = rospy.Time.now()
         if now > self.t_next:
             # Read the encoders
             try:
+                self.diagnostics.reads += 1
+                self.diagnostics.total_reads += 1
                 left_enc, right_enc = self.arduino.get_encoder_counts()
+                self.diagnostics.freq_diag.tick()
             except:
+                self.diagnostics.errors += 1
                 self.bad_encoder_count += 1
                 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
                 return
@@ -190,8 +209,12 @@ class BaseController:
             odom.twist.twist.linear.y = 0
             odom.twist.twist.angular.z = vth
             
+            self.current_speed = Twist()
+            self.current_speed.linear.x = vxy
+            self.current_speed.angular.z = vth
+
             """
-            Covariance values taken from Kobuki node odometry.hpp at:
+            Covariance values taken from Kobuki node odometry.cpp at:
             https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
             
             Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
@@ -200,11 +223,15 @@ class BaseController:
             """
             odom.pose.covariance[0]  = 0.1
             odom.pose.covariance[7]  = 0.1
-            odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2
+            if self.use_imu_heading:
+                #odom.pose.covariance[35] = 0.2
+                odom.pose.covariance[35] = 0.05
+            else:
+                odom.pose.covariance[35] = 0.05
             
-            odom.pose.covariance[14] = 1e6  # set a non-zero covariance on unused
+            odom.pose.covariance[14] = sys.float_info.max  # set a non-zero covariance on unused
-            odom.pose.covariance[21] = 1e6  # dimensions (z, pitch and roll); this
+            odom.pose.covariance[21] = sys.float_info.max  # dimensions (z, pitch and roll); this
-            odom.pose.covariance[28] = 1e6  # is a requirement of robot_pose_ekf
+            odom.pose.covariance[28] = sys.float_info.max  # is a requirement of robot_pose_ekf
 
             self.odomPub.publish(odom)
             
@@ -266,4 +293,5 @@ class BaseController:
         self.x = 0.0
         self.y = 0.0
         self.th = 0.0
+