example_map_from_contours.py

from __future__ import print_function
import CMap2D
import matplotlib.pyplot as plt
import numpy as np

from map2d import gridshow

contours = [
    [(-3.5647773123575437, 5.835222687642457),
     (-5.035222687642456, 5.835222687642457),
     (-5.035222687642456, 4.364777312357544),
     (-3.5647773123575437, 4.364777312357544)],
    [(-3.5330464973133493, 5.866953502686651),
     (-5.266953502686651, 5.866953502686651),
     (-5.266953502686651, 4.133046497313349),
     (-3.5330464973133493, 4.133046497313349)],
    [(-3.649238872233948, 3.450761127766052),
     (-4.950761127766052, 3.450761127766052),
     (-4.950761127766052, 2.1492388722339486),
     (-3.649238872233948, 2.1492388722339486)],
    [(3.8844705995895206, -3.21552940041048),
     (2.91552940041048, -3.21552940041048),
     (2.91552940041048, -4.18447059958952),
     (3.8844705995895206, -4.18447059958952)],
    [(5.126810354965404, -5.773189645034597),
     (4.273189645034597, -5.773189645034597),
     (4.273189645034597, -6.626810354965404),
     (5.126810354965404, -6.626810354965404)],
    [(-1.0, 5.4), (-3.0, 5.4), (-3.0, 4.4), (-1.0, 4.4)],
    [(3.8000000000000003, 4.0),
     (1.8000000000000003, 4.0),
     (1.8000000000000003, 3.0),
     (3.8000000000000003, 3.0)],
    [(-4.6000000000000005, 7.1000000000000005),
     (-7.6000000000000005, 7.1000000000000005),
     (-7.6000000000000005, 6.1000000000000005),
     (-4.6000000000000005, 6.1000000000000005)],
    [(3.1, -1.9000000000000004),
     (2.1, -1.9000000000000004),
     (2.1, -4.9),
     (3.1, -4.9)]
]

for c in contours:
    verts = np.concatenate((c, [c[0]]), axis=0)
    plt.plot(verts[:,0], verts[:,1])
plt.show()

map_ = CMap2D.CMap2D()
map_.from_closed_obst_vertices(contours)
plt.figure()
gridshow(map_.occupancy())
plt.show()