diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp index 7f95e43878aee62619f8c4997db25460e11ed8cd..ac5e5e1c36e66d47251700ec6565a9fb61ce5430 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp +++ b/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); + } } }