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
00043 roslib.load_manifest(PKG)
00044
00045 from ectools.msg import ecstats
00046 from std_srvs.srv import *
00047 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
00048
00049 import rospy
00050
00051 import threading
00052
00053 from pr2_hw_listener import PR2HWListenerBase
00054
00055 class ECStatsListener(PR2HWListenerBase):
00056 def __init__(self):
00057 self._ec_stats_sub = rospy.Subscriber('ecstats', ecstats, self._ecstats_cb)
00058 self._mutex = threading.Lock()
00059
00060 self._ok = True
00061 self._update_time = -1
00062
00063 self._halt_motors = rospy.ServiceProxy('pr2_etherCAT/halt_motors', Empty)
00064
00065 self._drop_count_at_reset = 0
00066 self._time_at_last_reset = rospy.get_time()
00067 self._reset_count = 0
00068
00069 self._has_link = False
00070 self._max_device_count = 0
00071 self._total_sent = 0
00072 self._interval_sent = 0
00073 self._total_dropped = 0
00074 self._interval_dropped = 0
00075 self._total_late = 0
00076 self._interval_late = 0
00077 self._total_bandwidth = 0
00078 self._interval_bandwidth = 0
00079
00080 self._lost_link_count_since_reset = 0
00081 self._lost_link_count = 0
00082
00083 def reset(self):
00084 with self._mutex:
00085 self._time_at_last_reset = rospy.get_time()
00086 self._drop_count_at_reset = self._total_dropped
00087 self._ok = True
00088 self._reset_count += 1
00089 self._lost_link_count_since_reset = 0
00090
00091 def _ecstats_cb(self, msg):
00092 with self._mutex:
00093 self._has_link = msg.has_link
00094 self._max_device_count = msg.max_device_count
00095 self._total_sent = msg.total_sent_packets
00096 self._interval_sent_packets = msg.interval_sent_packets
00097 self._total_dropped = msg.total_dropped_packets
00098 self._interval_dropped = msg.interval_dropped_packets
00099 self._total_late = msg.total_late_packets
00100 self._interval_late = msg.interval_late_packets
00101 self._total_bandwidth = msg.total_bandwidth_mbps
00102 self._interval_bandwidth = msg.interval_bandwidth_mbps
00103
00104 if not self._has_link:
00105 self._lost_link_count += 1
00106 self._lost_link_count_since_reset += 1
00107
00108 was_ok = self._ok
00109 self._ok = self._ok and self._has_link
00110
00111 if was_ok and not self._ok:
00112 try:
00113
00114 rospy.logerr('Should\'ve halted motors, went down')
00115 except Exception, e:
00116 rospy.logerr('Attempted to halt motors after dropped packets, failed')
00117
00118 self._update_time = rospy.get_time()
00119
00120
00121 def check_ok(self):
00122 with self._mutex:
00123 stat = 0 if self._ok else 2
00124 msg = '' if self._ok else 'Dropped Packets'
00125
00126 if rospy.get_time() - self._update_time > 3:
00127 stat = 3
00128 msg = 'Packet Data Stale'
00129 if self._update_time == -1:
00130 msg = 'No Packet Data'
00131
00132 diag = DiagnosticStatus()
00133 diag.name = 'EC Stats Packet Data'
00134 diag.level = stat
00135 diag.message = msg
00136 if diag.level == 0:
00137 diag.message = 'OK'
00138
00139 diag.values = [
00140 KeyValue(key='Has Link?', value=str(self._has_link)),
00141 KeyValue(key='Dropped Since Reset', value=str(self._total_dropped - self._drop_count_at_reset)),
00142 KeyValue(key='Total Drops', value=str(self._total_dropped)),
00143 KeyValue(key='Lost Link Count', value=str(self._lost_link_count)),
00144 KeyValue(key='Lost Links Since Reset', value=str(self._lost_link_count_since_reset)),
00145 KeyValue(key='Number of Resets', value=str(self._reset_count)),
00146 KeyValue(key='Time Since Last Reset', value=str(rospy.get_time() - self._time_at_last_reset)),
00147 KeyValue(key='Drops at Last Reset', value=str(self._drop_count_at_reset)),
00148 KeyValue(key='Max Device Count', value=str(self._max_device_count)),
00149 KeyValue(key='Interval Drops', value=str(self._interval_dropped)),
00150 KeyValue(key='Total Late Packets', value=str(self._total_late)),
00151 KeyValue(key='Interval Late Packets', value=str(self._interval_late)),
00152 KeyValue(key='Total Sent Packets', value=str(self._total_sent)),
00153 KeyValue(key='Interval Sent Packets', value=str(self._interval_sent)),
00154 KeyValue(key='Total Bandwidth', value=str(self._total_bandwidth)),
00155 KeyValue(key='Interval Bandwidth', value=str(self._interval_bandwidth))
00156 ]
00157
00158 return stat, msg, [ diag ]
00159