introducing mutexes for scene

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

source/armarx/navigation/core/types.h

@@ -78,7 +78,10 @@ namespace armarx::navigation::core
 
     struct Scene
     {
+        mutable std::mutex staticSceneMtx;
         std::optional<core::StaticScene> staticScene = std::nullopt;
+        
+        mutable std::mutex dynamicSceneMtx;
         std::optional<core::DynamicScene> dynamicScene = std::nullopt;
         // TopologicMapPtr topologicMap;
         VirtualRobot::RobotPtr robot;
@@ -86,6 +89,7 @@ namespace armarx::navigation::core
         mutable std::mutex platformVelocityMtx;
         std::optional<core::Twist> platformVelocity;
 
+        mutable std::mutex graphMtx;
         std::optional<core::SceneGraph> graph;
 
         TimeServerInterface* timeServer;

source/armarx/navigation/global_planning/AStar.cpp

 #include "AStar.h"
 
 #include <algorithm>
+#include <mutex>
 #include <optional>
 
 #include <Eigen/Geometry>
@@ -112,6 +113,7 @@ namespace armarx::navigation::glob_plan
     {
         ARMARX_TRACE;
 
+        std::lock_guard g{scene.staticSceneMtx};
         algorithm::astar::AStarPlanner planner(scene.robot, scene.staticScene->objects, 100.F);
         // planner.setRobotColModel("Platform-colmodel");