fixed voronoi display, still problem with unkown parts of map, included trajectory planner from base_local_planner