Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 from __future__ import with_statement
00040 PKG = 'pr2_hardware_test_monitor'
00041
00042 import roslib; roslib.load_manifest(PKG)
00043
00044 from std_msgs.msg import Bool
00045 from std_srvs.srv import *
00046
00047 import rospy
00048
00049 import threading
00050
00051 from pr2_hw_listener import PR2HWListenerBase
00052
00053 class DriveListener(PR2HWListenerBase):
00054 def __init__(self):
00055 self._mutex = threading.Lock()
00056
00057 self._cal = False
00058 self._ok = True
00059 self._update_time = -1
00060 self._reset_driving = rospy.ServiceProxy('pr2_base/reset_drive', Empty)
00061 self._halt_driving = rospy.ServiceProxy('pr2_base/halt_drive', Empty)
00062
00063 self._is_driving = True
00064 self._drive_sub = rospy.Subscriber('base_driving', Bool, self._drive_cb)
00065
00066 def halt(self):
00067 self._halt_driving()
00068
00069 def reset(self):
00070 self._reset_driving()
00071
00072 def _drive_cb(self, msg):
00073 with self._mutex:
00074 self._is_driving = not msg.data
00075 self._update_time = rospy.get_time()
00076
00077 def check_ok(self):
00078 with self._mutex:
00079 msg = ''
00080 stat = 0
00081 if not self._is_driving:
00082 stat = 1
00083 msg = 'Not driving'
00084
00085 if rospy.get_time() - self._update_time > 3:
00086 stat = 3
00087 msg = 'Drive Status Stale'
00088 if self._update_time == -1:
00089 msg = 'No Drive Status'
00090
00091 return stat, msg, None
00092