From b0f27d5b3a5d79aeb165d220db938a7a4a5b5427 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 21 Apr 2021 17:54:40 +0200
Subject: [PATCH] Improve/fix handling of attachment and updates
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 21 +-
.../ObjectPoseObserver/ObjectPoseObserver.h | 3 +-
.../ObjectPoseObserver/detail/Data.cpp | 383 +++++++++++-------
.../ObjectPoseObserver/detail/Data.h | 75 +++-
.../ObjectPoseObserver/detail/Visu.cpp | 11 +-
.../ObjectPoseObserverInterface.ice | 17 +-
6 files changed, 320 insertions(+), 190 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 30b7449f0..de71bb557 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -407,10 +407,11 @@ namespace armarx
return data.detachObjectFromRobotNode(input);
}
- objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
+ objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- return data.detachAllObjectsFromRobotNodes();
+ return data.detachAllObjectsFromRobotNodes(input);
}
@@ -492,10 +493,15 @@ namespace armarx
using namespace armarx::RemoteGui::Client;
tab.visu.setup(this->visu);
+ tab.data.setup(this->data);
tab.decay.setup(this->data.decay);
tab.robotHead.setup(this->robotHead);
- VBoxLayout root = {tab.visu.group, tab.decay.group, tab.robotHead.group, VSpacer()};
+ VBoxLayout root =
+ {
+ tab.visu.group, tab.data.group, tab.decay.group, tab.robotHead.group,
+ VSpacer()
+ };
RemoteGui_createTab(getName(), root, &tab);
}
@@ -506,14 +512,15 @@ namespace armarx
std::scoped_lock lock(visuMutex);
tab.visu.update(this->visu);
}
- {
- std::scoped_lock lock(robotHeadMutex);
- tab.robotHead.update(this->robotHead);
- }
{
std::scoped_lock lock(dataMutex);
+ tab.data.update(this->data);
tab.decay.update(this->data.decay);
}
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ tab.robotHead.update(this->robotHead);
+ }
}
void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index c079245f7..c0a7025dc 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -103,7 +103,7 @@ namespace armarx
objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(ICE_CURRENT_ARG) override;
+ objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
@@ -174,6 +174,7 @@ namespace armarx
struct RemoteGuiTab : RemoteGui::Client::Tab
{
objpose::observer::Visu::RemoteGui visu;
+ objpose::observer::Data::RemoteGui data;
objpose::observer::Decay::RemoteGui decay;
objpose::observer::RobotHeadMovement::RemoteGui robotHead;
};
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
index 88080c61e..f98b4083c 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
@@ -8,6 +8,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/core/Visitor.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -24,12 +25,43 @@ namespace armarx::objpose::observer
memoryToIceAdapter(memoryToIceAdapter)
{
memory.name() = defaultMemoryName;
+
+ oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ {
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ return objectInfo->loadOOBB();
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
+ return std::nullopt;
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ });
+
+ classNameToDatasetCache.setFetchFn([this](const std::string & className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
}
void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(memory.name(), "mem.MemoryName", "Name of this memory server.");
defs->optional(maxHistorySize, "mem.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ defs->optional(discardSnapshotsWhileAttached, "mem.DiscardSnapshotsWhileAttached",
+ "If true, no new snapshots are stored while an object is attached to a robot node.\n"
+ "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
decay.defineProperties(defs, prefix + "decay.");
}
@@ -48,14 +80,6 @@ namespace armarx::objpose::observer
{
CommitStats stats;
- // This stays empty on the first report.
- const armem::CoreSegment& coreSegment = getCoreSegment();
- armem::ProviderSegment const* providerSegment = nullptr;
- if (coreSegment.hasProviderSegment(providerName))
- {
- providerSegment = &coreSegment.getProviderSegment(providerName);
- }
-
// Build new poses.
objpose::ObjectPoseSeq newObjectPoses;
stats.numUpdated = 0;
@@ -63,48 +87,59 @@ namespace armarx::objpose::observer
{
const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
- // Check whether we have an old pose for this object.
+ // Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
- const std::string entityName = armarx::fromIce(provided.objectID).str();
- if (providerSegment && providerSegment->hasEntity(entityName))
+ const armem::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ if (entity)
{
- const armem::Entity& entity = providerSegment->getEntity(entityName);
- const objpose::arondto::ObjectPose data = objpose::observer::Data::getLatestInstanceData(entity);
+ const objpose::arondto::ObjectPose data = objpose::observer::Data::getLatestInstanceData(*entity);
previousPose = objpose::ObjectPose();
objpose::fromAron(data, *previousPose);
}
+ bool discard = false;
if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
{
- if (previousPose)
+ // Dicard updates temporarily (e.g. due to head movement).
+ discard = true;
+ }
+ else if (previousPose)
+ {
+ if (discardSnapshotsWhileAttached && previousPose->attachment)
{
- // Keep the old one
- newObjectPoses.push_back(*previousPose);
+ // Discard update due to active attachemnt.
+ discard = true;
}
- else
+ else if (timestamp == previousPose->timestamp)
{
- // Discard the new pose.
- // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
- // << "timestamp " << timestamp << " < " << robotHead.discard.updatesUntil;
+ // Discard update as it is not new.
+ discard = true;
}
}
- else if (previousPose && timestamp == previousPose->timestamp)
- {
- // Keep the old one.
- newObjectPoses.push_back(*previousPose);
- }
- else
+
+ if (!discard)
{
+ // Update the entity.
stats.numUpdated++;
+
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
newPose.fromProvidedPose(provided, robot);
+
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
if (newPose.objectID.dataset().empty())
{
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(newPose.objectID))
+ // Try to find the data set.
+ const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
{
- newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
+ newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
}
}
if (!provided.localOOBB)
@@ -165,8 +200,9 @@ namespace armarx::objpose::observer
ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
{
- ObjectPoseSeq latestObjectPoses = getLatestObjectPoses();
- return updateAndFilterObjectPoses(latestObjectPoses, now);
+ ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
}
@@ -176,7 +212,8 @@ namespace armarx::objpose::observer
{
ARMARX_CHECK_NOT_NULL(coreSegment);
ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
- return updateAndFilterObjectPoses(objectPoses, now);
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
}
armem::Entity* Data::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
@@ -192,14 +229,31 @@ namespace armarx::objpose::observer
return &prov.getEntity(entityID);
}
}
-
- throw armem::error::MissingEntry("entity", entityID.entityName,
- "any provider segment of core segment", coreSegment->name());
+ return nullptr;
}
else
{
entityID.providerSegmentName = providerName;
- return &coreSegment->getEntity(entityID);
+ if (coreSegment->hasProviderSegment(providerName))
+ {
+ armem::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
+ return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+ }
+
+
+ void Data::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ {
+ bool agentSynchronized = false;
+
+ for (ObjectPose& objectPose : objectPoses)
+ {
+ updateObjectPose(objectPose, now, robot, agentSynchronized);
}
}
@@ -232,23 +286,27 @@ namespace armarx::objpose::observer
}
+ ObjectPoseSeq Data::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ {
+ ObjectPoseSeq result;
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ return result;
+ }
+
+
void Data::updateAttachement(
ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
{
if (!objectPose.attachment)
{
- // Fetch attachment info from internal data structure.
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- // Store it in the objectPose.
- objectPose.attachment = it->second;
- }
- else
- {
- // No attachment, nothing to do.
- return;
- }
+ // No attachment, nothing to do.
+ return;
}
ARMARX_CHECK(objectPose.attachment);
@@ -266,22 +324,6 @@ namespace armarx::objpose::observer
return getLatestObjectPoses(*coreSegment);
}
- ObjectPoseSeq Data::updateAndFilterObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now) const
- {
- bool agentSynchronized = false;
-
- ObjectPoseSeq result;
- for (ObjectPose& objectPose : objectPoses)
- {
- updateObjectPose(objectPose, now, robot, agentSynchronized);
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- return result;
- }
-
ObjectPoseSeq Data::getLatestObjectPoses(const armem::CoreSegment& coreSeg)
{
ObjectPoseSeq result;
@@ -349,30 +391,9 @@ namespace armarx::objpose::observer
return data;
}
-
std::optional<simox::OrientedBoxf> Data::getObjectOOBB(const ObjectID& id)
{
- return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- try
- {
- return objectInfo->loadOOBB();
- }
- catch (const std::ios_base::failure& e)
- {
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- return std::nullopt;
- }
- }
- else
- {
- return std::nullopt;
- }
- });
+ return oobbCache.get(id);
}
ProviderInfo Data::getProviderInfo(const std::string& providerName)
@@ -457,7 +478,6 @@ namespace armarx::objpose::observer
info.poseInFrame = framed.toEigen();
}
}
- this->attachments[std::make_pair(data.providerName, objectID)] = info;
// Store attachment in new entity snapshot.
{
@@ -486,41 +506,28 @@ namespace armarx::objpose::observer
return output;
}
- DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input)
+ DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(
+ const DetachObjectFromRobotNodeInput& input)
{
const armem::Time now = armem::Time::now();
ObjectID objectID = armarx::fromIce(input.objectID);
std::string providerName = input.providerName;
- std::optional<ObjectAttachmentInfo> attachment;
+ std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
{
// Remove from latest pose (if it was cached).
// Find object pose (provider name can be empty).
- armem::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
- if (objectEntity)
+ armem::Entity* entity = this->findObjectEntity(objectID, input.providerName);
+ if (entity)
{
- ARMARX_CHECK_GREATER_EQUAL(objectEntity->size(), 1);
- armem::EntitySnapshot& snapshot = objectEntity->getLatestSnapshot();
-
- ARMARX_CHECK_EQUAL(snapshot.size(), 1);
- armem::EntityInstance& instance = snapshot.getInstance(0);
-
- arondto::ObjectPose data;
- data.fromAron(instance.data());
-
- // Store non-attached pose in new snapshot.
+ const arondto::ObjectPose data = getLatestInstanceData(*entity);
+ if (data.attachmentValid)
{
- armem::EntityUpdate update;
- update.entityID = objectEntity->id();
- update.timeCreated = now;
- {
- arondto::ObjectPose updated = data;
- updated.attachmentValid = false;
- toAron(updated.attachment, ObjectAttachmentInfo{});
- update.instancesData = { updated.toAron() };
- }
- objectEntity->update(update);
+ attachment = data.attachment;
+
+ // Store non-attached pose in new snapshot.
+ storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
}
if (providerName.empty())
@@ -528,31 +535,6 @@ namespace armarx::objpose::observer
providerName = data.providerName;
}
}
-
- // Remove from attachment map.
- if (input.providerName.size() > 0)
- {
- auto it = this->attachments.find(std::make_pair(input.providerName, objectID));
- if (it != this->attachments.end())
- {
- attachment = it->second;
- this->attachments.erase(it);
- }
- }
- else
- {
- // Search for entry with matching object ID.
- for (auto it = this->attachments.begin(); it != this->attachments.end(); ++it)
- {
- const ObjectID& id = it->first.second;
- if (id == objectID)
- {
- attachment = it->second;
- this->attachments.erase(it);
- break;
- }
- }
- }
}
DetachObjectFromRobotNodeOutput output;
@@ -571,38 +553,125 @@ namespace armarx::objpose::observer
return output;
}
- DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes()
+
+ struct DetachVisitor : public armem::Visitor
+ {
+ Data& owner;
+ armem::Time now;
+ bool commitAttachedPose;
+
+ int numDetached = 0;
+
+ DetachVisitor(Data& owner, armem::Time now, bool commitAttachedPose) :
+ owner(owner), now(now), commitAttachedPose(commitAttachedPose)
+ {
+ }
+
+ virtual bool visitEnter(armem::Entity& entity) override;
+ };
+
+ bool DetachVisitor::visitEnter(armem::Entity& entity)
+ {
+ const arondto::ObjectPose data = owner.getLatestInstanceData(entity);
+ if (data.attachmentValid)
+ {
+ numDetached++;
+ // Store non-attached pose in new snapshot.
+ owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
+ }
+
+ return false; // Stop descending.
+ }
+
+
+ DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes(
+ const DetachAllObjectsFromRobotNodesInput& input)
{
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ const armem::Time now = armem::Time::now();
+
+ DetachVisitor visitor(*this, now, input.commitAttachedPose);
+ visitor.applyTo(*coreSegment);
+
DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = int(this->attachments.size());
+ output.numDetached = visitor.numDetached;
- // Clear attachment map.
- this->attachments.clear();
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
- // Remove from poses (if it was cached).
- for (auto& [_, prov] : *coreSegment)
+ return output;
+ }
+
+ void Data::storeDetachedSnapshot(
+ armem::Entity& entity,
+ const arondto::ObjectPose& data,
+ armem::Time now,
+ bool commitAttachedPose)
+ {
+ armem::EntityUpdate update;
+ update.entityID = entity.id();
+ update.timeCreated = now;
{
- for (auto& [_, entity] : prov)
+ arondto::ObjectPose updated;
+ if (commitAttachedPose && data.attachmentValid)
{
- if (!entity.empty())
- {
- armem::EntitySnapshot& snapshot = entity.getLatestSnapshot();
- if (!snapshot.empty())
- {
- armem::EntityInstance& instance = snapshot.getInstance(0);
+ ObjectPose objectPose;
+ fromAron(data, objectPose);
- arondto::ObjectPose dto;
- dto.fromAron(instance.data());
- dto.attachmentValid = false;
- instance.setData(dto.toAron());
- }
- }
+ bool agentSynchronized = false;
+ updateAttachement(objectPose, robot, agentSynchronized);
+
+ objectPose.attachment = std::nullopt;
+ toAron(updated, objectPose);
}
+ else
+ {
+ updated = data;
+ updated.attachmentValid = false;
+ toAron(updated.attachment, ObjectAttachmentInfo{});
+ }
+
+ update.instancesData = { updated.toAron() };
}
+ entity.update(update);
+ }
- ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
- return output;
+
+ void Data::RemoteGui::setup(const Data& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ maxHistorySize.setValue(std::max(1, int(data.maxHistorySize)));
+ maxHistorySize.setRange(1, 1e6);
+ infiniteHistory.setValue(data.maxHistorySize == -1);
+ discardSnapshotsWhileAttached.setValue(data.discardSnapshotsWhileAttached);
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ row++;
+ grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ row++;
+ grid.add(Label("Discard Snapshots while attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ row++;
+
+ group.setLabel("Data");
+ group.addChild(grid);
+ }
+
+ void Data::RemoteGui::update(Data& data)
+ {
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ {
+ data.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (data.coreSegment)
+ {
+ data.coreSegment->setMaxHistorySize(long(data.maxHistorySize));
+ }
+ }
+
+ data.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
}
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
index 99e5926fd..95e6cc78a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
@@ -73,22 +73,9 @@ namespace armarx::objpose::observer
AttachObjectToRobotNodeOutput attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input);
DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input);
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const DetachAllObjectsFromRobotNodesInput& input);
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
-
/**
* @brief If the object is attached to a robot node, update it according to the current robot state.
*
@@ -117,7 +104,35 @@ namespace armarx::objpose::observer
private:
ObjectPoseSeq getLatestObjectPoses() const;
- ObjectPoseSeq updateAndFilterObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
+
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now);
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+ void updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+
+
+ ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+
+
+ void storeDetachedSnapshot(
+ armem::Entity& entity,
+ const arondto::ObjectPose& data,
+ armem::Time now,
+ bool commitAttachedPose);
+
+
+ friend struct DetachVisitor;
public:
@@ -128,12 +143,7 @@ namespace armarx::objpose::observer
ProviderInfoMap providers;
- std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments;
-
-
ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
/// Decay model.
Decay decay;
@@ -145,7 +155,32 @@ namespace armarx::objpose::observer
armem::server::MemoryToIceAdapter& memoryToIceAdapter;
armem::CoreSegment* coreSegment = nullptr;
+
+
long maxHistorySize = -1;
+ bool discardSnapshotsWhileAttached = true;
+
+
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ /// Class name -> dataset name.
+ simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ void setup(const Data& data);
+ void update(Data& data);
+ };
};
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
index 11f8a3ebb..3aaa36feb 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
@@ -1,5 +1,7 @@
#include "Visu.h"
+#include <SimoxUtility/math/pose.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
@@ -91,7 +93,7 @@ namespace armarx::objpose::observer
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
- Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
{
viz::Object object = viz::Object(key).pose(pose);
if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
@@ -115,9 +117,10 @@ namespace armarx::objpose::observer
if (oobbs && objectPose.localOOBB)
{
- const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
- layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
- .color(simox::Color::lime(255, 64)));
+ const simox::OrientedBoxf oobb = inGlobalFrame
+ ? objectPose.oobbGlobal().value()
+ : objectPose.oobbRobot().value();
+ layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
}
if (objectFrames)
{
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
index 6fe63e5f9..3f047323b 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserverInterface.ice
@@ -89,12 +89,27 @@ module armarx
{
string providerName;
armarx::data::ObjectID objectID;
+
+ /**
+ * @brief If true, the object will stay at the position before
+ * detaching until it is provided again.
+ */
+ bool commitAttachedPose = true;
};
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
bool wasAttached;
};
+
+ struct DetachAllObjectsFromRobotNodesInput
+ {
+ /**
+ * @brief If true, the objects will stay at the position before
+ * detaching until they are provided again.
+ */
+ bool commitAttachedPose = true;
+ }
struct DetachAllObjectsFromRobotNodesOutput
{
/// Number of objects that have been detached.
@@ -165,7 +180,7 @@ module armarx
/// Detach an attached object from a robot node.
DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
/// Detach all objects from robot nodes.
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(DetachAllObjectsFromRobotNodesInput input);
AgentFramesSeq getAttachableFrames();
--
GitLab