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 PKG = 'pr2_hardware_test_monitor'
00040
00041 import roslib
00042 roslib.load_manifest(PKG)
00043
00044 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
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 HokuyoListener(PR2HWListenerBase):
00054 """
00055 Listens to diagnostics from a Hokuyo laser scanner with the given namespace.
00056 """
00057 def __init__(self):
00058 self._diag_sub = rospy.Subscriber('/diagnostics', DiagnosticArray, self._diag_callback)
00059 self._mutex = threading.Lock()
00060
00061 self._ok = True
00062 self._update_time = 0
00063 self.name = 'tilt_hokuyo_node'
00064
00065 def create(self, params):
00066 """
00067 params must have the value "name", which is the namespace of the Hokuyo to listen to.
00068
00069 \param params { } : ROS Parameters in namespace
00070 \return bool : True if created OK
00071 """
00072 if not params.has_key('name'):
00073 rospy.logerr('Hokuyo Listener was not given param "name"')
00074 return False
00075 self.name = params['name']
00076
00077 return True
00078
00079 def _diag_callback(self, msg):
00080 self._mutex.acquire()
00081 for stat in msg.status:
00082 if stat.name.find(self.name) >= 0:
00083 self._ok = (stat.level == 0)
00084 self._update_time = rospy.get_time()
00085 if not self._ok:
00086 break
00087
00088 self._mutex.release()
00089
00090 def check_ok(self):
00091 self._mutex.acquire()
00092 msg = ''
00093 stat = 0
00094 if not self._ok:
00095 stat = 2
00096 msg = 'Hokuyo Error'
00097
00098 if rospy.get_time() - self._update_time > 3:
00099 stat = 3
00100 msg = 'Hokuyo Stale'
00101 if self._update_time == 0:
00102 msg = 'No Hokuyo Data'
00103
00104 self._mutex.release()
00105 return stat, msg, None
00106