mapper.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from nav_msgs.srv import GetMapResponse
00004 from PIL import Image
00005 from tf.transformations import euler_from_quaternion, quaternion_from_euler
00006 
00007 import os
00008 import sys
00009 import yaml
00010 
00011 def loadMapFromFile(yaml_file):
00012     try:
00013         map_info = yaml.load(open(yaml_file, 'r'))
00014     except:
00015         sys.stderr.write("Unable to load yaml file for map: %s\n" %yaml_file)
00016         return None
00017 
00018     resolution = map_info.get('resolution')
00019     origin = map_info.get('origin')
00020     negate = map_info.get('negate')
00021     occupied_thresh = map_info.get('occupied_thresh')
00022     free_thresh = map_info.get('free_thresh')
00023 
00024     image_file = map_info.get('image')
00025     if image_file[0] != '/': 
00026         yaml_file_dir = os.path.dirname(os.path.realpath(yaml_file))
00027         image_file = yaml_file_dir + '/' + image_file
00028 
00029     return loadMapFromImageFile(image_file, resolution,
00030           negate, occupied_thresh, free_thresh, origin)
00031 
00032 def getImageFileLocationFromMapFile(yaml_file):
00033     try:
00034         map_info = yaml.load(open(yaml_file, 'r'))
00035     except:
00036         sys.stderr.write("Unable to load yaml file for map: %s\n" %yaml_file)
00037         return None
00038 
00039     image_file = map_info.get('image')
00040     if image_file[0] != '/': 
00041         yaml_file_dir = os.path.dirname(os.path.realpath(yaml_file))
00042         image_file = yaml_file_dir + '/' + image_file
00043     return image_file
00044 
00045 def loadMapFromImageFile(image_file, resolution, negate, occupied_thresh,
00046                     free_thresh, origin):
00047 
00048     resp = GetMapResponse()
00049 
00050     image = Image.open(image_file)
00051     pix = image.load()
00052 
00053     image_size = image.size
00054     resp.map.info.width = image_size[0]
00055     resp.map.info.height = image_size[1]
00056     resp.map.info.resolution = resolution
00057 
00058     resp.map.info.origin.position.x = origin[0]
00059     resp.map.info.origin.position.y = origin[1]
00060     resp.map.info.origin.position.z = 0
00061     q = quaternion_from_euler(0,0,origin[2])
00062     resp.map.info.origin.orientation.x = q[0]
00063     resp.map.info.origin.orientation.y = q[1]
00064     resp.map.info.origin.orientation.z = q[2]
00065     resp.map.info.origin.orientation.w = q[3]
00066 
00067     test_pxl = pix[0,0]
00068     if isinstance(test_pxl, (list, tuple)):
00069       is_multi_layer = True
00070       num_layers = len(test_pxl)
00071     else:
00072       is_multi_layer = False
00073       num_layers = 1
00074 
00075     resp.map.data = [None] * image_size[0] * image_size[1]
00076     for j in range(image_size[1]):
00077       for i in range(image_size[0]):
00078 
00079         pxl = pix[i, j]
00080 
00081         if is_multi_layer:
00082           color_average = sum(pxl) / num_layers
00083         else:
00084           color_average = pxl
00085 
00086         if negate:
00087           occ = color_average / 255.0;
00088         else:
00089           occ = (255.0 - color_average) / 255.0;
00090 
00091         map_idx = resp.map.info.width * (resp.map.info.height - j - 1) + i 
00092         if (occ > occupied_thresh):
00093           resp.map.data[map_idx] = 100
00094         elif (occ < free_thresh):
00095           resp.map.data[map_idx] = 0
00096         else:
00097           resp.map.data[map_idx] = -1
00098 
00099     return resp
00100 
00101 def saveMapToFile(map, yaml_file, image_file, negate, free_thresh,
00102                   occupied_thresh, image=None):
00103 
00104     map_info = {}
00105 
00106     map_info['resolution'] = map.info.resolution
00107 
00108     origin = [0.0, 0.0, 0.0]
00109     origin[0] = map.info.origin.position.x 
00110     origin[1] = map.info.origin.position.y 
00111     a = euler_from_quaternion([
00112         map.info.origin.orientation.x,
00113         map.info.origin.orientation.y,
00114         map.info.origin.orientation.z,
00115         map.info.origin.orientation.w
00116     ])
00117     origin[2] = a[2] #yaw
00118     map_info['origin'] = origin
00119 
00120     map_info['negate'] = 1 if negate else 0
00121     map_info['occupied_thresh'] = occupied_thresh
00122     map_info['free_thresh'] = free_thresh
00123 
00124     map_info['image'] = image_file
00125     if image_file[0] != '/': 
00126         yaml_file_dir = os.path.dirname(os.path.realpath(yaml_file))
00127         image_file = yaml_file_dir + '/' + image_file
00128 
00129     saveMapToImageFile(map, image_file, negate, free_thresh, occupied_thresh, image)
00130 
00131     with open(yaml_file, "w") as outfile:
00132         outfile.write(yaml.dump(map_info))
00133         outfile.close()
00134 
00135 def saveMapToImageFile(map, image_file, negate, free_thresh, occupied_thresh, image=None):
00136     
00137     if image is None:
00138         # Construct image from the map
00139         image = Image.new("L", (map.info.width, map.info.height))
00140         pixels = image.load()
00141 
00142         for j in range(map.info.height):
00143             for i in range(map.info.width):
00144 
00145                 # The occupancy grid is vertically flipped
00146                 map_idx = map.info.width * (map.info.height - j - 1) + i 
00147 
00148                 # Setup negated image first
00149                 if map.data[map_idx] == 100:
00150                     pixels[i,j] = 255
00151                 elif map.data[map_idx] == 0:
00152                     pixels[i,j] = 0
00153                 else:
00154                     pixels[i,j] = 255 * ((free_thresh + occupied_thresh) / 2.0) 
00155 
00156                 if not negate:
00157                     pixels[i,j] = 255 - pixels[i,j]
00158 
00159     image.save(image_file)


bwi_tools
Author(s): Piyush Khandelwal
autogenerated on Fri Aug 28 2015 10:14:30