basic_localization.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import sys
00004 import time
00005 from math import fmod, pi
00006 
00007 import unittest
00008 import rospy
00009 import rostest
00010 
00011 from tf.msg import tfMessage
00012 from tf.transformations import euler_from_quaternion
00013 from std_srvs.srv import Empty
00014 
00015 
00016 class TestBasicLocalization(unittest.TestCase):
00017     def setUp(self):
00018         self.tf = None
00019         self.target_x = None
00020         self.target_y = None
00021         self.target_a = None
00022 
00023     def tf_cb(self, msg):
00024         for t in msg.transforms:
00025             if t.header.frame_id == 'map':
00026                 self.tf = t.transform
00027                 (a_curr, a_diff) = self.compute_angle_diff()
00028                 print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
00029                 print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
00030                 print 'Diff:\t %16.6f %16.6f %16.6f' % (
00031                     abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff)
00032 
00033     def compute_angle_diff(self):
00034         rot = self.tf.rotation
00035         a = euler_from_quaternion([rot.x, rot.y, rot.z, rot.w])[2]
00036         d_a = self.target_a
00037 
00038         return (a, abs(fmod(a - d_a + 5*pi, 2*pi) - pi))
00039 
00040     def test_basic_localization(self):
00041         global_localization = int(sys.argv[1])
00042         self.target_x = float(sys.argv[2])
00043         self.target_y = float(sys.argv[3])
00044         self.target_a = float(sys.argv[4])
00045         tolerance_d = float(sys.argv[5])
00046         tolerance_a = float(sys.argv[6])
00047         target_time = float(sys.argv[7])
00048 
00049         if global_localization == 1:
00050             #print 'Waiting for service global_localization'
00051             rospy.wait_for_service('global_localization')
00052             global_localization = rospy.ServiceProxy('global_localization', Empty)
00053             global_localization()
00054 
00055         rospy.init_node('test', anonymous=True)
00056         while(rospy.rostime.get_time() == 0.0):
00057             #print 'Waiting for initial time publication'
00058             time.sleep(0.1)
00059         start_time = rospy.rostime.get_time()
00060         # TODO: This should be replace by a pytf listener
00061         rospy.Subscriber('/tf', tfMessage, self.tf_cb)
00062 
00063         while (rospy.rostime.get_time() - start_time) < target_time:
00064             #print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
00065             time.sleep(0.1)
00066 
00067         (a_curr, a_diff) = self.compute_angle_diff()
00068         print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
00069         print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
00070         print 'Diff:\t %16.6f %16.6f %16.6f' % (
00071             abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff)
00072         self.assertNotEquals(self.tf, None)
00073         self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
00074         self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
00075         self.assertTrue(a_diff <= tolerance_a)
00076 
00077 if __name__ == '__main__':
00078     rostest.run('amcl', 'amcl_localization',
00079                 TestBasicLocalization, sys.argv)


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:48