test_toggle_a03_a05.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ##
00003 # Copyright (C) 2013 TopCoder Inc., All Rights Reserved.
00004 #
00005 # @author KennyAlive
00006 # @version 1.0
00007 #
00008 
00009 ## Tests for safe toggles A03/A05
00010 
00011 PKG = 'gazebo_taskboard'
00012 NAME = 'test_toggle_a03_a05'
00013 
00014 import math
00015 import roslib
00016 roslib.load_manifest(PKG)
00017 
00018 import sys, unittest
00019 import os, os.path, threading, time
00020 import rospy, rostest
00021 
00022 from std_msgs.msg import String
00023 from helper import *
00024 from gazebo_taskboard.msg import *
00025 from gazebo_taskboard.srv import *
00026 
00027 toggle_a03_state = 'DOWN'
00028 toggle_a05_state = 'DOWN'
00029 toggle_a03_led_state = 'OFF'
00030 toggle_a05_led_state = 'OFF'
00031 
00032 def onMessage(message):
00033     global toggle_a03_state
00034     if toggle_a03_state != message.A03_TOGGLE:
00035         rospy.loginfo("Toggle A03: " + message.A03_TOGGLE)
00036     toggle_a03_state = message.A03_TOGGLE
00037 
00038     global toggle_a05_state
00039     if toggle_a05_state != message.A05_TOGGLE:
00040         rospy.loginfo("Toggle A05: " + message.A05_TOGGLE)
00041     toggle_a05_state = message.A05_TOGGLE
00042 
00043     global toggle_a03_led_state
00044     if toggle_a03_led_state != message.A03_LED:
00045         rospy.loginfo("Toggle A03 LED: " + message.A03_LED)
00046     toggle_a03_led_state = message.A03_LED
00047 
00048     global toggle_a05_led_state
00049     if toggle_a05_led_state != message.A05_LED:
00050         rospy.loginfo("Toggle A05 LED: " + message.A05_LED)
00051     toggle_a05_led_state = message.A05_LED
00052 
00053 
00054 class ToggleA03A05Test(unittest.TestCase):
00055     def __init__(self, *args):
00056        super(ToggleA03A05Test, self).__init__(*args)
00057 
00058     def test_toggle_a03_a05(self):
00059         rospy.init_node(NAME, anonymous=True)
00060 
00061         rospy.Subscriber(TOPIC_NAME, TaskboardPanelA, onMessage)
00062 
00063         rospy.loginfo("Waiting for gazebo ...")
00064         rospy.wait_for_service(SERVICE_MANIPULATE_POWER_COVER)
00065         rospy.wait_for_service(SERVICE_MANIPULATE_POWER_SWITCH)
00066         rospy.wait_for_service(SERVICE_MANIPULATE_SAFE_TOGGLE)
00067         wait(GUI_WAIT_TIME)
00068         rospy.loginfo("Testing has started")
00069 
00070         try:
00071             manipulatePowerCover = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_COVER, ManipulatePowerCover)
00072             manipulatePowerSwitch = rospy.ServiceProxy(SERVICE_MANIPULATE_POWER_SWITCH, ManipulatePowerSwitch)
00073             manipulateSafeToggle = rospy.ServiceProxy(SERVICE_MANIPULATE_SAFE_TOGGLE, ManipulateSafeToggle)
00074             
00075             # !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00076             # Open power cover
00077             manipulatePowerCover(deg2rad(50), 1)
00078             wait()
00079             # Turn on power
00080             manipulatePowerSwitch(deg2rad(30), 1)
00081             wait()
00082             ## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00083 
00084             # Try to move up A03 toggle without pulling out. We should fail.
00085             manipulateSafeToggle(0, 1, deg2rad(40), 1)
00086             wait()
00087             assert toggle_a03_state == 'DOWN'
00088             assert toggle_a03_led_state == 'OFF'
00089             rospy.loginfo("Case 1 Done.")
00090             
00091             # Similar with A05 but at first pull out it. The operation should be successful.
00092             manipulateSafeToggle(2, 0, 4, 2)
00093             manipulateSafeToggle(2, 1, deg2rad(40), 1)
00094             wait(4)
00095             assert toggle_a05_state == 'UP'
00096             assert toggle_a05_led_state == 'ON'
00097             rospy.loginfo("Case 2 Done.")
00098 
00099             # Pull out A03 (OUT state) then release (again DOWN)
00100             manipulateSafeToggle(0, 0, 4, 5)
00101             wait(1)
00102             assert toggle_a03_state == 'OUT'
00103             assert toggle_a03_led_state == 'OFF'
00104             wait(5)
00105             assert toggle_a03_state == 'DOWN'
00106             assert toggle_a03_led_state == 'OFF'
00107             rospy.loginfo("Case 3 Done.")
00108 
00109              # Pull out A03 and move on small angle up (not sufficient for UP state)
00110             manipulateSafeToggle(0, 0, 4, 2)
00111             manipulateSafeToggle(0, 1, deg2rad(15), 1)
00112             wait(4)
00113             assert toggle_a03_state == 'DOWN'
00114             assert toggle_a03_led_state == 'OFF'
00115             rospy.loginfo("Case 4 Done.")
00116             
00117             # Finally put A03 to UP state
00118             manipulateSafeToggle(0, 0, 4, 2)
00119             manipulateSafeToggle(0, 1, deg2rad(40), 1)
00120             wait(4)
00121             assert toggle_a03_state == 'UP'
00122             assert toggle_a03_led_state == 'ON'
00123             rospy.loginfo("Case 5 Done.")
00124             
00125             # Put A05 into OUT state and check it (in the upper position)
00126             manipulateSafeToggle(2, 0, 5, 5)
00127             wait(1)
00128             assert toggle_a05_state == 'OUT'
00129             assert toggle_a05_led_state == 'OFF'
00130             rospy.loginfo("Case 6 Done.")
00131             
00132             # Move A05 on small angle not sufficient for DOWN position
00133             manipulateSafeToggle(2, 0, 4, 2)
00134             manipulateSafeToggle(2, 1, deg2rad(-15), 1)
00135             wait(4)
00136             assert toggle_a05_state == 'UP'
00137             assert toggle_a05_led_state == 'ON'
00138             rospy.loginfo("Case 7 Done.")
00139             
00140             # Move A05 to down position
00141             manipulateSafeToggle(2, 0, 4, 2)
00142             manipulateSafeToggle(2, 1, deg2rad(-40), 1)
00143             wait(4)
00144             assert toggle_a05_state == 'DOWN'
00145             assert toggle_a05_led_state == 'OFF'
00146             rospy.loginfo("Case 8 Done.")
00147             
00148             ## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00149             # Turn off the power
00150             manipulatePowerSwitch(deg2rad(-30), 1)
00151             wait()
00152             # Close power cover
00153             manipulatePowerCover(deg2rad(-50), 1)
00154             wait()
00155             ## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
00156             
00157             # Check leds
00158             assert toggle_a03_led_state == 'OFF'
00159             assert toggle_a05_led_state == 'OFF'
00160             rospy.loginfo("Case 9 Done.")
00161 
00162         except rospy.ServiceException, e:
00163             print "Service call failed: %s"%e
00164 
00165 if __name__ == '__main__':
00166     print "Waiting for test to start at time "
00167     rostest.run(PKG, 'test_toggle_a03_a05', ToggleA03A05Test)


gazebo_taskboard
Author(s): TCSASSEMBLER
autogenerated on Thu Jan 2 2014 11:32:30