- [ ] Finish testing generation of costs in costmaps - [ ] Design interface for planner to use costmaps