motor_resetter.py

Go to the documentation of this file.

#!/usr/bin/env python

from __future__ import with_statement

PKG = 'motor_resetter'
import roslib; roslib.load_manifest(PKG)

import rospy

from std_msgs.msg import Bool
from std_msgs.msg import Empty as mEmpty
from std_srvs.srv import Empty
from diagnostic_msgs.msg import DiagnosticArray
from collections import deque
import traceback

import threading
import rosbag
import time

MAX_LEN = 500
BUFFER_TIME = 10

class MotorResetter(object):
    def __init__(self):
        self._mutex = threading.Lock()
        self._motors = True
        self._diags = deque()

        self._reset_srv = rospy.ServiceProxy('pr2_etherCAT/reset_motors', Empty)
        self._reset_pub = rospy.Publisher('motors_reset', mEmpty)
        self._motors_sub = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self._motors_cb)
        self._diag_sub = rospy.Subscriber('/diagnostics', DiagnosticArray, self._diag_cb)

    def _reset_motors(self):
        try:
            self._reset_srv()
        except Exception, e:
            rospy.logerr('Unable to reset motors!\n%s' % traceback.format_exc())

        self._record_diagnostic_bag()

        self._reset_pub.publish()
            
    def _diag_cb(self, msg):
        with self._mutex:
            # Clean up the deque
            #self._diags.reverse()
            for m in reversed(self._diags):
                if rospy.get_time() - m.header.stamp.to_sec() > BUFFER_TIME:
                    self._diags.pop()
            #self._diags.reverse()
            
            # Append our message
            self._diags.append(msg)

    def _record_diagnostic_bag(self):
        with self._mutex:
            bag_name = '/hwlog/pr2_motors_halted_' + time.strftime('%Y_%m_%d_%I_%M_%S', time.localtime()) + '.bag'
            
            bag = rosbag.Bag(bag_name, 'w')
            for m in self._diags:
                bag.write('/diagnostics', m)
            bag.close()
        
        

    def _motors_cb(self, msg):
        if self._motors and msg.data:
            rospy.logwarn('Resetting motors after they halted. Fault condition')
            self._reset_motors()

        self._motors = not msg.data


if __name__ == '__main__':
    rospy.init_node('motor_resetter')
    
    MotorResetter()
    rospy.spin()