imu_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import PyKDL
00005 from sensor_msgs.msg import Imu
00006 from pr2_mechanism_controllers.msg import Odometer
00007 from threading import Lock
00008 from math import fabs, pi
00009 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00010 
00011 EPS = 0.0001
00012 
00013 class ImuMonitor:
00014     def __init__(self):
00015         self.lock = Lock()
00016 
00017         # reset state
00018         self.dist = 0.0
00019         self.drift = -1.0
00020         self.last_angle = 0.0
00021         self.start_angle = 0.0
00022         self.start_time = rospy.Time.now()
00023 
00024         # subscribe to topics
00025         self.imu_sub = rospy.Subscriber('torso_lift_imu/data', Imu, self.imu_cb)
00026         self.odom_sub = rospy.Subscriber('base_odometry/odometer', Odometer, self.odom_cb)
00027 
00028         # diagnostics
00029         self.pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray)
00030 
00031 
00032     def imu_cb(self, msg):
00033         with self.lock:
00034             rot = PyKDL.Rotation.Quaternion(msg.orientation.x, msg.orientation.y,
00035                                             msg.orientation.z, msg.orientation.w)
00036             (r, p, self.last_angle) = rot.GetRPY()
00037 
00038     def odom_cb(self, msg):
00039         with self.lock:
00040             dist = msg.distance + (msg.angle * 0.25)
00041 
00042             # check if base moved
00043             if dist > self.dist + EPS:
00044                 self.start_time = rospy.Time.now()
00045                 self.start_angle = self.last_angle
00046                 self.dist = dist
00047         
00048             # do imu test if possible
00049             if rospy.Time.now() > self.start_time + rospy.Duration(10.0):
00050                 self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10)
00051                 self.start_time = rospy.Time.now()
00052                 self.start_angle = self.last_angle
00053                 self.last_measured = rospy.Time.now()
00054                 
00055             # publish diagnostics
00056             d = DiagnosticArray()
00057             d.header.stamp = rospy.Time.now()
00058             ds = DiagnosticStatus()
00059             ds.name = "imu_node: Imu Drift Monitor"
00060             if self.drift < 0.5:
00061                 ds.level = DiagnosticStatus.OK
00062                 ds.message = 'OK'
00063             elif self.drift < 1.0:
00064                 ds.level = DiagnosticStatus.WARN
00065                 ds.message = 'Drifting'
00066             else:
00067                 ds.level = DiagnosticStatus.ERROR
00068                 ds.message = 'Drifting'
00069             drift = self.drift
00070             if self.drift < 0:
00071                 last_measured = 'No measurements yet, waiting for base to stop moving before measuring'
00072                 drift = 'N/A'
00073             else:
00074                 age = (rospy.Time.now() - self.last_measured).to_sec()
00075                 if age < 60:
00076                     last_measured = '%f seconds ago'%age
00077                 elif age < 3600:
00078                     last_measured = '%f minutes ago'%(age/60)
00079                 else:
00080                     last_measured = '%f hours ago'%(age/3600)                    
00081             ds.values = [
00082                 KeyValue('Last measured', last_measured),
00083                 KeyValue('Drift (deg/sec)', str(drift)) ]
00084             d.status = [ds]
00085             self.pub_diag.publish(d)
00086 
00087 def main():
00088     rospy.init_node('imu_monitor')
00089     rospy.sleep(0.001) # init time
00090 
00091     monitor = ImuMonitor()
00092 
00093     rospy.spin()
00094 
00095 
00096 if __name__ == '__main__':
00097     main()


imu_monitor
Author(s): Wim Meeussen
autogenerated on Thu Sep 3 2015 11:31:31