Provide costmap to homotopy class planner

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