$search
00001 #!/usr/bin/env python 00002 00003 PKG="cob_script_server" 00004 import roslib; roslib.load_manifest(PKG) 00005 00006 import sys 00007 import unittest 00008 00009 import actionlib 00010 00011 from simple_script_server import * 00012 00013 ## This test checks the correct call to commands from the cob_script_server. This does not cover the execution of the commands (this shoud be checked in the package where the calls refer to). 00014 class TestActionInterface(unittest.TestCase): 00015 def __init__(self, *args): 00016 super(TestActionInterface, self).__init__(*args) 00017 rospy.init_node('test_action_interface') 00018 self.cb_executed = False 00019 self.cb_move_executed = False 00020 self.component_name = "arm" # testing for component arm 00021 00022 # test trigger commands 00023 def test_init(self): 00024 goal = ScriptGoal() 00025 goal.function_name = "init" 00026 goal.component_name = "arm" 00027 self.script_action_trigger(goal) 00028 00029 def test_stop(self): 00030 goal = ScriptGoal() 00031 goal.function_name = "stop" 00032 goal.component_name = "arm" 00033 self.script_action_trigger(goal) 00034 00035 def test_recover(self): 00036 goal = ScriptGoal() 00037 goal.function_name = "recover" 00038 goal.component_name = "arm" 00039 self.script_action_trigger(goal) 00040 00041 def script_action_trigger(self,goal): 00042 rospy.Service("/" + goal.component_name + "_controller/" + goal.function_name, Trigger, self.cb) 00043 self.cb_executed = False 00044 00045 client = actionlib.SimpleActionClient('/script_server', ScriptAction) 00046 00047 if not client.wait_for_server(rospy.Duration(10)): 00048 self.fail('Action server not ready') 00049 client.send_goal(goal) 00050 if not client.wait_for_result(rospy.Duration(10)): 00051 self.fail('Action didnt give a result before timeout') 00052 00053 if not self.cb_executed: 00054 self.fail('Service Server not called. script server error_code: ' + str(client.get_result().error_code)) 00055 00056 def cb(self,req): 00057 self.cb_executed = True 00058 res = TriggerResponse() 00059 res.success.data = True 00060 return res 00061 00062 # test move base commands 00063 # def test_move_base(self): 00064 # goal = ScriptGoal() 00065 # goal.function_name = "move" 00066 # goal.component_name = "base" 00067 # goal.parameter_name = [0,0,0] 00068 # self.script_action_move_base(goal) 00069 00070 # def test_move_base_omni(self): #FIXME fails because client is already in DONE state (mode="" and mode="omni" is the same) 00071 # goal = ScriptGoal() 00072 # goal.function_name = "move" 00073 # goal.component_name = "base" 00074 # goal.parameter_name = "home" 00075 # goal.mode = "omni" 00076 # self.script_action_move_base(goal) 00077 00078 # def test_move_base_diff(self): 00079 # goal = ScriptGoal() 00080 # goal.function_name = "move" 00081 # goal.component_name = "base" 00082 # goal.parameter_name = [0,0,0] 00083 # goal.mode = "diff" 00084 # self.script_action_move_base(goal) 00085 00086 # def test_move_base_linear(self): 00087 # goal = ScriptGoal() 00088 # goal.function_name = "move" 00089 # goal.component_name = "base" 00090 # goal.parameter_name = [0,0,0] 00091 # goal.mode = "linear" 00092 # self.script_action_move_base(goal) 00093 00094 def script_action_move_base(self,goal): 00095 if goal.mode == None or goal.mode == "" or goal.mode == "omni": 00096 as_name = "/move_base" 00097 else: 00098 as_name = "/move_base_" + goal.mode 00099 self.as_server = actionlib.SimpleActionServer(as_name, MoveBaseAction, execute_cb=self.base_cb, auto_start=False) 00100 self.as_server.start() 00101 00102 client = actionlib.SimpleActionClient('/script_server', ScriptAction) 00103 00104 self.cb_move_executed = False 00105 if not client.wait_for_server(rospy.Duration(10)): 00106 self.fail('Action server not ready') 00107 client.send_goal(goal) 00108 client.wait_for_result(rospy.Duration(10)) 00109 #if not client.wait_for_result(rospy.Duration(10)): 00110 # self.fail('Action didnt give a result before timeout') 00111 00112 #if not self.cb_executed: 00113 # self.fail('Action Server not called. script server error_code: ' + str(client.get_result().error_code)) 00114 00115 def base_cb(self, goal): 00116 self.cb_move_executed = True 00117 result = MoveBaseResult() 00118 self.as_server.set_succeeded(result) 00119 00120 # test move trajectory commands 00121 #TODO 00122 00123 if __name__ == '__main__': 00124 import rostest 00125 rostest.rosrun(PKG, 'action_interface', TestActionInterface)