life_test.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import roslib
00003 roslib.load_manifest('cob_hardware_test')
00004 import rospy
00005 import sys
00006 
00007 from simple_script_server import *
00008 sss = simple_script_server()
00009 
00010 c1 = [["torso","shake"],
00011      ["tray","up"],
00012      ["arm","pregrasp"],
00013      ["sdh","cylopen"],
00014      ["head","back"]]
00015 
00016 # c2 should be "navigationable" configuration
00017 c2 = [["torso","nod"],
00018      ["tray","down"],
00019      ["arm","folded"],
00020      ["sdh","cylclosed"],
00021      ["head","front"]]
00022 
00023 def init(config_list):
00024         for config in config_list:
00025                 handle = sss.init(config[0])
00026                 if handle.get_error_code() > 0:
00027                         sss.set_light("red")
00028                         raise NameError('could not initialize ' + config[0])
00029 
00030 def recover(config_list):
00031         for config in config_list:
00032                 handle = sss.recover(config[0])
00033                 if handle.get_error_code() > 0:
00034                         sss.set_light("red")
00035                         raise NameError('could not recover ' + config[0])
00036 
00037 def move_single_component(config):
00038         # move
00039         handle = sss.move(config[0],config[1])
00040         
00041         # check result
00042         if handle.get_state() != 3:
00043                 sss.set_light("red")
00044                 raise NameError('something went wrong with ' + str(config[0]) + '. Action state = ' + str(handle.get_state()))
00045                 #sys.exit()
00046 
00047 def move_all_component(config_list):
00048         handles = []
00049 
00050         # move all components non-blocking
00051         for config in config_list:
00052                 handles.append(sss.move(config[0],config[1],False))
00053                 
00054         # wait for all components
00055         for handle in handles:
00056                 handle.wait()
00057         
00058         # check result
00059         for handle in handles:
00060                 if handle.get_state() != 3:
00061                         sss.set_light("red")
00062                         raise NameError('something went wrong in move all. Action state = ' + str(handle.get_state()))
00063                         #sys.exit()
00064 
00065 if __name__ == "__main__":
00066         rospy.init_node("life_test")
00067 
00068         init(c1)
00069 
00070         # do life test
00071         counter = 0
00072         while not rospy.is_shutdown():
00073                 counter += 1
00074                 try:
00075                         sss.say(["This is round " + str(counter)])
00076                         print "================================"
00077                         print "=== moving single components ==="
00078                         print "================================"
00079                         sss.set_light("yellow")
00080                         for config in c1:
00081                                 move_single_component(config)
00082                         for config in c2:
00083                                 move_single_component(config)
00084                         sss.set_light("green")
00085 
00086                         print "*****************************"
00087                         print "*** moving all components ***"
00088                         print "*****************************"
00089                         sss.set_light("yellow")
00090                         move_all_component(c1)
00091                         move_all_component(c2)
00092                         sss.set_light("green")
00093                 except NameError, e:
00094                         print "Error: %s"%e
00095                         print "Successfull round(s): <<" + str(counter) + ">>"
00096                         print "press <return> to continue"
00097                         sss.wait_for_input()
00098                         counter = 0
00099                         recover(c1)


cob_hardware_test
Author(s): Florian Weisshardt
autogenerated on Mon Mar 17 2014 10:13:52