00001
00002 PACKAGE='cob_sdh'
00003 import roslib; roslib.load_manifest(PACKAGE)
00004 import sys
00005 import rospy
00006 import unittest
00007 import time
00008 from cob_msgs.msg import *
00009 from cob_srvs.srv import *
00010 from pr2_controllers_msgs.msg import *
00011 from sensor_msgs.msg import *
00012
00013 class TestInititialization(unittest.TestCase):
00014 def __init__(self, *args):
00015 super(TestInititialization, self).__init__(*args)
00016 rospy.init_node("test_init")
00017
00018 def callback(self, msg):
00019 if(msg.name[0] == "sdh_thumb_2_joint"):
00020 self.received = True
00021 def callback_tac(self, msg):
00022 self.received_tac = True
00023
00024 def test_init(self):
00025 self.received = False
00026 self.received_tac = False
00027 rospy.wait_for_service('init')
00028 call_init = rospy.ServiceProxy('init', Trigger)
00029 try:
00030 resp1 = call_init(0)
00031 except rospy.ServiceException, e:
00032 self.assertEquals(0, 1, "Init failed: " +str(e))
00033 sub = rospy.Subscriber("/joint_states", JointState, self.callback)
00034 sub_tac = rospy.Subscriber("tactile_data", TactileSensor, self.callback_tac)
00035 time.sleep(1.0)
00036 if(self.received):
00037 self.assertEquals(1, 1, "Init Hand successful")
00038 else:
00039 self.assertEquals(0, 1, "No controller state messages received")
00040 if(self.received_tac):
00041 self.assertEquals(1, 1, "Init Tactile successful")
00042 else:
00043 self.assertEquals(0, 1, "No tactile messages received")
00044
00045 if __name__ == '__main__':
00046 import rostest
00047 rostest.rosrun(PACKAGE, 'test_init', TestInititialization)
