timedelasticbands: activated obstManager.addHumanObstacle

source/armarx/navigation/local_planning/TimedElasticBands.cpp

@@ -145,7 +145,7 @@ namespace armarx::navigation::local_planning
 
         viz::Layer* visPtr = nullptr;
         viz::Layer visLayer;
-        if (arviz)
+        if (arviz != nullptr)
         {
             visLayer = arviz->layer("local_planner_obstacles");
             visPtr = &visLayer;
@@ -154,6 +154,7 @@ namespace armarx::navigation::local_planning
 
         if (scene.staticScene)
         {
+            ARMARX_CHECK(scene.staticScene.has_value());
             for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
             {
                 obstManager.addBoxObstacle(obst->getGlobalBoundingBox(), visPtr);
@@ -161,14 +162,14 @@ namespace armarx::navigation::local_planning
         }
         if (scene.dynamicScene)
         {
-            //TODO include humans in dynamic scene
-            //for (const auto& obst : scene.dynamicScene.value().humans)
-            //{
-            //   obstManager.addHumanObstacle(obst, visPtr);
-            //}
+            ARMARX_CHECK(scene.dynamicScene.has_value());
+            for (const auto& obst : scene.dynamicScene.value().humans)
+            {
+              obstManager.addHumanObstacle(obst, visPtr);
+            }
         }
 
-        if (arviz)
+        if (arviz != nullptr)
         {
             arviz->commit(visLayer);
         }