From 3e4a8c292027256fcf5dbb147e5d898f5c4b92f6 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 19 Feb 2021 18:45:17 +0100
Subject: [PATCH] Prepare decay thread
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 26 +++++++++++++++++++
.../ObjectPoseObserver/ObjectPoseObserver.h | 17 ++++++++++++
2 files changed, 43 insertions(+)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 28fd1a789..069df7e68 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 190b0828e..2db380bf1 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:
--
GitLab