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 from diagnostic_msgs.msg import DiagnosticArray
00047
00048 import rospy
00049
00050 import threading
00051
00052 from pr2_hw_listener import PR2HWListenerBase
00053
00054 from collections import deque
00055
00056 DROPS_PER_HOUR = 10
00057
00058 ENCODER_ERRORS_FIELD = 'Num encoder_errors'
00059
00060 class EthercatListener(PR2HWListenerBase):
00061 def __init__(self):
00062 self._mutex = threading.Lock()
00063
00064 self._cal = False
00065 self._ok = True
00066 self._update_time = -1
00067 self._diag_update_time = -1
00068
00069
00070
00071 self._dropped_times = deque()
00072
00073 self._net_drops = 0
00074
00075 self._encoder_errors_detected = False
00076 self._last_encoder_errors_update = rospy.get_time()
00077
00078 self._encoder_errors_cnt = {}
00079
00080 self._drops_per_hour = DROPS_PER_HOUR
00081
00082 def create(self, params):
00083
00084 try:
00085 rospy.wait_for_service('pr2_etherCAT/halt_motors', 10)
00086 self._srv_ok = True
00087 except Exception, e:
00088 self._srv_ok = False
00089 rospy.logerr('Unable to find halt motors service. Unable to initialize ethercat listener')
00090 return False
00091
00092 if params.has_key('drops_per_hour'):
00093 self._drops_per_hour = params['drops_per_hour']
00094
00095 self._reset_motors = rospy.ServiceProxy('pr2_etherCAT/reset_motors', Empty)
00096
00097 self._halt_motors = rospy.ServiceProxy('pr2_etherCAT/halt_motors', Empty)
00098
00099
00100 self._halt_motors2 = rospy.ServiceProxy('pr2_etherCAT/halt_motors', Empty, persistent = True)
00101
00102 self._ecat_sub = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self._motors_cb)
00103
00104 self._cal_sub = rospy.Subscriber('calibrated', Bool, self._cal_cb)
00105
00106 self._diag_sub = rospy.Subscriber('/diagnostics', DiagnosticArray, self._diag_cb)
00107
00108 return True
00109
00110
00111 def halt(self):
00112 try:
00113 self._halt_motors2()
00114 self._halt_motors()
00115
00116 except Exception, e:
00117 try:
00118
00119 self._halt_motors2 = rospy.ServiceProxy('pr2_etherCAT/halt_motors', Empty, persistent = True)
00120 self._halt_motors2()
00121
00122 return
00123 except Exception, e:
00124 rospy.logerr('Unable to halt motors. pr2_etherCAT may have died')
00125
00126
00127 def reset(self):
00128 try:
00129 self._reset_motors()
00130 except Exception, e:
00131 rospy.logerr('Unable to reset motors. pr2_etherCAT may have died')
00132
00133 with self._mutex:
00134 self._dropped_times.clear()
00135 self._encoder_errors_detected = False
00136
00137 def _update_drops(self, stat, now):
00138 if stat.name != 'EtherCAT Master':
00139 raise Exception('Diagnostic status with invalid name! Expected \"EtherCAT Master\", got: %s',
00140 stat.name)
00141
00142 drops = -1
00143 lates = -1
00144 for kv in stat.values:
00145 if kv.key == 'Dropped Packets':
00146 drops = int(kv.value)
00147 elif kv.key == 'RX Late Packet':
00148 lates = int(kv.value)
00149
00150 if (drops == -1 or lates == -1):
00151 raise Exception("Diagnostics didn't contain data for dropped or late packets. Drops: %d. Lates: %d" %
00152 (drops, lates))
00153
00154
00155 new_net_drops = drops - lates
00156 if new_net_drops > self._net_drops:
00157 for i in range(min(new_net_drops - self._net_drops, self._drops_per_hour)):
00158 self._dropped_times.append(now)
00159
00160 self._net_drops = new_net_drops
00161
00162
00163 while len(self._dropped_times) > self._drops_per_hour:
00164 self._dropped_times.popleft()
00165
00166 self._diag_update_time = rospy.get_time()
00167
00168 def _update_encoder_errors(self, stat):
00169 """
00170 Check for encoder errors for every motor.
00171
00172 Updates cache of encoder errors. Reports encoder error detected if
00173 it can't find the number of encoder errors, or if encoder errors count
00174 increases.
00175 """
00176 if not stat.name.startswith('EtherCAT Device ('):
00177 raise Exception("Invalid diagnostic status name: %s" % stat.name)
00178
00179
00180 if stat.name.find('led_projector') > -1:
00181 return
00182
00183 last_errors = self._encoder_errors_cnt.get(stat.name, 0)
00184
00185 curr_errors = -1
00186 for kv in stat.values:
00187 if kv.key == ENCODER_ERRORS_FIELD:
00188 curr_errors = int(kv.value)
00189
00190 if curr_errors < 0:
00191 rospy.logerr('Unable to find encoder errors for motor %s', stat.name)
00192 self._encoder_errors_detected = True
00193 return
00194
00195 self._last_encoder_errors_update = rospy.get_time()
00196
00197 if curr_errors > last_errors:
00198 self._encoder_errors_detected = True
00199
00200
00201 self._encoder_errors_cnt[stat.name] = curr_errors
00202
00203
00204
00205 def _diag_cb(self, msg):
00206 with self._mutex:
00207 now = msg.header.stamp.to_sec()
00208 for stat in msg.status:
00209 if stat.name == 'EtherCAT Master':
00210 self._update_drops(stat, now)
00211 if stat.name.startswith('EtherCAT Device ('):
00212 self._update_encoder_errors(stat)
00213
00214
00215 def _cal_cb(self, msg):
00216 with self._mutex:
00217 self._cal = msg.data
00218
00219 def _motors_cb(self, msg):
00220 with self._mutex:
00221 self._ok = not msg.data
00222 self._update_time = rospy.get_time()
00223
00224 def _is_dropping_pkts(self):
00225 """
00226 Check if we're dropping packets.
00227 A drop is true if we've had more than 10 dropped packets in last hour.
00228
00229 @return bool : True if dropping packets
00230 """
00231 now = rospy.get_time()
00232
00233 if len(self._dropped_times) < self._drops_per_hour:
00234 return False
00235
00236 return abs(now - self._dropped_times[0]) < 3600
00237
00238
00239 def check_ok(self):
00240 msg = ''
00241 stat = 0
00242 with self._mutex:
00243 if not self._cal:
00244 stat = 1
00245 msg = 'Uncalibrated'
00246
00247 if not self._ok:
00248 stat = 2
00249 msg = 'Motors Halted'
00250
00251
00252 if self._is_dropping_pkts():
00253 stat = 2
00254 msg = 'Dropping Packets'
00255
00256
00257 if self._encoder_errors_detected:
00258 stat = 2
00259 msg = 'Encoder Errors'
00260
00261 if rospy.get_time() - self._last_encoder_errors_update > 3.0:
00262 stat = 3
00263 msg = 'No MCB Encoder Status'
00264
00265 if rospy.get_time() - self._diag_update_time > 3.0:
00266 stat = 3
00267 msg = 'No MCB Diagnostics'
00268
00269 if rospy.get_time() - self._update_time > 3.0:
00270 stat = 3
00271 msg = 'EtherCAT Stale'
00272 if self._update_time == -1:
00273 msg = 'No EtherCAT Data'
00274
00275 return stat, msg, []
00276