commented out
added costmap stuff
added evenything