Doc Cleanup and Fix Costmap Test