110 lines
3.9 KiB
Python
110 lines
3.9 KiB
Python
#!/usr/bin/env python
|
|
# Software License Agreement (BSD License)
|
|
#
|
|
# Copyright (c) 2008, Willow Garage, Inc.
|
|
# All rights reserved.
|
|
#
|
|
# Redistribution and use in source and binary forms, with or without
|
|
# modification, are permitted provided that the following conditions
|
|
# are met:
|
|
#
|
|
# * Redistributions of source code must retain the above copyright
|
|
# notice, this list of conditions and the following disclaimer.
|
|
# * Redistributions in binary form must reproduce the above
|
|
# copyright notice, this list of conditions and the following
|
|
# disclaimer in the documentation and/or other materials provided
|
|
# with the distribution.
|
|
# * Neither the name of the Willow Garage nor the names of its
|
|
# contributors may be used to endorse or promote products derived
|
|
# from this software without specific prior written permission.
|
|
#
|
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
# POSSIBILITY OF SUCH DAMAGE.
|
|
#
|
|
|
|
from __future__ import print_function
|
|
|
|
import sys
|
|
import yaml
|
|
from PIL import Image
|
|
import math
|
|
|
|
def find_bounds(map_image):
|
|
x_min = map_image.size[0]
|
|
x_end = 0
|
|
y_min = map_image.size[1]
|
|
y_end = 0
|
|
pix = map_image.load()
|
|
for x in range(map_image.size[0]):
|
|
for y in range(map_image.size[1]):
|
|
val = pix[x, y]
|
|
if val != 205: # not unknown
|
|
x_min = min(x, x_min)
|
|
x_end = max(x, x_end)
|
|
y_min = min(y, y_min)
|
|
y_end = max(y, y_end)
|
|
return x_min, x_end, y_min, y_end
|
|
|
|
def computed_cropped_origin(map_image, bounds, resolution, origin):
|
|
""" Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """
|
|
ox = origin[0]
|
|
oy = origin[1]
|
|
oth = origin[2]
|
|
|
|
# First figure out the delta we have to translate from the lower left corner (which is the origin)
|
|
# in the image system
|
|
dx = bounds[0] * resolution
|
|
dy = (map_image.size[1] - bounds[3] - 1) * resolution
|
|
|
|
# Next rotate this by the theta and add to the old origin
|
|
|
|
new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth)
|
|
new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth)
|
|
|
|
return [new_ox, new_oy, oth]
|
|
|
|
if __name__ == "__main__":
|
|
if len(sys.argv) < 2:
|
|
print("Usage: %s map.yaml [cropped.yaml]" % sys.argv[0], file=sys.stderr)
|
|
sys.exit(1)
|
|
|
|
with open(sys.argv[1]) as f:
|
|
map_data = yaml.safe_load(f)
|
|
|
|
if len(sys.argv) > 2:
|
|
crop_name = sys.argv[2]
|
|
if crop_name.endswith(".yaml"):
|
|
crop_name = crop_name[:-5]
|
|
crop_yaml = crop_name + ".yaml"
|
|
crop_image = crop_name + ".pgm"
|
|
else:
|
|
crop_yaml = "cropped.yaml"
|
|
crop_image = "cropped.pgm"
|
|
|
|
map_image_file = map_data["image"]
|
|
resolution = map_data["resolution"]
|
|
origin = map_data["origin"]
|
|
map_image = Image.open(map_image_file)
|
|
|
|
bounds = find_bounds(map_image)
|
|
|
|
# left, upper, right, lower
|
|
cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1))
|
|
|
|
cropped_image.save(crop_image)
|
|
map_data["image"] = crop_image
|
|
map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin)
|
|
with open(crop_yaml, "w") as f:
|
|
yaml.dump(map_data, f)
|
|
|