component_test.py

Go to the documentation of this file.

#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_hardware_test')
#from mpmath.functions.functions import fabs
import sys
import time
import unittest
import math

import rospy
import rostest
from cob_hardware_test.srv import *
from trajectory_msgs.msg import *
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 UnitTest(unittest.TestCase):
        def __init__(self, *args):
                super(UnitTest, self).__init__(*args)
                rospy.init_node('component_test')
                self.message_received = False
                self.sss = simple_script_server()
                self.command_traj = JointTrajectory()
                # get parameters
                try:
                        # component
                        if not rospy.has_param('~component'):
                                self.fail('Parameter component does not exist on ROS Parameter Server')
                        self.component = rospy.get_param('~component')
                        # movement command
                        if not rospy.has_param('~test_target'):
                                self.fail('Parameter test_target does not exist on ROS Parameter Server')
                        self.test_target = rospy.get_param('~test_target')
                        # movement command
                        if not rospy.has_param('~default_target'):
                                self.fail('Parameter default_target does not exist on ROS Parameter Server')
                        self.default_target = rospy.get_param('~default_target')
                        # time to wait before
                        self.wait_time = rospy.get_param('~wait_time', 5)
                        # error range
                        if not rospy.has_param('~error_range'):
                                self.fail('Parameter error_range does not exist on ROS Parameter Server')
                        self.error_range = rospy.get_param('~error_range')
                except KeyError, e:
                        self.fail('Parameters not set properly')
                print """
                        Component: %s
                        Targets: %s , %s
                        Wait Time: %s
                        Error Range: %s""" % (self.component, self.default_target, self.test_target, self.wait_time, self.error_range)
                # check parameters
                # \todo do more parameter tests
                if self.error_range < 0.0:
                        error_msg = "Parameter error_range should be positive, but is " + self.error_range
                        self.fail(error_msg)
                if self.wait_time < 0.0:
                        error_msg = "Parameter wait_time should be positive, but is " + self.wait_time
                        self.fail(error_msg)

                # init subscribers
                state_topic = "/" + self.component + "_controller/state"
                sub_state_topic = rospy.Subscriber(state_topic, JointTrajectoryControllerState, self.cb_state)

        def test_component(self):
                
                # init component
                init_handle = self.sss.init(self.component)
                if init_handle.get_error_code() != 0:
                        error_msg = 'Could not initialize ' + self.component
                        self.fail(error_msg)

                # start actual test
                print "Waiting for messages"
                #give the topics some seconds to receive messages
                abort_time = rospy.Time.now() + rospy.Duration(self.wait_time)
                while not self.message_received and rospy.get_rostime() < abort_time:
                        # print "###debug here###"
                        rospy.sleep(0.1)

                if not self.message_received:
                        self.fail('No state message received within wait_time(%s) from /%s_controller/state' % (self.wait_time, self.component))

                self.assertTrue(dialog_client(0, 'Ready to move <<%s>> to \n <<%s>> and <<%s>>?' % (self.component, self.test_target, self.default_target)))

                # execute movement to test_target and back to default_target
                self.execute_movement()

                self.assertTrue(dialog_client(1, ' Please press emergency stop and release it again.\n\n Ready to move <<%s>> to \n <<%s>> and <<%s>>?' % (self.component, self.test_target, self.default_target)))

                recover_handle = self.sss.recover(self.component)
                if recover_handle.get_error_code() != 0 :
                        error_msg = 'Could not recover ' + self.component
                        self.fail(error_msg)

                # execute movement to test_target and back to default_target
                self.execute_movement()

        def execute_movement(self):
                # test_target: send commands to component 
                move_handle = self.sss.move(self.component, self.test_target)
                self.assertEqual(move_handle.get_state(), 3) # state 3 equals errorcode 0 therefore the following will never be executed
                if move_handle.get_error_code() != 0:
                        error_msg = 'Could not move ' + self.component
                        self.fail(error_msg + "; errorCode: " + str(move_handle.get_error_code()))
                self.check_target_reached(self.test_target)

                # default_target: send commands to component 
                move_handle = self.sss.move(self.component, self.default_target)
                self.assertEqual(move_handle.get_state(), 3) # state 3 equals errorcode 0 therefore the following will never be executed
                if move_handle.get_error_code() != 0:
                        error_msg = 'Could not move ' + self.component
                        self.fail(error_msg + "; errorCode: " + str(move_handle.get_error_code()))
                self.check_target_reached(self.default_target)

        def check_target_reached(self,target):
                # get commanded trajectory
                command_traj = rospy.get_param("/script_server/" + self.component + "/" + target)
                print command_traj

                # get last point out of trajectory
                traj_endpoint = command_traj[len(command_traj) - 1]
                print traj_endpoint

                actual_pos = self.actual_pos # fix current position configuration for later evaluation

                # checking if target position is really reached
                print "actual_pos = ", actual_pos
                print "traj_endpoint = ", traj_endpoint
                for i in range(len(traj_endpoint)):
                        self.assert_(((math.fabs(traj_endpoint[i] - actual_pos[i])) < self.error_range), "Target position out of error_range")

                self.assertTrue(dialog_client(1, 'Did <<%s>> reach <<%s>>?' % (self.component, target)))

        # callback functions
        def cb_state(self, msg):
                self.actual_pos = msg.actual.positions
                self.message_received = True


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', 'component_test', UnitTest, sys.argv)
        except KeyboardInterrupt, e:
                pass
        print "exiting"