diff --git a/example/20190505_154653_map.png b/example/20190505_154653_map.png new file mode 100644 index 0000000..af8e270 Binary files /dev/null and b/example/20190505_154653_map.png differ diff --git a/libs/parser.py b/libs/parser.py index 3c70a93..8e2cfa1 100644 --- a/libs/parser.py +++ b/libs/parser.py @@ -285,4 +285,4 @@ if __name__ == "__main__": args, __ = parser.parse_known_args() - test(args) \ No newline at end of file + test(args) diff --git a/libs/parser_post.py b/libs/parser_post.py new file mode 100644 index 0000000..6777f7d --- /dev/null +++ b/libs/parser_post.py @@ -0,0 +1,110 @@ +import io +import os +from PIL import Image +from PIL import ImageDraw +from PIL import ImageChops + + + +def build_map(slam_data, map_image_data): + ''' + draws the path into the map. Returns the new map as a BytesIO + + from https://github.com/dgiese/dustcloud/blob/71f7af3e2b9607548bcd845aca251326128f742c/dustcloud/build_map.py + + ''' + map_image = Image.open(io.BytesIO(map_image_data)) + map_image = map_image.convert('RGBA') + + # calculate center of the image + center_x = map_image.size[0] / 2 + center_y = map_image.size[0] / 2 + + # rotate image by -90° + # map_image = map_image.rotate(-90) + + red = (255, 0, 0, 255) + green = (0, 255, 0, 255) + grey = (125, 125, 125, 255) # background color + transparent = (0, 0, 0, 0) + + # prepare for drawing + draw = ImageDraw.Draw(map_image) + + # loop each loc + prev_pos = None + for line in slam_data: + tmp = line.rstrip().split(",") + robotime = float(tmp[1]) + epoch = int(tmp[2]) + x = float(tmp[3]) + y = float(tmp[4]) + yaw = float(tmp[5]) + + # set x & y by center of the image + # 20 is the factor to fit coordinates in in map + x = center_x + (x * 20) + y = center_y + (-y * 20) + pos = (x, y) + + if prev_pos: + draw.line([prev_pos, pos], red) + + prev_pos = pos + + # rotate image back by 90° + # map_image = map_image.rotate(90) + + # crop image + bgcolor_image = Image.new('RGBA', map_image.size, grey) + cropbox = ImageChops.subtract(map_image, bgcolor_image).getbbox() + + # and replace background with transparent pixels + pixdata = map_image.load() + for y in range(map_image.size[1]): + for x in range(map_image.size[0]): + if pixdata[x, y] == grey: + pixdata[x, y] = transparent + + temp = io.BytesIO() + map_image.save(temp, format="png") + return temp + + +def test(args): + if args.loc and args.map: + with open(args.loc) as f: + # skip the first line which is coumn names + slam_data = f.readlines()[1:] + with open(args.map, 'rb') as f: + map_image_data = f.read() + augmented_map = build_map(slam_data, map_image_data) + filepath, ext = os.path.splitext(args.map) + with open("{}.png".format(filepath), 'wb') as f: + f.write(augmented_map.getvalue()) + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser( + description='post processing test' + ) + + parser.add_argument( + '-l', '--location', + dest='loc', + default=None, + help='Specify location file path' + ) + + parser.add_argument( + '-m', '--map', + dest='map', + default=None, + help='Specify map file path' + ) + + args, __ = parser.parse_known_args() + + test(args) diff --git a/libs/vacuum_controller.py b/libs/vacuum_controller.py index b3ae19c..811ed6c 100644 --- a/libs/vacuum_controller.py +++ b/libs/vacuum_controller.py @@ -438,4 +438,4 @@ def init_controller(ip, token): print("Cannot connect to vacuum!") exit(-1) - return c \ No newline at end of file + return c