Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest("pr2_power_board")
00003
00004 import sys
00005 import os
00006 import string
00007
00008 import rospy
00009 from pr2_power_board.srv import *
00010 from pr2_msgs.msg import PowerBoardState
00011
00012 current_state = PowerBoardState()
00013
00014 def callback(data):
00015 global current_state
00016 current_state = data
00017
00018
00019 if __name__ == "__main__":
00020
00021
00022 rospy.wait_for_service('power_board/control', 5)
00023
00024 rospy.init_node('test_power')
00025
00026
00027 control = rospy.ServiceProxy('power_board/control', PowerBoardCommand)
00028 state = rospy.Subscriber("/power_board/state", PowerBoardState, callback)
00029
00030 pause_on_fail = 0
00031 breaker_number = 1
00032 other_breaker1 = 0
00033 other_breaker2 = 2
00034 command_list = ["start", "stop"]
00035 serial = 0
00036 flags = 0
00037 try_count = 1000
00038 delay = rospy.Duration(3.0)
00039 fail_count = 0
00040 last_fail_count = 0
00041
00042 while (try_count > 0):
00043 try:
00044 command = command_list[ try_count % len(command_list) ]
00045 print "%d: Requesting %d to %s"%(try_count, breaker_number, command)
00046
00047
00048 resp1 = control(serial, breaker_number, command, flags)
00049 print " Response: ", resp1
00050 if resp1.retval != 0:
00051 fail_count = fail_count + 1
00052 print "Response to control is bad"
00053
00054 rospy.sleep(delay)
00055 for x in current_state.circuit_state:
00056 print "Current State: %d" % ( x )
00057
00058 if (command == "start") and ( current_state.circuit_state[breaker_number] != PowerBoardState.STATE_ON ):
00059 fail_count = fail_count + 1
00060 print "FAIL: Test circuit did not turn ON"
00061
00062 if (command == "stop") and ( current_state.circuit_state[breaker_number] != PowerBoardState.STATE_STANDBY ):
00063 fail_count = fail_count + 1
00064 print "FAIL: Test circuit not in STANDBY"
00065
00066 if ( current_state.circuit_state[other_breaker1] != PowerBoardState.STATE_ON ) or ( current_state.circuit_state[other_breaker2] != PowerBoardState.STATE_ON ):
00067 fail_count = fail_count + 1
00068 print "FAIL: The other circuit is not ON"
00069
00070 except rospy.ServiceException, e:
00071 print "Service call failed: %s"%e
00072
00073
00074 try_count = try_count - 1
00075 if pause_on_fail and fail_count > last_fail_count:
00076 print "Pause on Fail"
00077 line = sys.stdin.readline()
00078 last_fail_count = fail_count
00079
00080 print "\n"
00081 print "Test complete: "
00082 print " Test breaker_number = %d" % (breaker_number)
00083 print " Fail Count = %d " % (fail_count)
00084 print "\n"
00085