test_map.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
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   # Test that 2 map files are approximately the same
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     # get raw data for comparison
00058     im0d = im0.getdata()
00059     im1d = im1.getdata()
00060   
00061     # assert len(i0)==len(i1)
00062     self.assertTrue(len(im0d) == len(im1d))
00063   
00064     #compare pixel by pixel for thumbnails
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)


gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard; ROS wrapper by Brian Gerkey
autogenerated on Fri Jan 3 2014 11:58:25