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 = 'qualification'
00040
00041 import roslib
00042 roslib.load_manifest(PKG)
00043
00044 from ectools.msg import ecstats
00045 from pr2_self_test_msgs.srv import TestResult, TestResultRequest
00046
00047 import rospy
00048
00049 import threading
00050 import traceback
00051 from time import sleep
00052
00053 DURATION = 15
00054
00055 class ECStatsTest:
00056 def __init__(self):
00057 rospy.init_node('ecstats_test')
00058 self._ec_stats_sub = rospy.Subscriber('ecstats', ecstats, self._ecstats_cb)
00059 self._mutex = threading.Lock()
00060 self._start_time = rospy.get_time()
00061
00062 self._ok = False
00063 self._update_time = -1
00064
00065 self._has_link = False
00066 self._max_device_count = 0
00067 self._total_sent = 0
00068 self._interval_sent = 0
00069 self._total_dropped = 0
00070 self._interval_dropped = 0
00071 self._total_bandwidth = 0
00072 self._interval_bandwidth = 0
00073
00074 self._result_service = rospy.ServiceProxy('test_result', TestResult)
00075 self._data_sent = False
00076
00077 def send_results(self, r):
00078 if self._data_sent:
00079 return
00080 self._result_service.call(r)
00081 self._data_sent = True
00082
00083 def test_failed_service_call(self, exception = ''):
00084 r = TestResultRequest()
00085 r.result = TestResultRequest.RESULT_FAIL
00086 r.text_summary = 'Caught exception in ecstats test'
00087 r.html_result = '<p>%s</p>\n' % exception
00088 self.send_results(r)
00089
00090 def is_done(self):
00091 return rospy.get_time() - self._start_time > DURATION
00092
00093 def _ecstats_cb(self, msg):
00094 self._mutex.acquire()
00095 self._has_link = msg.has_link
00096 self._max_device_count = msg.max_device_count
00097 self._total_sent = msg.total_sent_packets
00098 self._interval_sent_packets = msg.interval_sent_packets
00099 self._total_dropped = msg.total_dropped_packets
00100 self._interval_dropped = msg.interval_dropped_packets
00101 self._total_bandwidth = msg.total_bandwidth_mbps
00102 self._interval_bandwidth = msg.interval_bandwidth_mbps
00103
00104 self._ok = self._total_dropped == 0 and self._has_link
00105
00106 self._mutex.release()
00107
00108 def process_results(self):
00109
00110 self._mutex.acquire()
00111 r = TestResultRequest()
00112 if self._ok:
00113 r.text_summary = 'Slip ring packet drop test: OK'
00114 else:
00115 r.text_summary = 'Slip ring packet drop test: FAIL'
00116
00117 html = ['<p>Slip ring packet drop test data.</p>']
00118 html.append('<table border="1" cellpadding="2" cellspacing="0">')
00119 html.append('<tr><td><b>Name</b></td><td><b>Value</b></td></tr>')
00120 html.append('<tr><td>Has Link?</td><td>%s</td></tr>' % (str(self._has_link)))
00121 html.append('<tr><td>Dropped Packets</td><td>%d</td></tr>' % (self._total_dropped))
00122 html.append('<tr><td>Total Sent</td><td>%d</td></tr>' % (self._total_sent))
00123 html.append('<tr><td>Max Device Count (should be 0)</td><td>%d</td></tr>' % (self._max_device_count))
00124 html.append('<tr><td>Duration</td><td>%d</td></tr>' % (DURATION))
00125 html.append('</table>')
00126
00127 r.html_result = '\n'.join(html)
00128
00129 if self._ok:
00130 r.result = TestResultRequest.RESULT_PASS
00131 else:
00132 r.result = TestResultRequest.RESULT_FAIL
00133
00134 self._mutex.release()
00135
00136 self.send_results(r)
00137
00138 if __name__ == '__main__':
00139 monitor = ECStatsTest()
00140 try:
00141 while not rospy.is_shutdown() and not monitor.is_done():
00142 sleep(0.5)
00143 monitor.process_results()
00144 rospy.spin()
00145 except KeyboardInterrupt:
00146 pass
00147 except:
00148 traceback.print_exc()
00149 monitor.test_failed_service_call(traceback.format_exc())
00150