From c0b9a10e123298500f2231df252189fe46082208 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 1 Dec 2022 15:50:25 +0100
Subject: [PATCH] Add filtering of object poses
---
.../ObjectInstanceToIndex.cpp | 7 ++
.../ObjectInstanceToIndex.h | 8 ++
.../impl/ObjectInstanceToIndex.cpp | 78 ++++++++++++++++---
.../impl/ObjectInstanceToIndex.h | 25 +++++-
source/RobotAPI/interface/aron/Aron.ice | 1 +
5 files changed, 106 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
index 2c1d33955..fedf604b0 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
+++ b/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());
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
index c3a29df6f..c5e6486e7 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
+++ b/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;
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
index c3a0779a0..4498d01ce 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
+++ b/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
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
index 41ba28b79..dd8f29399 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
+++ b/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
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index c766750c2..fb412c46c 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -170,6 +170,7 @@ module armarx
// useful for memory ice_conversions
sequence<Dict> AronDictSeq;
+ sequence<Dict> DictSeq;
};
};
};
--
GitLab