test_power.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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   #print "New State: %d" % ( current_state.circuit_state[1] )
00018 
00019 if __name__ == "__main__":
00020 
00021   # block until the add_two_ints service is available
00022   rospy.wait_for_service('power_board/control', 5)
00023 
00024   rospy.init_node('test_power')
00025 
00026   # create a handle to the add_two_ints service
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  # The breaker we are toggling
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         # simplified style
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 


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Thu Jun 6 2019 21:11:02