test_power.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib; roslib.load_manifest("pr2_power_board")
3 
4 import sys
5 import os
6 import string
7 
8 import rospy
9 from pr2_power_board.srv import *
10 from pr2_msgs.msg import PowerBoardState
11 
12 current_state = PowerBoardState()
13 
14 def callback(data):
15  global current_state
16  current_state = data
17  #print "New State: %d" % ( current_state.circuit_state[1] )
18 
19 if __name__ == "__main__":
20 
21  # block until the add_two_ints service is available
22  rospy.wait_for_service('power_board/control', 5)
23 
24  rospy.init_node('test_power')
25 
26  # create a handle to the add_two_ints service
27  control = rospy.ServiceProxy('power_board/control', PowerBoardCommand)
28  state = rospy.Subscriber("/power_board/state", PowerBoardState, callback)
29 
30  pause_on_fail = 0
31  breaker_number = 1 # The breaker we are toggling
32  other_breaker1 = 0
33  other_breaker2 = 2
34  command_list = ["start", "stop"]
35  serial = 0
36  flags = 0
37  try_count = 1000
38  delay = rospy.Duration(3.0)
39  fail_count = 0
40  last_fail_count = 0
41 
42  while (try_count > 0):
43  try:
44  command = command_list[ try_count % len(command_list) ]
45  print("%d: Requesting %d to %s"%(try_count, breaker_number, command))
46 
47  # simplified style
48  resp1 = control(serial, breaker_number, command, flags)
49  print(" Response: ", resp1)
50  if resp1.retval != 0:
51  fail_count = fail_count + 1
52  print("Response to control is bad")
53 
54  rospy.sleep(delay)
55  for x in current_state.circuit_state:
56  print("Current State: %d" % ( x ))
57 
58  if (command == "start") and ( current_state.circuit_state[breaker_number] != PowerBoardState.STATE_ON ):
59  fail_count = fail_count + 1
60  print("FAIL: Test circuit did not turn ON")
61 
62  if (command == "stop") and ( current_state.circuit_state[breaker_number] != PowerBoardState.STATE_STANDBY ):
63  fail_count = fail_count + 1
64  print("FAIL: Test circuit not in STANDBY")
65 
66  if ( current_state.circuit_state[other_breaker1] != PowerBoardState.STATE_ON ) or ( current_state.circuit_state[other_breaker2] != PowerBoardState.STATE_ON ):
67  fail_count = fail_count + 1
68  print("FAIL: The other circuit is not ON")
69 
70  except rospy.ServiceException as e:
71  print("Service call failed: %s"%e)
72 
73 
74  try_count = try_count - 1
75  if pause_on_fail and fail_count > last_fail_count:
76  print("Pause on Fail")
77  line = sys.stdin.readline()
78  last_fail_count = fail_count
79 
80  print("\n")
81  print("Test complete: ")
82  print(" Test breaker_number = %d" % (breaker_number))
83  print(" Fail Count = %d " % (fail_count))
84  print("\n")
85 
msg
test_power.callback
def callback(data)
Definition: test_power.py:14
srv
test_power.control
control
Definition: test_power.py:27


pr2_power_board
Author(s): Curt Meyers, Blaise Gassend
autogenerated on Tue Mar 7 2023 03:19:35