diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index 6ca0338ce6c4c8edae44bb3248d9d4e8b1ea6c03..19bb9d5aac9be101f1c532279977f58a262eab72 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -56,6 +56,8 @@ namespace armarx::objpose
robotConfig = ice.robotConfig;
robotPose = armarx::objpose::toEigen(ice.robotPose);
+ objpose::fromIce(ice.attachment, this->attachment);
+
confidence = ice.confidence;
timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
@@ -83,6 +85,8 @@ namespace armarx::objpose
ice.robotConfig = robotConfig;
ice.robotPose = new Pose(robotPose);
+ objpose::toIce(ice.attachment, this->attachment);
+
ice.confidence = confidence;
ice.timestampMicroSeconds = timestamp.toMicroSeconds();
@@ -107,6 +111,8 @@ namespace armarx::objpose
robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
robotPose = robot->getGlobalPose();
+ attachment = std::nullopt;
+
confidence = provided.confidence;
timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
@@ -137,6 +143,70 @@ namespace armarx::objpose
namespace armarx
{
+ void objpose::fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment)
+ {
+ attachment.agentName = ice.agentName;
+ attachment.frameName = ice.frameName;
+ attachment.poseInFrame = toEigen(ice.poseInFrame);
+ }
+
+ void objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment)
+ {
+ if (ice)
+ {
+ attachment = ObjectAttachmentInfo();
+ fromIce(*ice, *attachment);
+ }
+ else
+ {
+ attachment = std::nullopt;
+ }
+ }
+
+ std::optional<objpose::ObjectAttachmentInfo> objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice)
+ {
+ if (!ice)
+ {
+ return std::nullopt;
+ }
+ objpose::ObjectAttachmentInfo info;
+ fromIce(*ice, info);
+ return info;
+ }
+
+
+ void objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
+ {
+ ice.agentName = attachment.agentName;
+ ice.frameName = attachment.frameName;
+ ice.poseInFrame = new Pose(attachment.poseInFrame);
+ }
+
+ void objpose::toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment)
+ {
+ if (attachment)
+ {
+ ice = new objpose::data::ObjectAttachmentInfo();
+ toIce(*ice, *attachment);
+ }
+ else
+ {
+ ice = nullptr;
+ }
+ }
+
+ objpose::data::ObjectAttachmentInfoPtr objpose::toIce(const std::optional<ObjectAttachmentInfo>& attachment)
+ {
+ if (!attachment)
+ {
+ return nullptr;
+ }
+ objpose::data::ObjectAttachmentInfoPtr ice = new objpose::data::ObjectAttachmentInfo();
+ toIce(*ice, *attachment);
+ return ice;
+ }
+
+
void objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
{
pose.fromIce(ice);
@@ -190,7 +260,3 @@ namespace armarx
}
-
-
-
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
index d00e548b82bc9bdd72a46aeb997a6eb297e89ba4..cdfd1cfdece0570cf77162549de5da7d443c0e26 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -16,6 +16,14 @@
namespace armarx::objpose
{
+ struct ObjectAttachmentInfo
+ {
+ std::string frameName;
+ std::string agentName;
+ Eigen::Matrix4f poseInFrame;
+ };
+
+
/**
* @brief An object pose as stored by the ObjectPoseObserver.
*/
@@ -48,6 +56,8 @@ namespace armarx::objpose
std::map<std::string, float> robotConfig;
Eigen::Matrix4f robotPose;
+ std::optional<ObjectAttachmentInfo> attachment;
+
/// Confidence in [0, 1] (1 = full, 0 = none).
float confidence = 0;
/// Source timestamp.
@@ -67,6 +77,10 @@ namespace armarx::objpose
using ObjectPoseSeq = std::vector<ObjectPose>;
+ void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
+ void fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment);
+ std::optional<ObjectAttachmentInfo> fromIce(const data::ObjectAttachmentInfoPtr& ice);
+
void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
ObjectPose fromIce(const data::ObjectPose& ice);
@@ -74,6 +88,10 @@ namespace armarx::objpose
ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice);
+ void toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment);
+ void toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment);
+ data::ObjectAttachmentInfoPtr toIce(const std::optional<ObjectAttachmentInfo>& ice);
+
void toIce(data::ObjectPose& ice, const ObjectPose& pose);
data::ObjectPose toIce(const ObjectPose& pose);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 892094431698411a2ad66189986a7fa0f2cbb6ef..a6b0afcdd5b61d39917be0b440c440472f31af79 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -156,6 +156,7 @@ namespace armarx
updateProviderInfo(providerName, info);
}
+
void ObjectPoseObserver::reportObjectPoses(
const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
{
@@ -204,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;
}
@@ -240,13 +241,11 @@ namespace armarx
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)
{
- for (const auto& pose : poses)
- {
- result.push_back(pose.toIce());
- }
+ toIceWithAttachments(poses, robot, result, synchronized);
}
// ARMARX_INFO << "getObjectPoses() took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
return result;
@@ -255,7 +254,10 @@ namespace armarx
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- return objpose::toIce(objectPoses.at(providerName));
+ bool synchronized = false;
+ objpose::data::ObjectPoseSeq result;
+ toIceWithAttachments(objectPoses.at(providerName), robot, result, synchronized);
+ return result;
}
@@ -316,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);
@@ -334,7 +336,6 @@ namespace armarx
return output;
}
-
objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
@@ -373,6 +374,175 @@ namespace armarx
}
+
+ objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
+ const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(dataMutex);
+
+ objpose::AttachObjectToRobotNodeOutput output;
+ output.success = false; // We are not successful until proven otherwise.
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+
+ if (input.agentName != "" && input.agentName != robot->getName())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ << "\n(You can leave the agent name empty if there is only one agent.)\n"
+ << "\nKnown agents: " << std::vector<std::string> {robot->getName()};
+ return output;
+ }
+ VirtualRobot::RobotPtr agent = this->robot;
+
+ if (!agent->hasRobotNode(input.frameName))
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
+ << "' of agent '" << agent->getName() << "'.";
+ return output;
+ }
+ std::string frameName = input.frameName;
+
+
+ // Find object pose.
+ objpose::ObjectPose* currentObjectPose = findObjectPose(input.providerName, objectID);
+ if (!currentObjectPose)
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
+ << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ return output;
+ }
+
+ objpose::ObjectAttachmentInfo info;
+ info.agentName = agent->getName();
+ info.frameName = frameName;
+
+ if (input.poseInFrame)
+ {
+ info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
+ }
+ else
+ {
+ RobotState::synchronizeLocalClone(robot);
+
+ armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ if (frameName == armarx::GlobalFrame)
+ {
+ info.poseInFrame = framed.toGlobalEigen(robot);
+ }
+ else
+ {
+ framed.changeFrame(robot, info.frameName);
+ 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();
+ output.attachment->frameName = info.frameName;
+ output.attachment->agentName = info.agentName;
+ output.attachment->poseInFrame = new Pose(info.poseInFrame);
+
+ return output;
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+ {
+ 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)
+ {
+ 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
+ {
+ // 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&)
+ {
+ objpose::DetachAllObjectsFromRobotNodesOutput output;
+ output.numDetached = 0;
+
+ {
+ 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)
+ {
+ for (auto& pose : poses)
+ {
+ pose.attachment = std::nullopt;
+ }
+ }
+ }
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
+
+ return output;
+ }
+
+
+
void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
{
// Initialized to 0 on first access.
@@ -400,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))
@@ -427,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)
@@ -470,5 +658,97 @@ namespace armarx
return oobb;
}
+ objpose::ObjectPose* ObjectPoseObserver::findObjectPose(const std::string& providerName, ObjectID& objectID)
+ {
+ objpose::ObjectPose* pose = nullptr;
+ if (!providerName.empty())
+ {
+ pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
+ }
+ else
+ {
+ for (auto& [_, poses] : objectPoses)
+ {
+ pose = findObjectPoseByID(poses, objectID);
+ if (pose)
+ {
+ break;
+ }
+ }
+ }
+ return pose;
+ }
+
+ objpose::ObjectPose* ObjectPoseObserver::findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id)
+ {
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ 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 agent)
+ {
+ ARMARX_CHECK(objectPose.attachment);
+
+ objpose::ObjectPose updated = objectPose;
+ const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
+
+ ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
+
+ 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();
+
+ return updated;
+ }
+
+
+ void ObjectPoseObserver::toIceWithAttachments(
+ objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
+ objpose::data::ObjectPoseSeq& result, bool& synchronized)
+ {
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ if (isAttachedCached(pose))
+ {
+ if (!synchronized) // Synchronize only once.
+ {
+ RobotState::synchronizeLocalClone(agent);
+ synchronized = true;
+ }
+ result.push_back(applyAttachment(pose, agent).toIce());
+ }
+ else
+ {
+ result.push_back(pose.toIce());
+ }
+ }
+ }
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 687db8ec03dfc84ab83a9e57da6e443d59e23dc5..ad7d2e3de5016685a58e2a65f1921b59cf16d2c7 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -88,17 +88,26 @@ namespace armarx
// ObjectPoseObserverInterface interface
public:
+ // READING
+
objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ // MODIFICATION
+
+ objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+ 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;
+
+
//Ice::Int getUpdateCounterByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
//StringIntDictionary getAllUpdateCounters(ICE_CURRENT_ARG) override;
@@ -129,18 +138,31 @@ namespace armarx
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+ 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(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.
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
index 00677428b7b88ead1fbf7dbd1fb380ffad0a94d9..b6e436bb8e4805a0b0c4ac6d40615b404447333c 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>621</width>
- <height>350</height>
+ <width>736</width>
+ <height>391</height>
</rect>
</property>
<property name="windowTitle">
@@ -48,7 +48,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>1</number>
+ <number>0</number>
</property>
<widget class="QWidget" name="tabObjects">
<attribute name="title">
@@ -90,23 +90,23 @@
<string>Timestamp</string>
</property>
</column>
+ <column>
+ <property name="text">
+ <string>Attached</string>
+ </property>
+ </column>
<item>
<property name="text">
- <string>Provider2</string>
+ <string>Provider1</string>
</property>
<item>
<property name="text">
- <string>Dataset1/Object2</string>
+ <string>Dataset1/Object1</string>
</property>
<property name="text">
- <string>Provider2</string>
+ <string>Provider1</string>
</property>
</item>
- </item>
- <item>
- <property name="text">
- <string>Provider1</string>
- </property>
<item>
<property name="text">
<string>Dataset2/Object2/instance1</string>
@@ -115,12 +115,17 @@
<string>Provider1</string>
</property>
</item>
+ </item>
+ <item>
+ <property name="text">
+ <string>Provider2</string>
+ </property>
<item>
<property name="text">
- <string>Dataset1/Object1</string>
+ <string>Dataset1/Object2</string>
</property>
<property name="text">
- <string>Provider1</string>
+ <string>Provider2</string>
</property>
</item>
</item>
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 828d0fb5b2d1ea32a288bbd6c2fd6ed00ba2bc47..4fc35cf54815b585866146d2ce0f8f30ef784285 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -156,10 +156,10 @@ namespace armarx
}
IceUtil::Time start = IceUtil::Time::now();
- ARMARX_INFO << "Getting object poses...";
+ ARMARX_VERBOSE << "Getting object poses...";
const objpose::data::ObjectPoseSeq objectPosesIce = objectPoseObserver->getObjectPoses();
- ARMARX_INFO << "Got " << objectPosesIce.size() << " object poses. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPosesIce);
@@ -200,20 +200,31 @@ namespace armarx
item->setText(col++, QString::fromStdString(pose.providerName));
item->setText(col++, QString::fromStdString(objpose::ObjectTypeEnumNames.to_name(pose.objectType)));
- std::stringstream ss;
- if (pose.localOOBB)
{
- static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
- ss << pose.localOOBB->dimensions().format(iof);
+ std::stringstream ss;
+ if (pose.localOOBB)
+ {
+ static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
+ ss << pose.localOOBB->dimensions().format(iof);
+ }
+ else
+ {
+ ss << "None";
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
}
- else
- {
- ss << "None";
- }
- item->setText(col++, QString::fromStdString(ss.str()));
item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
item->setText(col++, QString::fromStdString(TimeUtil::toStringDateTime(pose.timestamp)));
+ {
+ std::stringstream ss;
+ if (pose.attachment)
+ {
+ ss << pose.attachment->agentName << "/" << pose.attachment->frameName;
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
+ }
+
return true;
});
builder.updateTree(item, objectPoses);
@@ -227,7 +238,7 @@ namespace armarx
});
builder.updateTree(tree, objectPosesByProvider);
- ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
@@ -241,8 +252,8 @@ namespace armarx
IceUtil::Time start = IceUtil::Time::now();
objpose::ProviderInfoMap availableProvidersInfo = objectPoseObserver->getAvailableProvidersInfo();
- ARMARX_INFO << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
// Restructure data.
@@ -300,7 +311,7 @@ namespace armarx
});
builder.updateTree(tree, data);
- ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
void ObjectPoseGuiWidgetController::requestSelectedObjects()
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 0333c21c51055c711e29f06a34570d341987a226..9004c2afcc900c2f0a35d1134f1d100d551ff0ad 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -59,6 +59,44 @@ module armarx
};
};
+ struct AttachObjectToRobotNodeInput
+ {
+ string providerName;
+ armarx::data::ObjectID objectID;
+
+ /// The frame (robot node) to attach to.
+ string frameName;
+ /// The agent's name.
+ string agentName;
+
+ /**
+ * If given, specifies the object's pose in the frame.
+ * If not given, the current object's pose is used.
+ */
+ PoseBase poseInFrame;
+ };
+ struct AttachObjectToRobotNodeOutput
+ {
+ bool success;
+ data::ObjectAttachmentInfo attachment;
+ };
+
+ struct DetachObjectFromRobotNodeInput
+ {
+ string providerName;
+ armarx::data::ObjectID objectID;
+ };
+ struct DetachObjectFromRobotNodeOutput
+ {
+ /// Whether the object was attached before.
+ bool wasAttached;
+ };
+ struct DetachAllObjectsFromRobotNodesOutput
+ {
+ /// Number of objects that have been detached.
+ int numDetached;
+ };
+
interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
{
// Reading
@@ -74,6 +112,13 @@ module armarx
// Modifying
observer::RequestObjectsOutput requestObjects(observer::RequestObjectsInput input);
+ /// Attach an object to a robot node.
+ AttachObjectToRobotNodeOutput attachObjectToRobotNode(AttachObjectToRobotNodeInput input);
+ /// Detach an attached object from a robot node.
+ DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
+ /// Detach all objects from robot nodes.
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+
};
};
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index d6b4d7a6eb93a681068f5d3583c9e80e5d0fe91b..66618485d73ea91819fca88d6951875fba892bcd 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -83,6 +83,8 @@ module armarx
sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
+ class ObjectAttachmentInfo;
+
/// An object pose as stored by the ObjectPoseObserver.
struct ObjectPose
{
@@ -102,6 +104,8 @@ module armarx
StringFloatDictionary robotConfig;
PoseBase robotPose;
+ ObjectAttachmentInfo attachment;
+
/// Confidence in [0, 1] (1 = full, 0 = none).
float confidence = 0;
/// Source timestamp.
@@ -111,6 +115,15 @@ module armarx
Box localOOBB;
};
sequence<ObjectPose> ObjectPoseSeq;
+
+
+ class ObjectAttachmentInfo
+ {
+ string frameName;
+ string agentName;
+
+ PoseBase poseInFrame;
+ };
}
};