$search
00001 #!/usr/bin/env python 00002 import roslib 00003 roslib.load_manifest('cob_hardware_test') 00004 import time 00005 import sys 00006 import unittest 00007 import rospy 00008 import rostest 00009 00010 from cob_hardware_test.srv import * 00011 from std_msgs.msg import String 00012 from simple_script_server import * 00013 from pr2_controllers_msgs.msg import * 00014 00015 00016 def dialog_client(dialog_type, message): 00017 #dialog type: 0=confirm 1=question 00018 rospy.wait_for_service('dialog') 00019 try: 00020 dialog = rospy.ServiceProxy('dialog', Dialog) 00021 resp1 = dialog(dialog_type, message) 00022 return resp1.answer 00023 except rospy.ServiceException, e: 00024 print "Service call failed: %s" % e 00025 00026 class HardwareTest(unittest.TestCase): 00027 def __init__(self, *args): 00028 00029 super(HardwareTest, self).__init__(*args) 00030 rospy.init_node('test_hardware_test') 00031 torso_joint_states = [] 00032 self.message_received = False 00033 self.sss = simple_script_server() 00034 00035 def test_play(self): 00036 dialog_client(0, 'Listen up for the sound' ) 00037 rospy.set_param("script_server/sound/language","de") 00038 handle = self.sss.play("grasp_tutorial_01") 00039 self.assertEqual(handle.get_state(), 3) 00040 self.assertTrue(dialog_client(1, 'Did you hear some audio file?')) 00041 00042 def test_say(self): 00043 dialog_client(0, 'Listen up for the sound' ) 00044 handle = self.sss.say(["Hello"]) 00045 self.assertEqual(handle.get_state(), 3) 00046 if handle.get_error_code() != 0: 00047 error_msg = 'Could say something' 00048 self.fail(error_msg + "; errorCode: " + str(handle.get_error_code())) 00049 self.assertTrue(dialog_client(1, 'Did you hear <<Hello>>?')) 00050 00051 if __name__ == '__main__': 00052 00053 # Copied from hztest: A dirty hack to work around an apparent race condition at startup 00054 # that causes some hztests to fail. Most evident in the tests of 00055 # rosstage. 00056 time.sleep(0.75) 00057 00058 try: 00059 rostest.run('rostest', 'test_hardware_test', HardwareTest, sys.argv) 00060 except KeyboardInterrupt, e: 00061 pass 00062 print "exiting"