From 8ae195b0ad1e910e58f77325cc54500400cc46ad Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 5 Jul 2021 13:27:40 +0200
Subject: [PATCH] robotstatememory: fixes to avoid race condition
---
.../server/proprioception/Segment.cpp | 22 +++++++++++++------
1 file changed, 15 insertions(+), 7 deletions(-)
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 7f95e4387..ac5e5e1c3 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);
+ }
}
}
--
GitLab