From 1aa79350c914534667ea724e7c225796136a0b9d Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 13 Oct 2020 09:41:45 +0200
Subject: [PATCH] Refactor and fix attachment and detachment
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 156 +++++++++++++-----
.../ObjectPoseObserver/ObjectPoseObserver.h | 11 +-
2 files changed, 127 insertions(+), 40 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index bd8d091c8..a6b0afcdd 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -205,7 +205,7 @@ namespace armarx
{
if (!info.proxy)
{
- ARMARX_WARNING << "Received availability signal from provider '" << providerName << "' "
+ ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
<< "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
return;
}
@@ -238,12 +238,12 @@ namespace armarx
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
// IceUtil::Time start = IceUtil::Time::now();
- std::scoped_lock lock(dataMutex, robotMutex);
+ std::scoped_lock lock(dataMutex);
// ARMARX_INFO << "Locking took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
// start = IceUtil::Time::now();
bool synchronized = false;
objpose::data::ObjectPoseSeq result;
- for (const auto& [name, poses] : objectPoses)
+ for (auto& [name, poses] : objectPoses)
{
toIceWithAttachments(poses, robot, result, synchronized);
}
@@ -253,7 +253,7 @@ namespace armarx
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex, robotMutex);
+ std::scoped_lock lock(dataMutex);
bool synchronized = false;
objpose::data::ObjectPoseSeq result;
toIceWithAttachments(objectPoses.at(providerName), robot, result, synchronized);
@@ -318,7 +318,7 @@ namespace armarx
{
if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
{
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects from provider '"
+ ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
<< providerName << "' for " << request.relativeTimeoutMS << " ms.";
objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
@@ -378,7 +378,7 @@ namespace armarx
objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex, robotMutex);
+ std::scoped_lock lock(dataMutex);
objpose::AttachObjectToRobotNodeOutput output;
output.success = false; // We are not successful until proven otherwise.
@@ -412,8 +412,7 @@ namespace armarx
return output;
}
- currentObjectPose->attachment = objpose::ObjectAttachmentInfo();
- objpose::ObjectAttachmentInfo& info = *currentObjectPose->attachment;
+ objpose::ObjectAttachmentInfo info;
info.agentName = agent->getName();
info.frameName = frameName;
@@ -436,6 +435,11 @@ namespace armarx
info.poseInFrame = framed.toEigen();
}
}
+ attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
+
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << currentObjectPose->providerName << "' "
+ << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
+ << "Object pose in frame: \n" << info.poseInFrame;
output.success = true;
output.attachment = new objpose::data::ObjectAttachmentInfo();
@@ -449,44 +453,90 @@ namespace armarx
objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex);
objpose::DetachObjectFromRobotNodeOutput output;
ObjectID objectID = armarx::fromIce(input.objectID);
+ std::string providerName = input.providerName;
+
+ std::optional<objpose::ObjectAttachmentInfo> attachment;
{
std::scoped_lock lock(dataMutex);
+
+ // Remove from latest pose (if it was cached).
objpose::ObjectPose* objectPose = findObjectPose(input.providerName, objectID);
- if (!objectPose)
+ if (objectPose)
{
- output.wasAttached = false;
+ objectPose->attachment = std::nullopt;
+ }
+
+ if (providerName.empty() && objectPose)
+ {
+ providerName = objectPose->providerName;
+ }
+ // Remove from attachment map.
+ if (input.providerName.size() > 0)
+ {
+ auto it = attachments.find(std::make_pair(input.providerName, objectID));
+ if (it != attachments.end())
+ {
+ attachment = it->second;
+ attachments.erase(it);
+ }
}
else
{
- output.wasAttached = bool(objectPose->attachment);
- objectPose->attachment = std::nullopt;
+ // Search for entry with matching object ID.
+ for (auto it = attachments.begin(); it != attachments.end(); ++it)
+ {
+ const ObjectID& id = it->first.second;
+ if (id == objectID)
+ {
+ attachment = it->second;
+ attachments.erase(it);
+ break;
+ }
+ }
}
}
+ if (attachment)
+ {
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
+ << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ output.wasAttached = true;
+ }
+ else
+ {
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ << "from robot node, but it was not attached.";
+ }
+
return output;
}
objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
{
- std::scoped_lock lock(dataMutex);
objpose::DetachAllObjectsFromRobotNodesOutput output;
output.numDetached = 0;
- for (auto& [prov, poses] : objectPoses)
{
- for (auto& pose : poses)
+ std::scoped_lock lock(dataMutex);
+
+ // Clear attachment map.
+ size_t num = attachments.size();
+ attachments.clear();
+ output.numDetached = int(num);
+
+ // Remove from poses (if it was cached).
+ for (auto& [prov, poses] : objectPoses)
{
- if (pose.attachment)
+ for (auto& pose : poses)
{
pose.attachment = std::nullopt;
- output.numDetached++;
}
}
}
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
return output;
}
@@ -520,12 +570,30 @@ namespace armarx
{
viz::Layer layer = arviz.layer(providerName);
- for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
+ bool synchronized = false;
+
+ for (objpose::ObjectPose& objectPose : objectPoses.at(providerName))
{
const armarx::ObjectID id = objectPose.objectID;
std::string key = id.str();
- Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ if (isAttachedCached(objectPose))
+ {
+ if (!synchronized)
+ {
+ RobotState::synchronizeLocalClone(robot);
+ synchronized = true;
+ }
+ objpose::ObjectPose updated = applyAttachment(objectPose, robot);
+ pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
+ }
+ else
+ {
+ pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ }
+
{
viz::Object object = viz::Object(key).pose(pose);
if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
@@ -547,7 +615,7 @@ namespace armarx
{
const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
layer.add(viz::Box(key + " OOBB")
- .pose(pose * simox::math::pose(oobb.center(), oobb.rotation()))
+ .pose(pose * oobb.transformation())
.size(oobb.dimensions()).color(simox::Color::lime(255, 64)));
}
if (visu.objectFrames)
@@ -623,36 +691,50 @@ namespace armarx
return nullptr;
}
+ bool ObjectPoseObserver::isAttachedCached(objpose::ObjectPose& objectPose) const
+ {
+ if (!objectPose.attachment)
+ {
+ auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
+ if (it != attachments.end())
+ {
+ objectPose.attachment = it->second;
+ }
+ }
+ return bool(objectPose.attachment);
+ }
+
objpose::ObjectPose ObjectPoseObserver::applyAttachment(
- const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot)
+ const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr agent)
{
+ ARMARX_CHECK(objectPose.attachment);
+
objpose::ObjectPose updated = objectPose;
- if (objectPose.attachment)
- {
- VirtualRobot::RobotPtr agent = robot;
- ARMARX_CHECK_EQUAL(objectPose.attachment->agentName, agent->getName());
+ const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
- VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(objectPose.attachment->frameName);
- ARMARX_CHECK_NOT_NULL(robotNode);
+ ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
- // Overwrite object pose
- updated.objectPoseRobot = robotNode->getPoseInRootFrame() * objectPose.attachment->poseInFrame;
- updated.objectPoseGlobal = robot->getGlobalPose() * updated.objectPoseRobot;
+ VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment.frameName);
+ ARMARX_CHECK_NOT_NULL(robotNode);
+
+ // Overwrite object pose
+ updated.objectPoseRobot = robotNode->getPoseInRootFrame() * attachment.poseInFrame;
+ updated.objectPoseGlobal = agent->getGlobalPose() * updated.objectPoseRobot;
+
+ updated.robotPose = agent->getGlobalPose();
+ updated.robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
- updated.robotPose = robot->getGlobalPose();
- updated.robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
- }
return updated;
}
void ObjectPoseObserver::toIceWithAttachments(
- const objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
+ objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
objpose::data::ObjectPoseSeq& result, bool& synchronized)
{
- for (const objpose::ObjectPose& pose : objectPoses)
+ for (objpose::ObjectPose& pose : objectPoses)
{
- if (pose.attachment)
+ if (isAttachedCached(pose))
{
if (!synchronized) // Synchronize only once.
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index c02238075..ad7d2e3de 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -141,23 +141,28 @@ namespace armarx
objpose::ObjectPose* findObjectPose(const std::string& providerName, ObjectID& objectID);
static objpose::ObjectPose* findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id);
+
+ bool isAttachedCached(objpose::ObjectPose& objectPose) const;
static objpose::ObjectPose applyAttachment(const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot);
- void toIceWithAttachments(const objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
+
+ void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
objpose::data::ObjectPoseSeq& result, bool& synchronized);
private:
- std::mutex robotMutex;
+ std::mutex dataMutex;
+
VirtualRobot::RobotPtr robot;
- std::mutex dataMutex;
objpose::ProviderInfoMap providers;
std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
std::map<std::string, int> updateCounters;
+ std::map<std::pair<std::string, ObjectID>, objpose::ObjectAttachmentInfo> attachments;
+
ObjectFinder objectFinder;
/// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
--
GitLab