Prepare decay thread

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp

@@ -31,6 +31,7 @@
 #include <VirtualRobot/RobotConfig.h>
 
 #include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/observers/variant/Variant.h>
 
 #include <RobotAPI/libraries/core/Pose.h>
@@ -97,6 +98,11 @@ namespace armarx
         robotHead.debugObserver = debugObserver;
         robotHead.fetchDatafields();
 
+        decay.updateTask = new SimpleRunningTask([this]()
+        {
+            this->decayUpdateRun();
+        });
+
         createRemoteGuiTab();
         RemoteGui_startRunningTask();
     }
@@ -740,6 +746,26 @@ namespace armarx
         }
     }
 
+    void ObjectPoseObserver::updateDecayRun()
+    {
+        CycleUtil cycle(static_cast<int>(1000 / decay.updateFrequencyHz));
+        while (decay.updateTask && !decay.updateTask->isStopped())
+        {
+            {
+                std::scoped_lock lock(dataMutex);
+                for (auto& [providerName, objectPoses] : data.objectPoses)
+                {
+                    for (objpose::ObjectPose& pose : objectPoses)
+                    {
+                        // ToDo: update their confidence according to their
+                        // timestamp and remove objects whose confidence is 0.
+                    }
+                }
+            }
+            cycle.waitForCycleDuration();
+        }
+    }
+
 
     objpose::ObjectPose* ObjectPoseObserver::Data::findObjectPose(const std::string& providerName, ObjectID& objectID)
     {

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h

@@ -27,6 +27,7 @@
 #include <SimoxUtility/caching/CacheMap.h>
 
 #include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 #include <ArmarXCore/observers/Observer.h>
 #include <ArmarXCore/observers/variant/DatafieldRef.h>
 
@@ -149,6 +150,9 @@ namespace armarx
         void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
                                   objpose::data::ObjectPoseSeq& result, bool& synchronized);
 
+        void updateDecayRun();
+
+
     private:
 
         DebugObserverInterfacePrx debugObserver;
@@ -181,6 +185,19 @@ namespace armarx
         Data data;
 
 
+        struct Decay
+        {
+            /// Time for an object pose to decay.
+            float durationSeconds = 20.0;
+            /// Frequency of updating the current decay.
+            float updateFrequencyHz = 10.0;
+
+            SimpleRunningTask<>::pointer_type updateTask;
+        };
+        Decay decay;
+
+
+
         class RobotHeadMovement : public armarx::Logging
         {
         public: