First tests. Now I know how to collect data from the costmap. No real planning implemented YET.
Merge branch 'master' of https://github.com/gstavrinos/gs_fastplanner