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

source/armarx/navigation/local_planning/TimedElasticBands.cpp

@@ -221,10 +221,16 @@ namespace armarx::navigation::local_planning
     void
     TimedElasticBands::setTebCostmap()
     {
-        if (!scene.staticScene)
+        if (not scene.staticScene.has_value())
+        {
             return;
-        if (!scene.staticScene->distanceToObstaclesCostmap)
+        }
+        
+        if (not scene.staticScene->distanceToObstaclesCostmap.has_value())
+        {
             return;
+        }
+
         const algorithms::Costmap& navigationCostmap =
             scene.staticScene->distanceToObstaclesCostmap.value();
         teb_costmap.emplace(conv::toRos(navigationCostmap));