included copying of costmap to get obstacle values, transformation into voronoi grid not yet functional