robotstatememory: fixes to avoid race condition

source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp

@@ -93,21 +93,29 @@ namespace armarx::armem::server::robot_state::proprioception
     {
         std::unordered_map<std::string, std::map<std::string, float>> jointMap;
 
+        std::lock_guard g{memoryMutex};
+
         for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
         {
             for (const auto& [name, entity] :  provSeg.entities())
             {
-                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+                try
+                {
+                    const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+                    const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
 
-                const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+                    if (not jointState)
+                    {
+                        // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+                        continue;
+                    }
+                    jointMap[robotName].emplace(jointState->name, jointState->position);
 
-                if (not jointState)
-                {
-                    // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
-                    continue;
                 }
+                catch (...) // empty history etc
+                {
 
-                jointMap[robotName].emplace(jointState->name, jointState->position);
+                }
             }
         }