$search
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()