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


imu_monitor
Author(s): Wim Meeussen
autogenerated on Fri Dec 6 2013 20:23:45