diff --git a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
index 7d20a4078328a2cf5221073c68d0a50c6ddc6cd5..280a465cbaac7408259e2c439c827bdbc0fc19e5 100644
--- a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
@@ -16,10 +16,10 @@ namespace armarx::armem::wm::detail
template <class DerivedT>
struct FindInstanceDataMixin
{
- armarx::aron::datanavigator::DictNavigatorPtr
+ aron::datanavigator::DictNavigatorPtr
findLatestInstanceData(const MemoryID& entityID, int instanceIndex = 0) const
{
- const auto* instance = derived<DerivedT>(*this).findLatestInstance(entityID, instanceIndex);
+ const auto* instance = derived<DerivedT>(this).findLatestInstance(entityID, instanceIndex);
return instance ? instance->data() : nullptr;
}
@@ -28,7 +28,7 @@ namespace armarx::armem::wm::detail
std::optional<AronDtoT>
findLatestInstanceDataAs(const MemoryID& entityID, int instanceIndex = 0) const
{
- if (auto data = derived<DerivedT>(*this).findLatestInstanceData(entityID, instanceIndex))
+ if (aron::datanavigator::DictNavigatorPtr data = derived<DerivedT>(this).findLatestInstanceData(entityID, instanceIndex))
{
AronDtoT aron;
aron.fromAron(data);
diff --git a/source/RobotAPI/libraries/armem/server/segment/Segment.h b/source/RobotAPI/libraries/armem/server/segment/Segment.h
index a34f89e9cab53b75860599c1d6f5174400843abd..dc35846aa89d14c125e5a6f3093e1a19ee85c6e2 100644
--- a/source/RobotAPI/libraries/armem/server/segment/Segment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/Segment.h
@@ -76,7 +76,7 @@ namespace armarx::armem::server::segment
template <class FunctionT>
- auto doLocked(FunctionT&& function)
+ auto doLocked(FunctionT&& function) const
{
return segment->doLocked(function);
}
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
index 59b198ec20be181a6359eee06e2ddac93be4cc1d..271536db21ffdfadb3fba874f60bdd8a33d21f44 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedSegment.h
@@ -36,7 +36,7 @@ namespace armarx::armem::server::segment
template <class FunctionT>
- auto doLocked(FunctionT&& function)
+ auto doLocked(FunctionT&& function) const
{
return coreSegment->doLocked(function);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 2ddcc827d80c0700f59cb5f9ee8638322f4e5633..2c60cb9bd35ab7b3010e5b5f5fb0e719d0817f98 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -54,30 +54,31 @@ namespace armarx::armem::server::obj::attachments
Segment::getAttachments(const armem::Time& timestamp) const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
- std::scoped_lock(coreSegment->mutex());
-
- std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
- coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
+ return coreSegment->doLocked([this]()
{
- const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
- if (not aronAttachment)
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+ coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
{
- ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
- return true;
- }
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
+ if (not aronAttachment)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
+ return true;
+ }
- armarx::armem::attachment::ObjectAttachment attachment;
- fromAron(*aronAttachment, attachment);
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
- attachments.push_back(attachment);
- return true;
- });
+ armarx::armem::attachment::ObjectAttachment attachment;
+ fromAron(*aronAttachment, attachment);
- return attachments;
+ attachments.push_back(attachment);
+ return true;
+ });
+
+ return attachments;
+ });
}
} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index ed941f1223dc617896c13118c237144bdf71d011..8b1ae942a95b73119eea072b7f33a539a05fbbdf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -124,11 +124,10 @@ namespace armarx::armem::server::obj::clazz
{
try
{
- std::optional<arondto::ObjectClass> aron = coreSegment->doLocked([this, &entity]()
+ std::optional<arondto::ObjectClass> aron = coreSegment->doLocked([this, &entityID]()
{
return coreSegment->findLatestInstanceDataAs<arondto::ObjectClass>(entityID, 0);
});
-
if (not aron.has_value())
{
return;
@@ -272,12 +271,14 @@ namespace armarx::armem::server::obj::clazz
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- std::scoped_lock lock(segment.mutex());
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.coreSegment)
+ segment.doLocked([this, &segment]()
{
- segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
- }
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ }
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index d6d224fe0580e4aa73656515a671ba5753e33326..9668e307ba7c7ff3ed1143087bf4aee1adf2608c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1039,28 +1039,29 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
armem::obj::SceneSnapshot scene;
+ segment.doLocked([&scene, &segment]()
{
- std::scoped_lock lock(segment.mutex());
scene = segment.getSceneSnapshot();
- }
+ });
segment.storeScene(storeLoadLine.getValue(), scene);
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
- std::scoped_lock lock(segment.mutex());
-
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ segment.doLocked([this, &segment]()
{
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.coreSegment)
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ }
}
- }
- segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ });
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 7cd8b1202ac34600f8bfc6ecc912037b49d8bee6..a9f2aaf31acec502463676cff6e0fc692d736911 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -123,8 +123,8 @@ namespace armarx::armem::server::obj::instance
<< "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
return;
}
+ segment.doLocked([this, &providerName, &info]()
{
- std::scoped_lock lock(segment.mutex());
std::stringstream ss;
for (const auto& id : info.supportedObjects)
{
@@ -133,7 +133,7 @@ namespace armarx::armem::server::obj::instance
ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
<< "Supported objects: \n" << ss.str();
segment.providers[providerName] = info;
- }
+ });
}
@@ -164,8 +164,8 @@ namespace armarx::armem::server::obj::instance
return;
}
+ segment.doLocked([&]()
{
- std::scoped_lock lock(segment.mutex());
RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
if (segment.robot->hasRobotNode(calibration.robotNode))
@@ -200,7 +200,7 @@ namespace armarx::armem::server::obj::instance
{ "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
});
}
- }
+ });
}
@@ -218,12 +218,12 @@ namespace armarx::armem::server::obj::instance
{
TIMING_START(tGetObjectPoses);
- TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(segment.mutex());
- TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
-
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPoses(now)));
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now]()
+ {
+ return simox::alg::get_values(segment.getObjectPoses(now));
+ });
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
@@ -232,7 +232,6 @@ namespace armarx::armem::server::obj::instance
debugObserver->setDebugChannel(getName(),
{
{ "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
});
}
@@ -243,12 +242,12 @@ namespace armarx::armem::server::obj::instance
{
TIMING_START(GetObjectPoses);
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(segment.mutex());
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now)));
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now, &providerName]()
+ {
+ return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
+ });
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
@@ -257,7 +256,6 @@ namespace armarx::armem::server::obj::instance
debugObserver->setDebugChannel(getName(),
{
{ "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
});
}
@@ -296,26 +294,28 @@ namespace armarx::armem::server::obj::instance
}
else
{
- std::scoped_lock lock(segment.mutex());
- for (const auto& objectID : input.request.objectIDs)
+ segment.doLocked([&]()
{
- bool found = true;
- for (const auto& [providerName, info] : segment.providers)
+ for (const auto& objectID : input.request.objectIDs)
{
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
{
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ {
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
+ {
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
}
}
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
+ });
}
for (const auto& [providerName, request] : providerRequests)
@@ -343,69 +343,84 @@ namespace armarx::armem::server::obj::instance
objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.providers;
+ return segment.doLocked([this]()
+ {
+ return segment.providers;
+ });
}
Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return simox::alg::get_keys(segment.providers);
+ return segment.doLocked([this]()
+ {
+ return simox::alg::get_keys(segment.providers);
+ });
}
objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.getProviderInfo(providerName);
+ return segment.doLocked([this, &providerName]()
+ {
+ return segment.getProviderInfo(providerName);
+ });
}
bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.providers.count(providerName) > 0;
+ return segment.doLocked([this, &providerName]()
+ {
+ return segment.providers.count(providerName) > 0;
+ });
}
objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.attachObjectToRobotNode(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.attachObjectToRobotNode(input);
+ });
}
objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.detachObjectFromRobotNode(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.detachObjectFromRobotNode(input);
+ });
}
objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
- return segment.detachAllObjectsFromRobotNodes(input);
+ return segment.doLocked([this, &input]()
+ {
+ return segment.detachAllObjectsFromRobotNodes(input);
+ });
}
objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
{
- std::scoped_lock lock(segment.mutex());
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
- for (VirtualRobot::RobotPtr agent : agents)
+ return segment.doLocked([this]()
{
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ });
}
@@ -433,9 +448,8 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseMap objectPoses;
visu.minConfidence = -1;
+ return segment.doLocked([this, &objectPoses, &objectFinder]()
{
- std::scoped_lock lock(segment.mutex());
-
const IceUtil::Time now = TimeUtil::GetTime();
// Also include decayed objects in result
@@ -452,7 +466,7 @@ namespace armarx::armem::server::obj::instance
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
- }
+ });
const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
@@ -514,8 +528,10 @@ namespace armarx::armem::server::obj::instance
}
this->segment.update(adapter.segment);
{
- std::scoped_lock lock(adapter.segment.mutex());
- this->decay.update(adapter.segment.decay);
+ adapter.segment.doLocked([this, &adapter]()
+ {
+ this->decay.update(adapter.segment.decay);
+ });
}
{
std::scoped_lock lock(adapter.robotHeadMutex);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 8fd1077e58c9c13ca6eeb3b429aacf569c837950..9925ab30ff83b84d1e8927c51c16fddc6536b185 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -98,8 +98,10 @@ namespace armarx::armem::server::robot_state::description
Segment::RobotDescriptionMap Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotDescriptions(timestamp);
+ return doLocked([this, ×tamp]()
+ {
+ return getRobotDescriptions(timestamp);
+ });
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 4eb19d15583fd12493d5dd2a6312c6154931e15f..4d2fa4b85d69a1bb5aef0871af5e455490447ea2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -55,8 +55,10 @@ namespace armarx::armem::server::robot_state::localization
Segment::RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotFramePoses(timestamp);
+ return this->doLocked([this, ×tamp]()
+ {
+ return getRobotFramePoses(timestamp);
+ });
}
@@ -99,8 +101,10 @@ namespace armarx::armem::server::robot_state::localization
Segment::RobotPoseMap
Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
{
- std::scoped_lock lock(mutex());
- return getRobotGlobalPoses(timestamp);
+ return this->doLocked([this, ×tamp]()
+ {
+ return getRobotGlobalPoses(timestamp);
+ });
}
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 a63610cf6f7727feaebf60ca2139ab09c0d6e6af..8a88e6fae3a9df8980e1bb5bd54451e2457c342b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -59,14 +59,10 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
- TIMING_START(tRobotJointPositionsLock);
- std::scoped_lock lock(mutex());
- TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_DEBUG);
- if (debugObserver)
+ return doLocked([this, ×tamp, &debugObserver]()
{
- debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
- }
- return getRobotJointPositions(timestamp, debugObserver);
+ return getRobotJointPositions(timestamp, debugObserver);
+ });
}