00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 import PIL.Image
00037 import unittest
00038 import subprocess
00039 import sys
00040
00041 import roslib
00042 import os
00043 roslib.load_manifest('gmapping')
00044 import rostest
00045
00046 class TestGmapping(unittest.TestCase):
00047
00048
00049 def cmp_maps(self, f0, f1):
00050 im0 = PIL.Image.open(f0+'.pgm')
00051 im1 = PIL.Image.open(f1+'.pgm')
00052
00053 size = 100,100
00054 im0.thumbnail(size,PIL.Image.ANTIALIAS)
00055 im1.thumbnail(size,PIL.Image.ANTIALIAS)
00056
00057
00058 im0d = im0.getdata()
00059 im1d = im1.getdata()
00060
00061
00062 self.assertTrue(len(im0d) == len(im1d))
00063
00064
00065 error_count = 0
00066 error_total = 0
00067 pixel_tol = 0
00068 total_error_tol = 0.1
00069 for i in range(len(im0d)):
00070 (p0) = im0d[i]
00071 (p1) = im1d[i]
00072 if abs(p0-p1) > pixel_tol:
00073 error_count = error_count + 1
00074 error_total = error_total + abs(p0-p1)
00075 error_avg = float(error_total)/float(len(im0d))
00076 print '%d / %d = %.6f (%.6f)'%(error_total,len(im0d),error_avg,total_error_tol)
00077 self.assertTrue(error_avg <= total_error_tol)
00078
00079 def test_basic_localization_stage(self):
00080 if sys.argv > 1:
00081 target_time = float(sys.argv[1])
00082
00083 import time
00084 import rospy
00085 rospy.init_node('test', anonymous=True)
00086 while(rospy.rostime.get_time() == 0.0):
00087 print 'Waiting for initial time publication'
00088 time.sleep(0.1)
00089 start_time = rospy.rostime.get_time()
00090
00091 while (rospy.rostime.get_time() - start_time) < target_time:
00092 print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
00093 time.sleep(0.1)
00094
00095 f0 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_groundtruth')
00096 f1 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_generated')
00097
00098 cmd = ['rosrun', 'map_server', 'map_saver', 'map:=dynamic_map', '-f', f1]
00099 self.assertTrue(subprocess.call(cmd) == 0)
00100
00101 self.cmp_maps(f0,f1)
00102
00103 if __name__ == '__main__':
00104 rostest.run('gmapping', 'gmapping_slam', TestGmapping, sys.argv)