diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index a6b0afcdd5b61d39917be0b440c440472f31af79..a3850e5d2844c8bf4c84ff113b183db628ed1197 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -453,8 +453,6 @@ namespace armarx
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;
@@ -499,11 +497,12 @@ namespace armarx
}
}
+ objpose::DetachObjectFromRobotNodeOutput output;
+ output.wasAttached = bool(attachment);
if (attachment)
{
ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
<< attachment->frameName << "' of agent '" << attachment->agentName << "'.";
- output.wasAttached = true;
}
else
{
@@ -542,6 +541,22 @@ namespace armarx
}
+ objpose::AgentFramesSeq ObjectPoseObserver::getAttachableFrames(const Ice::Current&)
+ {
+ std::scoped_lock lock(dataMutex);
+
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ }
+
+
void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index ad7d2e3de5016685a58e2a65f1921b59cf16d2c7..a9db3aeeb213a101179accccd0fa4ffaad4da10e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -88,28 +88,30 @@ namespace armarx
// ObjectPoseObserverInterface interface
public:
- // READING
+ // OBJECT POSES
objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
+ // PROVIDER INFORMATION
+
+ bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+ Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
- bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- // MODIFICATION
+ // REQUESTING
objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+ // ATTACHING
+
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;
+ objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
// Remote GUI
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
index b6e436bb8e4805a0b0c4ac6d40615b404447333c..e201caac70e9cce63d98578373084921c80f14e0 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -54,7 +54,7 @@
<attribute name="title">
<string>Objects</string>
</attribute>
- <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QTreeWidget" name="objectsTree">
<property name="alternatingRowColors">
@@ -131,6 +131,18 @@
</item>
</widget>
</item>
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="font">
+ <font>
+ <pointsize>9</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Right click an object entry for more options.</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<widget class="QWidget" name="tabRequest">
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 4fc35cf54815b585866146d2ce0f8f30ef784285..04fc0dfd0b5fd607406a55567d836659b7141ec1 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -25,6 +25,7 @@
#include <string>
#include <QTimer>
+#include <QMenu>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -37,12 +38,15 @@
namespace armarx
{
+ static const int OBJECTS_COLUMN_ATTACHMENT = 6;
+
ObjectPoseGuiWidgetController::ObjectPoseGuiWidgetController()
{
widget.setupUi(getWidget());
widget.objectsTree->clear();
widget.objectsTree->sortItems(0, Qt::SortOrder::AscendingOrder);
+ widget.objectsTree->setContextMenuPolicy(Qt::CustomContextMenu);
widget.requestTree->clear();
widget.requestTree->sortItems(0, Qt::SortOrder::AscendingOrder);
@@ -53,6 +57,9 @@ namespace armarx
connect(widget.updateButton, &QPushButton::pressed, this, &This::updateTab);
connect(widget.tabWidget, &QTabWidget::currentChanged, this, &This::updateTab);
+ connect(widget.objectsTree, &QTreeWidget::customContextMenuRequested,
+ this, &This::prepareObjectContextMenu);
+
connect(widget.requestButton, &QPushButton::pressed, this, &This::requestSelectedObjects);
QTimer* timer = new QTimer(this);
@@ -126,6 +133,16 @@ namespace armarx
{
getProxy(objectPoseObserver, objectPoseObserverName);
}
+
+ this->attachableFrames = objectPoseObserver->getAttachableFrames();
+ std::sort(attachableFrames.begin(), attachableFrames.end(), [](const auto & lhs, const auto & rhs)
+ {
+ return lhs.agent < rhs.agent;
+ });
+ for (objpose::AgentFrames& frames : attachableFrames)
+ {
+ std::sort(frames.frames.begin(), frames.frames.end());
+ }
}
void ObjectPoseGuiWidgetController::onDisconnectComponent()
@@ -220,7 +237,7 @@ namespace armarx
std::stringstream ss;
if (pose.attachment)
{
- ss << pose.attachment->agentName << "/" << pose.attachment->frameName;
+ ss << pose.attachment->frameName << " (" << pose.attachment->agentName << ")";
}
item->setText(col++, QString::fromStdString(ss.str()));
}
@@ -314,6 +331,82 @@ namespace armarx
ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
+ void ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
+ {
+ QTreeWidget* tree = widget.objectsTree;
+ QTreeWidgetItem* item = tree->itemAt(pos);
+
+ if (item == nullptr || item->parent() == nullptr)
+ {
+ // No item or top level item => no context menu.
+ return;
+ }
+
+ QString providerName = item->parent()->text(0);
+ QString objectID = item->text(0);
+
+ QMenu* attachMenu = new QMenu("Attach to robot node", tree);
+ for (const objpose::AgentFrames& agentFrames : attachableFrames)
+ {
+ QMenu* agentMenu = new QMenu(QString::fromStdString(agentFrames.agent), tree);
+
+ for (const std::string& frame : agentFrames.frames)
+ {
+ QAction* attachAgentAction = new QAction(QString::fromStdString(frame), tree);
+ // attachAgentAction->setStatusTip(tr("Attach object rigidly to a robot node"));
+ connect(attachAgentAction, &QAction::triggered,
+ [ = ]()
+ {
+ this->attachObjectToRobotNode(providerName, objectID, agentFrames.agent, frame);
+ });
+ agentMenu->addAction(attachAgentAction);
+ }
+ attachMenu->addMenu(agentMenu);
+ }
+
+ QAction* detachAction = new QAction(tr("Detach from to robot node"), tree);
+ detachAction->setEnabled(!item->text(OBJECTS_COLUMN_ATTACHMENT).isEmpty());
+ connect(detachAction, &QAction::triggered, [ = ]()
+ {
+ this->detachObjectFromRobotNode(providerName, objectID);
+ });
+
+ QMenu menu(tree);
+ menu.addMenu(attachMenu);
+ menu.addAction(detachAction);
+
+ menu.exec(tree->mapToGlobal(pos));
+ }
+
+ void ObjectPoseGuiWidgetController::attachObjectToRobotNode(
+ QString providerName, QString objectID,
+ const std::string& agentName, const std::string& frameName)
+ {
+ ARMARX_VERBOSE << "Attaching " << objectID << " by '" << providerName << "' to robot node '"
+ << frameName << "' of agent '" << agentName << "'.";
+
+ objpose::AttachObjectToRobotNodeInput input;
+ input.providerName = providerName.toStdString();
+ input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
+ input.agentName = agentName;
+ input.frameName = frameName;
+
+ objpose::AttachObjectToRobotNodeOutput output = objectPoseObserver->attachObjectToRobotNode(input);
+ ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ }
+
+ void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
+ {
+ ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName << "' from robot node.";
+
+ objpose::DetachObjectFromRobotNodeInput input;
+ input.providerName = providerName.toStdString();
+ input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
+
+ objpose::DetachObjectFromRobotNodeOutput output = objectPoseObserver->detachObjectFromRobotNode(input);
+ ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ }
+
void ObjectPoseGuiWidgetController::requestSelectedObjects()
{
std::map<std::string, objpose::observer::RequestObjectsInput> requestsPerProvider;
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
index 26767131953cdd7f756951fdf011597d7716426b..84a0e6e964ffdd2cead4f144627f478f41c94d10 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -97,6 +97,11 @@ namespace armarx
void updateObjectsTab();
void updateRequestTab();
+ void prepareObjectContextMenu(const QPoint& pos);
+ void attachObjectToRobotNode(QString providerName, QString objectID,
+ const std::string& agentName, const std::string& frameName);
+ void detachObjectFromRobotNode(QString providerName, QString objectID);
+
void requestSelectedObjects();
@@ -113,6 +118,7 @@ namespace armarx
std::string objectPoseObserverName;
armarx::objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+ objpose::AgentFramesSeq attachableFrames;
};
}
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 9004c2afcc900c2f0a35d1134f1d100d551ff0ad..602043ab3b17e3afbdfbbd87b835dda80f264f29 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -97,21 +97,33 @@ module armarx
int numDetached;
};
+ struct AgentFrames
+ {
+ string agent;
+ Ice::StringSeq frames;
+ };
+ sequence<AgentFrames> AgentFramesSeq;
+
interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
{
- // Reading
+ // Object poses
+
data::ObjectPoseSeq getObjectPoses();
data::ObjectPoseSeq getObjectPosesByProvider(string providerName);
+ // Provider information
+
bool hasProvider(string providerName);
Ice::StringSeq getAvailableProviderNames();
ProviderInfoMap getAvailableProvidersInfo();
ProviderInfo getProviderInfo(string providerName);
+ // Requesting
- // Modifying
observer::RequestObjectsOutput requestObjects(observer::RequestObjectsInput input);
+ // Attaching
+
/// Attach an object to a robot node.
AttachObjectToRobotNodeOutput attachObjectToRobotNode(AttachObjectToRobotNodeInput input);
/// Detach an attached object from a robot node.
@@ -119,6 +131,8 @@ module armarx
/// Detach all objects from robot nodes.
DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ AgentFramesSeq getAttachableFrames();
+
};
};