00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 PKG = 'iss_taskboard_gazebo'
00012 NAME = 'test_toggle_a03_a05'
00013
00014 import math
00015 import roslib
00016 roslib.load_manifest(PKG)
00017
00018 import sys
00019 import unittest
00020 import os
00021 import os.path
00022 import threading
00023 import time
00024 import rospy
00025 import rostest
00026
00027 from std_msgs.msg import String
00028 from helper import *
00029 from iss_taskboard_gazebo.msg import *
00030 from iss_taskboard_gazebo.srv import *
00031
00032 toggle_a03_state = 'DOWN'
00033 toggle_a05_state = 'DOWN'
00034 toggle_a03_led_state = 'OFF'
00035 toggle_a05_led_state = 'OFF'
00036
00037
00038 def onMessage(message):
00039 global toggle_a03_state
00040 if toggle_a03_state != message.A03_TOGGLE:
00041 rospy.loginfo("Toggle A03: " + message.A03_TOGGLE)
00042 toggle_a03_state = message.A03_TOGGLE
00043
00044 global toggle_a05_state
00045 if toggle_a05_state != message.A05_TOGGLE:
00046 rospy.loginfo("Toggle A05: " + message.A05_TOGGLE)
00047 toggle_a05_state = message.A05_TOGGLE
00048
00049 global toggle_a03_led_state
00050 if toggle_a03_led_state != message.A03_LED:
00051 rospy.loginfo("Toggle A03 LED: " + message.A03_LED)
00052 toggle_a03_led_state = message.A03_LED
00053
00054 global toggle_a05_led_state
00055 if toggle_a05_led_state != message.A05_LED:
00056 rospy.loginfo("Toggle A05 LED: " + message.A05_LED)
00057 toggle_a05_led_state = message.A05_LED
00058
00059
00060 class ToggleA03A05Test(unittest.TestCase):
00061
00062 def __init__(self, *args):
00063 super(ToggleA03A05Test, self).__init__(*args)
00064
00065 def test_toggle_a03_a05(self):
00066 rospy.init_node(NAME, anonymous=True)
00067
00068 rospy.Subscriber(TOPIC_NAME, TaskboardPanelA, onMessage)
00069
00070 rospy.loginfo("Waiting for gazebo ...")
00071 rospy.wait_for_service(SERVICE_MANIPULATE_POWER_COVER)
00072 rospy.wait_for_service(SERVICE_MANIPULATE_POWER_SWITCH)
00073 rospy.wait_for_service(SERVICE_MANIPULATE_SAFE_TOGGLE)
00074 wait(GUI_WAIT_TIME)
00075 rospy.loginfo("Testing has started")
00076
00077 try:
00078 manipulatePowerCover = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_COVER, ManipulatePowerCover)
00079 manipulatePowerSwitch = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_SWITCH, ManipulatePowerSwitch)
00080 manipulateSafeToggle = rospy.ServiceProxy(SERVICE_MANIPULATE_SAFE_TOGGLE, ManipulateSafeToggle)
00081
00082
00083
00084 manipulatePowerCover(deg2rad(50), 1)
00085 wait()
00086
00087 manipulatePowerSwitch(deg2rad(30), 1)
00088 wait()
00089
00090
00091
00092 manipulateSafeToggle(0, 1, deg2rad(40), 1)
00093 wait()
00094 assert toggle_a03_state == 'DOWN'
00095 assert toggle_a03_led_state == 'OFF'
00096 rospy.loginfo("Case 1 Done.")
00097
00098
00099 manipulateSafeToggle(2, 0, 4, 2)
00100 manipulateSafeToggle(2, 1, deg2rad(40), 1)
00101 wait(4)
00102 assert toggle_a05_state == 'UP'
00103 assert toggle_a05_led_state == 'ON'
00104 rospy.loginfo("Case 2 Done.")
00105
00106
00107 manipulateSafeToggle(0, 0, 4, 5)
00108 wait(1)
00109 assert toggle_a03_state == 'OUT'
00110 assert toggle_a03_led_state == 'OFF'
00111 wait(5)
00112 assert toggle_a03_state == 'DOWN'
00113 assert toggle_a03_led_state == 'OFF'
00114 rospy.loginfo("Case 3 Done.")
00115
00116
00117 manipulateSafeToggle(0, 0, 4, 2)
00118 manipulateSafeToggle(0, 1, deg2rad(15), 1)
00119 wait(4)
00120 assert toggle_a03_state == 'DOWN'
00121 assert toggle_a03_led_state == 'OFF'
00122 rospy.loginfo("Case 4 Done.")
00123
00124
00125 manipulateSafeToggle(0, 0, 4, 2)
00126 manipulateSafeToggle(0, 1, deg2rad(40), 1)
00127 wait(4)
00128 assert toggle_a03_state == 'UP'
00129 assert toggle_a03_led_state == 'ON'
00130 rospy.loginfo("Case 5 Done.")
00131
00132
00133 manipulateSafeToggle(2, 0, 5, 5)
00134 wait(1)
00135 assert toggle_a05_state == 'OUT'
00136 assert toggle_a05_led_state == 'OFF'
00137 rospy.loginfo("Case 6 Done.")
00138
00139
00140 manipulateSafeToggle(2, 0, 4, 2)
00141 manipulateSafeToggle(2, 1, deg2rad(-15), 1)
00142 wait(4)
00143 assert toggle_a05_state == 'UP'
00144 assert toggle_a05_led_state == 'ON'
00145 rospy.loginfo("Case 7 Done.")
00146
00147
00148 manipulateSafeToggle(2, 0, 4, 2)
00149 manipulateSafeToggle(2, 1, deg2rad(-40), 1)
00150 wait(4)
00151 assert toggle_a05_state == 'DOWN'
00152 assert toggle_a05_led_state == 'OFF'
00153 rospy.loginfo("Case 8 Done.")
00154
00155
00156
00157 manipulatePowerSwitch(deg2rad(-30), 1)
00158 wait()
00159
00160 manipulatePowerCover(deg2rad(-50), 1)
00161 wait()
00162
00163
00164
00165 assert toggle_a03_led_state == 'OFF'
00166 assert toggle_a05_led_state == 'OFF'
00167 rospy.loginfo("Case 9 Done.")
00168
00169 except rospy.ServiceException, e:
00170 print "Service call failed: %s" % e
00171
00172 if __name__ == '__main__':
00173 print "Waiting for test to start at time "
00174 rostest.run(PKG, 'test_toggle_a03_a05', ToggleA03A05Test)