Update usages

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);

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);
             }

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);
         }

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

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));
+                }
+            });
         }
     }
 

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();
+            });
         }
     }
 

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);

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, &timestamp]()
+        {
+            return getRobotDescriptions(timestamp);
+        });
     }
 
 

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, &timestamp]()
+        {
+            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, &timestamp]()
+        {
+            return getRobotGlobalPoses(timestamp);
+        });
     }
 
 

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, &timestamp, &debugObserver]()
         {
-            debugObserver->setDebugObserverDatafield(dp + "t 0 Lock Core Segment (ms)", tRobotJointPositionsLock.toMilliSecondsDouble());
-        }
-        return getRobotJointPositions(timestamp, debugObserver);
+            return getRobotJointPositions(timestamp, debugObserver);
+        });
     }