Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 PKG = 'iss_taskboard_gazebo'
00012 NAME = 'test_toggle_a04'
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_a04_state = 'DOWN'
00033 toggle_a04_led_top_state = 'OFF'
00034 toggle_a04_led_bottom_state = 'OFF'
00035
00036
00037 def onMessage(message):
00038 global toggle_a04_state
00039 if toggle_a04_state != message.A04_TOGGLE:
00040 rospy.loginfo("Toggle A03: " + message.A04_TOGGLE)
00041 toggle_a04_state = message.A04_TOGGLE
00042
00043 global toggle_a04_led_top_state
00044 if toggle_a04_led_top_state != message.A04_LED_TOP:
00045 rospy.loginfo("Toggle A04 TOP LED: " + message.A04_LED_TOP)
00046 toggle_a04_led_top_state = message.A04_LED_TOP
00047
00048 global toggle_a04_led_bottom_state
00049 if toggle_a04_led_bottom_state != message.A04_LED_BOTTOM:
00050 rospy.loginfo("Toggle A04 BOTTOM LED: " + message.A04_LED_BOTTOM)
00051 toggle_a04_led_bottom_state = message.A04_LED_BOTTOM
00052
00053
00054 class ToggleA04Test(unittest.TestCase):
00055
00056 def __init__(self, *args):
00057 super(ToggleA04Test, self).__init__(*args)
00058
00059 def test_toggle_a04(self):
00060 rospy.init_node(NAME, anonymous=True)
00061
00062 rospy.Subscriber(TOPIC_NAME, TaskboardPanelA, onMessage)
00063
00064 rospy.loginfo("Waiting for gazebo ...")
00065 rospy.wait_for_service(SERVICE_MANIPULATE_POWER_COVER)
00066 rospy.wait_for_service(SERVICE_MANIPULATE_POWER_SWITCH)
00067 rospy.wait_for_service(SERVICE_MANIPULATE_SAFE_TOGGLE)
00068 wait(GUI_WAIT_TIME)
00069 rospy.loginfo("Testing has started")
00070
00071 try:
00072 manipulatePowerCover = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_COVER, ManipulatePowerCover)
00073 manipulatePowerSwitch = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_SWITCH, ManipulatePowerSwitch)
00074 manipulateSafeToggle = rospy.ServiceProxy(SERVICE_MANIPULATE_SAFE_TOGGLE, ManipulateSafeToggle)
00075
00076
00077
00078 manipulatePowerCover(deg2rad(50), 1)
00079 wait()
00080
00081 manipulatePowerSwitch(deg2rad(30), 1)
00082 wait()
00083
00084
00085
00086 manipulateSafeToggle(1, 1, deg2rad(40), 1)
00087 wait()
00088 assert toggle_a04_state == 'DOWN'
00089 assert toggle_a04_led_top_state == 'OFF'
00090 assert toggle_a04_led_bottom_state == 'ON'
00091 rospy.loginfo("Case 1 Done.")
00092
00093
00094 manipulateSafeToggle(1, 0, 10, 2)
00095 manipulateSafeToggle(1, 1, deg2rad(40), 1)
00096 wait(4)
00097 assert toggle_a04_state == 'UP'
00098 assert toggle_a04_led_top_state == 'ON'
00099 assert toggle_a04_led_bottom_state == 'OFF'
00100 rospy.loginfo("Case 2 Done.")
00101
00102
00103 manipulateSafeToggle(1, 0, 10, 2)
00104 manipulateSafeToggle(1, 1, deg2rad(-26), 1)
00105 wait(4)
00106 assert toggle_a04_state == 'CENTER'
00107 assert toggle_a04_led_top_state == 'OFF'
00108 assert toggle_a04_led_bottom_state == 'OFF'
00109 rospy.loginfo("Case 3 Done.")
00110
00111 except rospy.ServiceException, e:
00112 print "Service call failed: %s" % e
00113
00114 time.sleep(3.0)
00115
00116 if __name__ == '__main__':
00117 print "Waiting for test to start at time "
00118 rostest.run(PKG, 'test_toggle_a04', ToggleA04Test)