Fix/update scene before planning

To avoid collisions when static objects have been moved.

source/armarx/navigation/components/Navigator/Navigator.cpp

@@ -145,10 +145,7 @@ namespace armarx::navigation::components
 
         // initialize scene
         ARMARX_TRACE;
-        scene.robot = getRobot();
-        scene.staticScene = staticScene();
-        scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
-
+        updateContext();
 
         executor = server::PlatformUnitExecutor(platformUnit);
 
@@ -215,9 +212,17 @@ namespace armarx::navigation::components
         ARMARX_TRACE;
 
         scene.robot = getRobot();
-        scene.staticScene = staticScene();
+
+        {
+            std::lock_guard g{scene.staticSceneMtx};
+            scene.staticScene = staticScene();
+        }
+
         // TODO dynamic scene
-        scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
+        {
+            std::lock_guard g{scene.graphMtx};
+            scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
+        }
     }
 
     std::string
@@ -274,6 +279,7 @@ namespace armarx::navigation::components
 
         try
         {
+            updateContext();
             navigators.at(callerId).moveTo(convert(waypoints),
                                            core::NavigationFrameNames.from_name(navigationMode));
         }
@@ -302,6 +308,8 @@ namespace armarx::navigation::components
 
         ARMARX_CHECK(navigators.count(callerId) > 0)
             << "Navigator config for caller `" << callerId << "` not registered!";
+
+        updateContext();
         navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
 
         // navigators.at(callerId).moveTo()
@@ -319,6 +327,8 @@ namespace armarx::navigation::components
 
         ARMARX_CHECK(navigators.count(callerId) > 0)
             << "Navigator config for caller `" << callerId << "` not registered!";
+
+        updateContext();
         navigators.at(callerId).moveTowards(direction,
                                             core::NavigationFrameNames.from_name(navigationMode));
     }