motor_resetter.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from __future__ import with_statement
00004 
00005 PKG = 'motor_resetter'
00006 import roslib; roslib.load_manifest(PKG)
00007 
00008 import rospy
00009 
00010 from std_msgs.msg import Bool
00011 from std_msgs.msg import Empty as mEmpty
00012 from std_srvs.srv import Empty
00013 from diagnostic_msgs.msg import DiagnosticArray
00014 from collections import deque
00015 import traceback
00016 
00017 import threading
00018 import rosbag
00019 import time
00020 
00021 MAX_LEN = 500
00022 BUFFER_TIME = 10
00023 
00024 class MotorResetter(object):
00025     def __init__(self):
00026         self._mutex = threading.Lock()
00027         self._motors = True
00028         self._diags = deque()
00029 
00030         self._reset_srv = rospy.ServiceProxy('pr2_etherCAT/reset_motors', Empty)
00031         self._reset_pub = rospy.Publisher('motors_reset', mEmpty)
00032         self._motors_sub = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self._motors_cb)
00033         self._diag_sub = rospy.Subscriber('/diagnostics', DiagnosticArray, self._diag_cb)
00034 
00035     def _reset_motors(self):
00036         try:
00037             self._reset_srv()
00038         except Exception, e:
00039             rospy.logerr('Unable to reset motors!\n%s' % traceback.format_exc())
00040 
00041         self._record_diagnostic_bag()
00042 
00043         self._reset_pub.publish()
00044             
00045     def _diag_cb(self, msg):
00046         with self._mutex:
00047             # Clean up the deque
00048             #self._diags.reverse()
00049             for m in reversed(self._diags):
00050                 if rospy.get_time() - m.header.stamp.to_sec() > BUFFER_TIME:
00051                     self._diags.pop()
00052             #self._diags.reverse()
00053             
00054             # Append our message
00055             self._diags.append(msg)
00056 
00057     def _record_diagnostic_bag(self):
00058         with self._mutex:
00059             bag_name = '/hwlog/pr2_motors_halted_' + time.strftime('%Y_%m_%d_%I_%M_%S', time.localtime()) + '.bag'
00060             
00061             bag = rosbag.Bag(bag_name, 'w')
00062             for m in self._diags:
00063                 bag.write('/diagnostics', m)
00064             bag.close()
00065         
00066         
00067 
00068     def _motors_cb(self, msg):
00069         if self._motors and msg.data:
00070             rospy.logwarn('Resetting motors after they halted. Fault condition')
00071             self._reset_motors()
00072 
00073         self._motors = not msg.data
00074 
00075 
00076 if __name__ == '__main__':
00077     rospy.init_node('motor_resetter')
00078     
00079     MotorResetter()
00080     rospy.spin()


motor_resetter
Author(s):
autogenerated on Fri Dec 6 2013 21:22:27