Add filtering of object poses

source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp

@@ -53,6 +53,8 @@ namespace armarx
         armarx::PropertyDefinitionsPtr defs =
             new ComponentPropertyDefinitions(getConfigIdentifier());
 
+        defs->optional(properties.object.maxFrequencyHz, "p.object.maxFrequency");
+
         return defs;
     }
 
@@ -212,6 +214,7 @@ namespace armarx
     ObjectInstanceToIndex::processObjectInstance(const armem::MemoryID& id,
                                                  const std::vector<armem::MemoryID>& snapshotIDs)
     {
+        std::scoped_lock lock(objectMutex);
         if (not object.has_value())
         {
             object = armem::objects::ObjectInstanceToIndex{
@@ -221,6 +224,10 @@ namespace armarx
                     armem::index::spatialSegmentID.withProviderSegmentName(getName()),
                 .indexNamedProviderSegmentID =
                     armem::index::namedSegmentID.withProviderSegmentName(getName()),
+                .params = armem::objects::ObjectInstanceToIndex::Parameters{
+                    .maxFrequency = armarx::Frequency::Hertz(properties.object.maxFrequencyHz)
+                },
+                .state = {}
             };
         }
         ARMARX_CHECK(object.has_value());

source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h

@@ -23,6 +23,7 @@
 #pragma once
 
 #include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/Frequency.h>
 
 #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
 
@@ -86,6 +87,11 @@ namespace armarx
     private:
         struct Properties
         {
+            struct Object
+            {
+                float maxFrequencyHz = 10;
+            };
+            Object object;
         };
         Properties properties;
 
@@ -93,6 +99,8 @@ namespace armarx
 
 
         armarx::plugins::ObjectPoseClientPlugin* objectClientPlugin = nullptr;
+
+        std::mutex objectMutex;
         std::optional<armem::objects::ObjectInstanceToIndex> object;
 
         armem::client::Reader robotMemoryReader;

source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp

@@ -38,22 +38,26 @@ namespace armarx::armem::objects
 
     void
     ObjectInstanceToIndex::fetchAndCommitObjectInstances(
-        const std::vector<armem::MemoryID>& objectPoseSnapshotIDs)
+        const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs)
     {
         ARMARX_CHECK(objectPoseClient.isConnected());
 
         // Fetch the latest poses.
         const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
 
-        // ToDo: Filter by snapshotIDs
+        const auto filtered = filterObjectPoses(objectPoses, updatedObjectInstanceSnapshotIDs);
+
 
         // Prepare the commit.
 
         armem::Commit commit;
 
-        for (const objpose::ObjectPose& objectPose : objectPoses)
+        for (const objpose::ObjectPose* objectPosePtr : filtered)
         {
-            armem::MemoryID objectInstanceID = armem::objects::reconstructObjectInstanceID(objectPose);
+            const objpose::ObjectPose& objectPose = *objectPosePtr;
+
+            armem::MemoryID objectInstanceID =
+                armem::objects::reconstructObjectInstanceID(objectPose);
 
             // Spatial
 
@@ -69,7 +73,8 @@ namespace armarx::armem::objects
                 toAron(spatial.aabbGlobal, aabb);
 
                 armem::EntityUpdate& update = commit.add();
-                update.entityID = indexSpatialProviderSegmentID.withEntityName(objectInstanceID.getEntityID().str());
+                update.entityID = indexSpatialProviderSegmentID.withEntityName(
+                    objectInstanceID.getEntityID().str());
                 update.timeCreated = objectPose.timestamp;
                 update.instancesData = {spatial.toAron()};
             }
@@ -77,7 +82,8 @@ namespace armarx::armem::objects
             // Named
 
             // Load object class information.
-            std::optional<ObjectInfo> info = objectPoseClient.getObjectFinder().findObject(objectPose.objectID);
+            std::optional<ObjectInfo> info =
+                objectPoseClient.getObjectFinder().findObject(objectPose.objectID);
             if (info.has_value())
             {
                 std::optional<std::vector<std::string>> recognized, spoken;
@@ -93,7 +99,7 @@ namespace armarx::armem::objects
                 }
                 else
                 {
-                    named.names.recognized = { info->className() };
+                    named.names.recognized = {info->className()};
                 }
 
                 if (spoken.has_value())
@@ -102,11 +108,12 @@ namespace armarx::armem::objects
                 }
                 else
                 {
-                    named.names.spoken = { info->className() };
+                    named.names.spoken = {info->className()};
                 }
 
                 armem::EntityUpdate& update = commit.add();
-                update.entityID = indexNamedProviderSegmentID.withEntityName(objectInstanceID.getEntityID().str());
+                update.entityID = indexNamedProviderSegmentID.withEntityName(
+                    objectInstanceID.getEntityID().str());
                 update.timeCreated = objectPose.timestamp;
                 update.instancesData = {named.toAron()};
             }
@@ -116,4 +123,55 @@ namespace armarx::armem::objects
         indexSpatialMemoryWriter.commit(commit);
     }
 
-} // namespace armarx
+
+    std::vector<const objpose::ObjectPose*>
+    ObjectInstanceToIndex::filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
+                                             const std::vector<MemoryID>& updatedSnapshotIDs)
+    {
+        // Returns true to keep the item, false to skip it.
+        auto filter = [this, &updatedSnapshotIDs](const objpose::ObjectPose& objectPose)
+        {
+            auto it = state.latestUpdateDateTimes.find(objectPose.objectID);
+            if (it == state.latestUpdateDateTimes.end())
+            {
+                // Never encountered that before, commit it.
+                return true;
+            }
+
+            const armarx::DateTime& latestTime = it->second;
+            armarx::DateTime nextDueTime = latestTime + params.maxFrequency.toCycleDuration();
+            if (objectPose.timestamp < nextDueTime)
+            {
+                // Skip.
+                return false;
+            }
+
+            armem::MemoryID objectInstanceID =
+                armem::objects::reconstructObjectInstanceID(objectPose);
+            bool found = false;
+            for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+            {
+                if (armem::contains(updatedSnapshotID, objectInstanceID))
+                {
+                    found = true;
+                    break;
+                }
+            }
+            return found;
+        };
+
+
+        std::vector<const objpose::ObjectPose*> filtered;
+        for (const objpose::ObjectPose& objectPose : objectPoses)
+        {
+            if (filter(objectPose))
+            {
+                filtered.push_back(&objectPose);
+                state.latestUpdateDateTimes[objectPose.objectID] = objectPose.timestamp;
+            }
+        }
+
+        return filtered;
+    }
+
+} // namespace armarx::armem::objects

source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h

@@ -24,6 +24,8 @@
 
 #include <vector>
 
+#include <ArmarXCore/core/time/Frequency.h>
+
 #include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
 #include <RobotAPI/libraries/armem/client/Writer.h>
 #include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -35,9 +37,13 @@ namespace armarx::armem::objects
     class ObjectInstanceToIndex
     {
     public:
-        void
-        fetchAndCommitObjectInstances(const std::vector<armem::MemoryID>& objectPoseSnapshotIDs);
+        void fetchAndCommitObjectInstances(
+            const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
 
+    private:
+        std::vector<const objpose::ObjectPose*>
+        filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
+                          const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
 
     public:
         objpose::ObjectPoseClient objectPoseClient;
@@ -45,6 +51,19 @@ namespace armarx::armem::objects
 
         armem::MemoryID indexSpatialProviderSegmentID;
         armem::MemoryID indexNamedProviderSegmentID;
+
+
+        struct Parameters
+        {
+            armarx::Frequency maxFrequency = armarx::Frequency::HertzDouble(60);
+        };
+        Parameters params;
+
+        struct State
+        {
+            std::map<armarx::ObjectID, armarx::DateTime> latestUpdateDateTimes;
+        };
+        State state;
     };
 
-} // namespace armarx
+} // namespace armarx::armem::objects

source/RobotAPI/interface/aron/Aron.ice

@@ -170,6 +170,7 @@ module armarx
 
                 // useful for memory ice_conversions
                 sequence<Dict> AronDictSeq;
+                sequence<Dict> DictSeq;
             };
         };
     };