convert algorithms::Costmap to teb_local_planner::Costmap and hand it over to the homotopy class planner
This uses some new functionality from teb_local_planner from merge request teb_local_planner/!5