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