diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 28fd1a7898a0abb3c11c3f21a74cbaabf3a921e0..069df7e68b00c64af70e770c8be7b11a77d23096 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/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) { diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h index 190b0828eec902adfa082ccf37805edf981dcdaf..2db380bf1a38b602ca7991e050a55153696f9e49 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h +++ b/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: