basic_localization.py

Go to the documentation of this file.

#!/usr/bin/env python

import sys
import time
from math import fmod, pi

import unittest
import rospy
import rostest

from tf.msg import tfMessage
from tf.transformations import euler_from_quaternion
from std_srvs.srv import Empty


class TestBasicLocalization(unittest.TestCase):
    def setUp(self):
        self.tf = None
        self.target_x = None
        self.target_y = None
        self.target_a = None

    def tf_cb(self, msg):
        for t in msg.transforms:
            if t.header.frame_id == 'map':
                self.tf = t.transform
                (a_curr, a_diff) = self.compute_angle_diff()
                print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
                print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
                print 'Diff:\t %16.6f %16.6f %16.6f' % (
                    abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff)

    def compute_angle_diff(self):
        rot = self.tf.rotation
        a = euler_from_quaternion([rot.x, rot.y, rot.z, rot.w])[2]
        d_a = self.target_a

        return (a, abs(fmod(a - d_a + 5*pi, 2*pi) - pi))

    def test_basic_localization(self):
        global_localization = int(sys.argv[1])
        self.target_x = float(sys.argv[2])
        self.target_y = float(sys.argv[3])
        self.target_a = float(sys.argv[4])
        tolerance_d = float(sys.argv[5])
        tolerance_a = float(sys.argv[6])
        target_time = float(sys.argv[7])

        if global_localization == 1:
            #print 'Waiting for service global_localization'
            rospy.wait_for_service('global_localization')
            global_localization = rospy.ServiceProxy('global_localization', Empty)
            global_localization()

        rospy.init_node('test', anonymous=True)
        while(rospy.rostime.get_time() == 0.0):
            #print 'Waiting for initial time publication'
            time.sleep(0.1)
        start_time = rospy.rostime.get_time()
        # TODO: This should be replace by a pytf listener
        rospy.Subscriber('/tf', tfMessage, self.tf_cb)

        while (rospy.rostime.get_time() - start_time) < target_time:
            #print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
            time.sleep(0.1)

        (a_curr, a_diff) = self.compute_angle_diff()
        print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
        print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
        print 'Diff:\t %16.6f %16.6f %16.6f' % (
            abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff)
        self.assertNotEquals(self.tf, None)
        self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
        self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
        self.assertTrue(a_diff <= tolerance_a)

if __name__ == '__main__':
    rostest.run('amcl', 'amcl_localization',
                TestBasicLocalization, sys.argv)