base_test.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_hardware_test')

import sys
import unittest
import rospy
import rostest
import time
from cob_hardware_test.srv import *
from std_msgs.msg import String
from simple_script_server import *
from pr2_controllers_msgs.msg import *


def dialog_client(dialog_type, message):
        #dialog type: 0=confirm 1=question
        rospy.wait_for_service('dialog')
        try:
                dialog = rospy.ServiceProxy('dialog', Dialog)
                resp1 = dialog(dialog_type, message)
                return resp1.answer
        except rospy.ServiceException, e:
                print "Service call failed: %s" % e

class HardwareTest(unittest.TestCase):
        def __init__(self, *args):

                super(HardwareTest, self).__init__(*args)
                rospy.init_node('test_hardware_test')
                torso_joint_states = []
                self.message_received = False
                self.sss = simple_script_server()

                try:
                                # move_
                                if not rospy.has_param('~move_x'):
                                        self.fail('Parameter move_x does not exist on ROS Parameter Server')
                                self.move_x = rospy.get_param('~move_x')

                                if not rospy.has_param('~move_y'):
                                        self.fail('Parameter move_ does not exist on ROS Parameter Server')
                                self.move_y = rospy.get_param('~move_y')

                                if not rospy.has_param('~move_theta'):
                                        self.fail('Parameter move_ does not exist on ROS Parameter Server')
                                self.move_theta = rospy.get_param('~move_theta')

                except KeyError, e:
                                self.fail('Parameters not set properly')


        def test_base(self):
                self.sss.init("base")
                self.assertTrue(dialog_client(0, 'Ready to move base?' ))
                handle = self.sss.move_base_rel("base", [self.move_x, self.move_y, self.move_theta])
                self.assertEqual(handle.get_state(), 3)
                self.assertTrue(dialog_client(1, 'Did I move?'))
                self.assertTrue(dialog_client(0, 'EM Pressed and Released? \n Ready to move my Base ?' ))
                self.sss.recover("base")
                handle = self.sss.move_base_rel("base", [self.move_x, self.move_y, self.move_theta])

                

                

if __name__ == '__main__':
        # Copied from hztest: A dirty hack to work around an apparent race condition at startup
        # that causes some hztests to fail. Most evident in the tests of
        # rosstage.
        time.sleep(0.75)
        try:
                rostest.run('rostest', 'test_hardware_test', HardwareTest, sys.argv)
        except KeyboardInterrupt, e:
                pass
        print "exiting"