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

data/armarx_navigation/local_planner_config/TimedElasticBands/default.json

@@ -6,7 +6,7 @@
     "pse": {
       "pse_costum_obstacle_penalties": true,
       "pse_costum_obstacle_penalties_dynamic": true,
-      "weight_costmap": 0.5,
+      "weight_costmap": 1,
       "weight_global_path_position": 0.3,
       "weight_global_path_orientation": 0.3,
       "lrk_use_alternative_approach": false,