diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
index e201caac70e9cce63d98578373084921c80f14e0..03b15fc4513ab5765b393c82007a7a6cc413714f 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -43,6 +43,29 @@
</property>
</spacer>
</item>
+ <item>
+ <widget class="QPushButton" name="detchAllButton">
+ <property name="text">
+ <string>Detach All Objects</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="detchAllCommitAttachedCheckBox">
+ <property name="toolTip">
+ <string><html><head/><body><p>If checked, the object's pose according to the attachment will be committed as a new snapshot when the object is detached.</p><p>If unchecked, no new snapshot will be created, and the object's pose will return to where it was before the attachment. </p><p>In both cases, new snapshots to the object ID will overwrite the previous/committed snapshot.</p></body></html></string>
+ </property>
+ <property name="text">
+ <string>Commit Attached Pose</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="tristate">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item>
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 705f0f7584a84aff9354cf2986505436491f98ff..c337382c781ffe407403468e48b66a9c36c65054 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -24,17 +24,15 @@
#include <string>
-#include <QTimer>
#include <QMenu>
+#include <QTimer>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-
namespace armarx
{
@@ -56,101 +54,126 @@ namespace armarx
connect(widget.updateButton, &QPushButton::pressed, this, &This::updateTab);
connect(widget.tabWidget, &QTabWidget::currentChanged, this, &This::updateTab);
+ connect(widget.detchAllButton,
+ &QPushButton::pressed,
+ [this]() {
+ this->detachAllObjectsFromRobotNodes(
+ widget.detchAllCommitAttachedCheckBox->isChecked());
+ });
- connect(widget.objectsTree, &QTreeWidget::customContextMenuRequested,
- this, &This::prepareObjectContextMenu);
+ connect(widget.objectsTree,
+ &QTreeWidget::customContextMenuRequested,
+ this,
+ &This::prepareObjectContextMenu);
connect(widget.requestButton, &QPushButton::pressed, this, &This::requestSelectedObjects);
QTimer* timer = new QTimer(this);
timer->setInterval(500);
connect(timer, &QTimer::timeout, this, &This::updateTab);
- connect(widget.autoUpdateCheckBox, &QCheckBox::toggled, this, [timer](bool checked)
- {
- if (checked)
- {
- timer->start();
- }
- else
- {
- timer->stop();
- }
- });
+ connect(widget.autoUpdateCheckBox,
+ &QCheckBox::toggled,
+ this,
+ [timer](bool checked)
+ {
+ if (checked)
+ {
+ timer->start();
+ }
+ else
+ {
+ timer->stop();
+ }
+ });
}
-
ObjectPoseGuiWidgetController::~ObjectPoseGuiWidgetController()
{
}
-
- void ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
+ void
+ ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
{
- (void) settings;
+ (void)settings;
}
- void ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
+ void
+ ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
{
- (void) settings;
+ (void)settings;
}
- QString ObjectPoseGuiWidgetController::GetWidgetName()
+ QString
+ ObjectPoseGuiWidgetController::GetWidgetName()
{
- return "MemoryX.ObjectPoseGui";
+ return "ArMem.ObjectPoseGui";
}
- static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseStorage";
+ static const std::string CONFIG_KEY_OBJECT_POSE_STORAGE = "ObjectPoseStorage";
- QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "*"});
+ configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>(
+ {CONFIG_KEY_OBJECT_POSE_STORAGE, "Object pose storage", "Object*"});
}
return qobject_cast<QDialog*>(configDialog);
}
- void ObjectPoseGuiWidgetController::configured()
+ void
+ ObjectPoseGuiWidgetController::configured()
{
if (configDialog)
{
- ObjectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
+ objectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_STORAGE);
+ if (objectPoseStorageName.empty())
+ {
+ objectPoseStorageName = "ObjectMemory";
+ }
}
}
- void ObjectPoseGuiWidgetController::onInitComponent()
+ void
+ ObjectPoseGuiWidgetController::onInitComponent()
{
- if (!ObjectPoseStorageName.empty())
+ if (!objectPoseStorageName.empty())
{
- usingProxy(ObjectPoseStorageName);
+ usingProxy(objectPoseStorageName);
}
}
- void ObjectPoseGuiWidgetController::onConnectComponent()
+ void
+ ObjectPoseGuiWidgetController::onConnectComponent()
{
- if (!ObjectPoseStorageName.empty())
+ if (!objectPoseStorageName.empty())
{
- getProxy(ObjectPoseStorage, ObjectPoseStorageName);
+ getProxy(ObjectPoseStorage, objectPoseStorageName);
}
this->attachableFrames = ObjectPoseStorage->getAttachableFrames();
- std::sort(attachableFrames.begin(), attachableFrames.end(), [](const auto & lhs, const auto & rhs)
- {
- return lhs.agent < rhs.agent;
- });
+ 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());
}
+
+ // Update once.
+ updateTab();
}
- void ObjectPoseGuiWidgetController::onDisconnectComponent()
+ void
+ ObjectPoseGuiWidgetController::onDisconnectComponent()
{
ObjectPoseStorage = nullptr;
}
- void ObjectPoseGuiWidgetController::updateTab()
+ void
+ ObjectPoseGuiWidgetController::updateTab()
{
if (widget.tabWidget->currentWidget() == widget.tabObjects)
{
@@ -162,14 +185,13 @@ namespace armarx
}
}
-
-
- void ObjectPoseGuiWidgetController::updateObjectsTab()
+ void
+ ObjectPoseGuiWidgetController::updateObjectsTab()
{
if (!ObjectPoseStorage)
{
// Probably disconnected.
- ARMARX_VERBOSE << "No object pose observer.";
+ ARMARX_VERBOSE << "No object pose storage.";
return;
}
@@ -177,7 +199,8 @@ namespace armarx
ARMARX_VERBOSE << "Getting object poses...";
const objpose::data::ObjectPoseSeq objectPosesIce = ObjectPoseStorage->getObjectPoses();
ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms.)";
const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPosesIce);
@@ -192,87 +215,97 @@ namespace armarx
QTreeWidget* tree = widget.objectsTree;
MapTreeWidgetBuilder builder(objectPosesByProvider);
- builder.setMakeItemFn([](const std::string & provider, const objpose::ObjectPoseSeq&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
- return item;
- });
- builder.setUpdateItemFn([](const std::string&, const objpose::ObjectPoseSeq & objectPoses, QTreeWidgetItem * item)
- {
- bool expand = item->childCount() == 0;
-
- TreeWidgetBuilder<objpose::ObjectPose> builder;
- builder.setNameFn([](const objpose::ObjectPose & pose)
+ builder.setMakeItemFn(
+ [](const std::string& provider, const objpose::ObjectPoseSeq&)
{
- return pose.objectID.str();
- });
- builder.setMakeItemFn([](const objpose::ObjectPose&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
return item;
});
- builder.setUpdateItemFn([](const objpose::ObjectPose & pose, QTreeWidgetItem * item)
+ builder.setUpdateItemFn(
+ [](const std::string&, const objpose::ObjectPoseSeq& objectPoses, QTreeWidgetItem* item)
{
- int col = 0;
- item->setText(col++, QString::fromStdString(pose.objectID.str()));
- item->setText(col++, QString::fromStdString(pose.providerName));
- item->setText(col++, QString::fromStdString(objpose::ObjectTypeNames.to_name(pose.objectType)));
+ bool expand = item->childCount() == 0;
- {
- std::stringstream ss;
- if (pose.localOOBB)
+ TreeWidgetBuilder<objpose::ObjectPose> builder;
+ builder.setNameFn([](const objpose::ObjectPose& pose)
+ { return pose.objectID.str(); });
+ builder.setMakeItemFn(
+ [](const objpose::ObjectPose&)
{
- static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
- ss << pose.localOOBB->dimensions().format(iof);
- }
- else
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
+ return item;
+ });
+ builder.setUpdateItemFn(
+ [](const objpose::ObjectPose& pose, QTreeWidgetItem* item)
{
- ss << "None";
- }
- item->setText(col++, QString::fromStdString(ss.str()));
- }
- item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
- item->setText(col++, QString::fromStdString(pose.timestamp.toDateTimeString()));
-
+ int col = 0;
+ item->setText(col++, QString::fromStdString(pose.objectID.str()));
+ item->setText(col++, QString::fromStdString(pose.providerName));
+ item->setText(col++,
+ QString::fromStdString(
+ objpose::ObjectTypeNames.to_name(pose.objectType)));
+
+ {
+ 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()));
+ }
+ item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
+ item->setText(col++,
+ QString::fromStdString(pose.timestamp.toDateTimeString()));
+
+ {
+ std::stringstream ss;
+ if (pose.attachment)
+ {
+ ss << pose.attachment->frameName << " ("
+ << pose.attachment->agentName << ")";
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
+ }
+
+ return true;
+ });
+ builder.updateTreeWithContainer(item, objectPoses);
+
+ if (expand)
{
- std::stringstream ss;
- if (pose.attachment)
- {
- ss << pose.attachment->frameName << " (" << pose.attachment->agentName << ")";
- }
- item->setText(col++, QString::fromStdString(ss.str()));
+ item->setExpanded(true);
}
return true;
});
- builder.updateTreeWithContainer(item, objectPoses);
-
- if (expand)
- {
- item->setExpanded(true);
- }
-
- return true;
- });
builder.updateTree(tree, objectPosesByProvider);
- ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took "
+ << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
-
- void ObjectPoseGuiWidgetController::updateRequestTab()
+ void
+ ObjectPoseGuiWidgetController::updateRequestTab()
{
if (!ObjectPoseStorage)
{
// Probably disconnected.
- ARMARX_VERBOSE << "No object pose observer.";
+ ARMARX_VERBOSE << "No object pose storage.";
return;
}
IceUtil::Time start = IceUtil::Time::now();
- objpose::ProviderInfoMap availableProvidersInfo = ObjectPoseStorage->getAvailableProvidersInfo();
- ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ objpose::ProviderInfoMap availableProvidersInfo =
+ ObjectPoseStorage->getAvailableProvidersInfo();
+ ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size()
+ << " object pose providers. "
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms.)";
// Restructure data.
@@ -290,50 +323,62 @@ namespace armarx
QTreeWidget* tree = widget.requestTree;
MapTreeWidgetBuilder builder(data);
- builder.setMakeItemFn([](const std::string & dataset, const auto&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
- return item;
- });
- builder.setUpdateItemFn([tree](const std::string & dataset, const auto & datasetData, QTreeWidgetItem * datasetItem)
- {
- (void) dataset;
-
- TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
- builder.setCompareFn([](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem * item)
- {
- auto rhs = std::make_pair(item->text(0).toStdString(), item->text(1).toStdString());
- if (lhs < rhs)
- {
- return -1;
- }
- return lhs == rhs ? 0 : 1;
- });
- builder.setMakeItemFn([](const std::pair<std::string, std::string>& element)
+ builder.setMakeItemFn(
+ [](const std::string& dataset, const auto&)
{
- QTreeWidgetItem* item = new QTreeWidgetItem({ QString::fromStdString(element.first), QString::fromStdString(element.second)});
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
return item;
});
- builder.setUpdateItemFn([tree](const std::pair<std::string, std::string>& element, QTreeWidgetItem * item)
+ builder.setUpdateItemFn(
+ [tree](
+ const std::string& dataset, const auto& datasetData, QTreeWidgetItem* datasetItem)
{
- (void) element;
- if (!tree->itemWidget(item, 2))
- {
- QCheckBox* requestCheckBox = new QCheckBox();
- tree->setItemWidget(item, 2, requestCheckBox);
- }
+ (void)dataset;
+
+ TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
+ builder.setCompareFn(
+ [](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem* item)
+ {
+ auto rhs = std::make_pair(item->text(0).toStdString(),
+ item->text(1).toStdString());
+ if (lhs < rhs)
+ {
+ return -1;
+ }
+ return lhs == rhs ? 0 : 1;
+ });
+ builder.setMakeItemFn(
+ [](const std::pair<std::string, std::string>& element)
+ {
+ QTreeWidgetItem* item =
+ new QTreeWidgetItem({QString::fromStdString(element.first),
+ QString::fromStdString(element.second)});
+ return item;
+ });
+ builder.setUpdateItemFn(
+ [tree](const std::pair<std::string, std::string>& element,
+ QTreeWidgetItem* item)
+ {
+ (void)element;
+ if (!tree->itemWidget(item, 2))
+ {
+ QCheckBox* requestCheckBox = new QCheckBox();
+ tree->setItemWidget(item, 2, requestCheckBox);
+ }
+ return true;
+ });
+ builder.updateTreeWithContainer(datasetItem, datasetData);
+
return true;
});
- builder.updateTreeWithContainer(datasetItem, datasetData);
-
- return true;
- });
builder.updateTree(tree, data);
- ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took "
+ << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
- void ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
+ void
+ ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
{
QTreeWidget* tree = widget.objectsTree;
QTreeWidgetItem* item = tree->itemAt(pos);
@@ -356,11 +401,12 @@ namespace armarx
{
QAction* attachAgentAction = new QAction(QString::fromStdString(frame), tree);
// attachAgentAction->setStatusTip(tr("Attach object rigidly to a robot node"));
- connect(attachAgentAction, &QAction::triggered,
- [ =, this ]()
- {
- this->attachObjectToRobotNode(providerName, objectID, agentFrames.agent, frame);
- });
+ connect(attachAgentAction,
+ &QAction::triggered,
+ [=, this]() {
+ this->attachObjectToRobotNode(
+ providerName, objectID, agentFrames.agent, frame);
+ });
agentMenu->addAction(attachAgentAction);
}
attachMenu->addMenu(agentMenu);
@@ -368,10 +414,9 @@ namespace armarx
QAction* detachAction = new QAction(tr("Detach from to robot node"), tree);
detachAction->setEnabled(!item->text(OBJECTS_COLUMN_ATTACHMENT).isEmpty());
- connect(detachAction, &QAction::triggered, [ =, this ]()
- {
- this->detachObjectFromRobotNode(providerName, objectID);
- });
+ connect(detachAction,
+ &QAction::triggered,
+ [=, this]() { this->detachObjectFromRobotNode(providerName, objectID); });
QMenu menu(tree);
menu.addMenu(attachMenu);
@@ -380,9 +425,11 @@ namespace armarx
menu.exec(tree->mapToGlobal(pos));
}
- void ObjectPoseGuiWidgetController::attachObjectToRobotNode(
- QString providerName, QString objectID,
- const std::string& agentName, const std::string& frameName)
+ 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 << "'.";
@@ -395,7 +442,8 @@ namespace armarx
try
{
- objpose::AttachObjectToRobotNodeOutput output = ObjectPoseStorage->attachObjectToRobotNode(input);
+ objpose::AttachObjectToRobotNodeOutput output =
+ ObjectPoseStorage->attachObjectToRobotNode(input);
ARMARX_VERBOSE << "Success of attaching: " << output.success;
}
catch (const IceUtil::Exception& e)
@@ -406,9 +454,11 @@ namespace armarx
}
}
- void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
+ void
+ ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
{
- ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName << "' from robot node.";
+ ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName
+ << "' from robot node.";
objpose::DetachObjectFromRobotNodeInput input;
input.providerName = providerName.toStdString();
@@ -416,17 +466,39 @@ namespace armarx
try
{
- objpose::DetachObjectFromRobotNodeOutput output = ObjectPoseStorage->detachObjectFromRobotNode(input);
+ objpose::DetachObjectFromRobotNodeOutput output =
+ ObjectPoseStorage->detachObjectFromRobotNode(input);
ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
}
catch (const IceUtil::Exception& e)
{
- ARMARX_WARNING << "Failed to detach object '" << input.objectID << "' from a robot node."
+ ARMARX_WARNING << "Failed to detach object '" << input.objectID
+ << "' from a robot node."
<< "\nReason: " << e.what();
}
}
- void ObjectPoseGuiWidgetController::requestSelectedObjects()
+ void
+ ObjectPoseGuiWidgetController::detachAllObjectsFromRobotNodes(bool commitAttachedPose)
+ {
+
+ objpose::DetachAllObjectsFromRobotNodesInput input;
+ input.commitAttachedPose = commitAttachedPose;
+
+ try
+ {
+ objpose::DetachAllObjectsFromRobotNodesOutput output =
+ ObjectPoseStorage->detachAllObjectsFromRobotNodes(input);
+ ARMARX_VERBOSE << "Detached " << output.numDetached << " objects from robot nodes.";
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to detach all objects from robot nodes.";
+ }
+ }
+
+ void
+ ObjectPoseGuiWidgetController::requestSelectedObjects()
{
std::map<std::string, objpose::observer::RequestObjectsInput> requestsPerProvider;
@@ -442,7 +514,8 @@ namespace armarx
if (selected->isChecked())
{
std::string providerName = classItem->text(1).toStdString();
- objpose::observer::RequestObjectsInput& requests = requestsPerProvider[providerName];
+ objpose::observer::RequestObjectsInput& requests =
+ requestsPerProvider[providerName];
data::ObjectID& id = requests.request.objectIDs.emplace_back();
id.dataset = datasetItem->text(0).toStdString();
id.className = classItem->text(0).toStdString();
@@ -463,14 +536,16 @@ namespace armarx
ARMARX_INFO << "Requesting " << request.request.objectIDs.size() << " objects for "
<< request.request.relativeTimeoutMS << " ms.";
- objpose::observer::RequestObjectsOutput output = ObjectPoseStorage->requestObjects(request);
+ objpose::observer::RequestObjectsOutput output =
+ ObjectPoseStorage->requestObjects(request);
int successful = 0;
for (const auto& [id, result] : output.results)
{
successful += int(!result.providerName.empty() && result.result.success);
}
- ARMARX_INFO << successful << " of " << request.request.objectIDs.size() << " object request successful.";
+ ARMARX_INFO << successful << " of " << request.request.objectIDs.size()
+ << " object request successful.";
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
index 5e9910bd8185cb13adbd54435758ee871dbd8460..462b1b0485eefe270e7bdcad5b2060bd2f3077dd 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -21,17 +21,15 @@
*/
#pragma once
-#include <RobotAPI/gui-plugins/ObjectPoseGui/ui_ObjectPoseGuiWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
+#include <RobotAPI/gui-plugins/ObjectPoseGui/ui_ObjectPoseGuiWidget.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
namespace armarx
{
/**
@@ -52,9 +50,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ObjectPoseGuiWidgetController :
- public armarx::ArmarXComponentWidgetControllerTemplate < ObjectPoseGuiWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT ObjectPoseGuiWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<ObjectPoseGuiWidgetController>
{
Q_OBJECT
@@ -98,9 +95,12 @@ namespace armarx
void updateRequestTab();
void prepareObjectContextMenu(const QPoint& pos);
- void attachObjectToRobotNode(QString providerName, QString objectID,
- const std::string& agentName, const std::string& frameName);
+ void attachObjectToRobotNode(QString providerName,
+ QString objectID,
+ const std::string& agentName,
+ const std::string& frameName);
void detachObjectFromRobotNode(QString providerName, QString objectID);
+ void detachAllObjectsFromRobotNodes(bool commitAttachedPose);
void requestSelectedObjects();
@@ -109,18 +109,14 @@ namespace armarx
/* QT signal declarations */
private:
-
/// Widget Form
Ui::ObjectPoseGuiWidget widget;
QPointer<SimpleConfigDialog> configDialog;
- std::string ObjectPoseStorageName;
+ std::string objectPoseStorageName;
armarx::objpose::ObjectPoseStorageInterfacePrx ObjectPoseStorage;
objpose::AgentFramesSeq attachableFrames;
-
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 03da4c2061d786b22fd1f126d9e8c2983e28faec..a5822667f61138a40a9f581de8dc68fb7fd812d6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,59 +1,53 @@
#include "Segment.h"
-#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
-#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <sstream>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem/util/util.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include "ArmarXCore/core/time/Clock.h"
-#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/regression/linear.h>
-
-#include <Eigen/Geometry>
-#include <Eigen/Dense>
-
-#include <sstream>
-
-
namespace armarx::armem::server::obj::instance
{
- void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(robotName, prefix + "robotName",
+ defs->optional(robotName,
+ prefix + "robotName",
"Name of robot whose note can be calibrated.\n"
"If not given, the 'fallbackName' is used.");
defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
@@ -66,111 +60,126 @@ namespace armarx::armem::server::obj::instance
arondto::ObjectInstance::ToAronType(),
64)
{
- oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ oobbCache.setFetchFn(
+ [this](const ObjectID& id) -> std::optional<simox::OrientedBoxf>
{
- try
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
{
- objectInfo->setLogError(false); // Don't log missing files
- return objectInfo->loadOOBB();
+ try
+ {
+ objectInfo->setLogError(false); // Don't log missing files
+ 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;
+ }
}
- catch (const std::ios_base::failure& e)
+ else
{
- // 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() : "";
- });
+ classNameToDatasetCache.setFetchFn(
+ [this](const std::string& className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
}
-
Segment::~Segment()
{
}
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
SpecializedCoreSegment::defineProperties(defs, prefix);
- defs->optional(p.discardSnapshotsWhileAttached, prefix + "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.");
+ defs->optional(
+ p.discardSnapshotsWhileAttached,
+ prefix + "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.");
- defs->optional(robots.fallbackName, prefix + "robots.FallbackName",
+ defs->optional(robots.fallbackName,
+ prefix + "robots.FallbackName",
"Robot name to use as fallback if the robot name is not specified "
"in a provided object pose.");
- defs->optional(p.sceneSnapshotsPackage, prefix + "scene.10_Package",
- "ArmarX package containing the scene snapshots.\n"
- "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
- defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
+ defs->optional(
+ p.sceneSnapshotsPackage,
+ prefix + "scene.10_Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotsDirectory,
+ prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
- std::vector<std::string> sceneSnapshotToLoadDescription =
- {
+ std::vector<std::string> sceneSnapshotToLoadDescription = {
"Scene(s) to load on startup.",
"Specify multiple scenes in a ; separated list.",
"Each entry must be one of the following:",
"(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
"e.g. 'MyScene', 'MyScene.json'",
- "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), "
+ "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' "
+ "extension), "
"e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
"(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
};
- defs->optional(p.sceneSnapshotsToLoad, prefix + "scene.12_SnapshotToLoad",
+ defs->optional(p.sceneSnapshotsToLoad,
+ prefix + "scene.12_SnapshotToLoad",
simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
decay.defineProperties(defs, prefix + "decay.");
}
+ std::vector<std::string>
+ Segment::Properties::getSceneSnapshotsToLoad() const
+ {
+ if (sceneSnapshotsToLoad.empty())
+ {
+ return {};
+ }
+ bool trim = true;
+ return simox::alg::split(sceneSnapshotsToLoad, ";", trim);
+ }
- void Segment::init()
+ void
+ Segment::init()
{
SpecializedCoreSegment::init();
- if (not p.sceneSnapshotsToLoad.empty())
+ const std::vector<std::string> scenes = p.getSceneSnapshotsToLoad();
+ for (const std::string& scene : scenes)
{
- bool trim = true;
- const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
- for (const std::string& scene : scenes)
- {
- const bool lockMemory = false;
- commitSceneSnapshotFromFilename(scene, lockMemory);
- }
+ const bool lockMemory = false;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
}
robots.setTag(Logging::tag);
}
-
- void Segment::connect(viz::Client arviz)
+ void
+ Segment::connect(viz::Client arviz)
{
- (void) arviz;
+ (void)arviz;
// ARMARX_INFO << "ArticulatedObjectVisu";
// this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
// visu->init();
}
-
- Segment::CommitStats Segment::commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<armem::Time> discardUpdatesUntil)
+ Segment::CommitStats
+ Segment::commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<armem::Time> discardUpdatesUntil)
{
CommitStats stats;
@@ -187,7 +196,8 @@ namespace armarx::armem::server::obj::instance
// Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
- const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ const wm::Entity* entity =
+ findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
const arondto::ObjectInstance data = getLatestInstanceData(*entity);
@@ -221,11 +231,13 @@ namespace armarx::armem::server::obj::instance
// Update the entity.
stats.numUpdated++;
- VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(provided.robotName, provided.providerName);
- if(not robot)
+ if (not robot)
{
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" << provided.robotName << "`.";
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
}
// robot may be null!
@@ -233,10 +245,10 @@ namespace armarx::armem::server::obj::instance
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp
- << ". This is " << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
-
+ ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
+ << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
+ << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+
robotSyncTimestamp = timestamp;
@@ -244,7 +256,8 @@ namespace armarx::armem::server::obj::instance
{
if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
float value = robotNode->getJointValue();
float newValue = value + calibration.offset;
@@ -255,7 +268,8 @@ namespace armarx::armem::server::obj::instance
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ VirtualRobot::RobotNode::JointLimits limits =
+ robotNode->getJointLimits();
bool limitsChanged = false;
if (newValue < limits.low)
{
@@ -282,11 +296,11 @@ namespace armarx::armem::server::obj::instance
{
objpose::data::ProvidedObjectPose copy = provided;
copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
}
else
{
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
}
if (previousPose && previousPose->attachment)
@@ -299,10 +313,12 @@ namespace armarx::armem::server::obj::instance
if (newPose.objectID.dataset().empty())
{
// Try to find the data set.
- const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
if (!dataset.empty())
{
- newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
if (!provided.localOOBB)
@@ -318,8 +334,8 @@ namespace armarx::armem::server::obj::instance
return stats;
}
-
- void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
+ void
+ Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
Time now = Time::Now();
@@ -332,7 +348,7 @@ namespace armarx::armem::server::obj::instance
EntityUpdate& update = commit.updates.emplace_back();
const MemoryID providerID = coreSegmentID.withProviderSegmentName(
- providerName.empty() ? pose.providerName : providerName);
+ providerName.empty() ? pose.providerName : providerName);
update.entityID = providerID.withEntityName(pose.objectID.str());
update.arrivedTime = now;
@@ -352,13 +368,11 @@ namespace armarx::armem::server::obj::instance
}
toAron(dto.sourceID, MemoryID());
update.instancesData.push_back(dto.toAron());
-
}
// Commit non-locking.
iceMemory.commit(commit);
}
-
objpose::ObjectPoseMap
Segment::getObjectPoses(const DateTime& now)
{
@@ -367,20 +381,16 @@ namespace armarx::armem::server::obj::instance
return filterObjectPoses(objectPoses);
}
-
-
objpose::ObjectPoseMap
- Segment::getObjectPosesByProvider(
- const std::string& providerName,
- const DateTime& now)
+ Segment::getObjectPosesByProvider(const std::string& providerName, const DateTime& now)
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
- ObjectPoseMap objectPoses = getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
+ ObjectPoseMap objectPoses =
+ getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
-
wm::Entity*
Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
@@ -389,15 +399,16 @@ namespace armarx::armem::server::obj::instance
if (providerName.empty())
{
wm::Entity* result = nullptr;
- segmentPtr->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov)
- {
- if (prov.hasEntity(entityID.entityName))
+ segmentPtr->forEachProviderSegment(
+ [&result, &entityID](wm::ProviderSegment& prov)
{
- result = &prov.getEntity(entityID);
- return false;
- }
- return true;
- });
+ if (prov.hasEntity(entityID.entityName))
+ {
+ result = &prov.getEntity(entityID);
+ return false;
+ }
+ return true;
+ });
return result;
}
else
@@ -415,24 +426,24 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
{
bool agentSynchronized = false;
for (auto& [id, objectPose] : objectPoses)
{
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
updateObjectPose(objectPose, now, robot, agentSynchronized);
}
}
-
- void Segment::updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
for (auto& [id, objectPose] : objectPoses)
{
@@ -440,12 +451,11 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
updateAttachement(objectPose, agent, agentSynchronized);
@@ -455,8 +465,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- Segment::ObjectPoseMap Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
+ Segment::ObjectPoseMap
+ Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
{
ObjectPoseMap result;
for (const auto& [id, objectPose] : objectPoses)
@@ -469,9 +479,10 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ void
+ Segment::updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
+ bool& synchronized) const
{
if (not objectPose.attachment.has_value())
{
@@ -487,7 +498,7 @@ namespace armarx::armem::server::obj::instance
}
ARMARX_CHECK_NOT_NULL(agent);
- if (not synchronized) // Synchronize only once.
+ if (not synchronized) // Synchronize only once.
{
ARMARX_CHECK_NOT_NULL(robots.reader);
@@ -499,7 +510,6 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses() const
{
@@ -507,7 +517,6 @@ namespace armarx::armem::server::obj::instance
return getLatestObjectPoses(*segmentPtr);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
{
@@ -516,7 +525,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
{
@@ -525,7 +533,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPose
Segment::getLatestObjectPose(const wm::Entity& entity)
{
@@ -534,51 +541,51 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
- coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
- {
- getLatestObjectPoses(provSegment, out);
- });
+ coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment& provSegment)
+ { getLatestObjectPoses(provSegment, out); });
}
-
- void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
- provSegment.forEachEntity([&out](const wm::Entity & entity)
- {
- if (!entity.empty())
+ provSegment.forEachEntity(
+ [&out](const wm::Entity& entity)
{
- ObjectPose pose = getLatestObjectPose(entity);
- // Try to insert. Fails and returns false if an entry already exists.
- const auto [it, success] = out.insert({pose.objectID, pose});
- if (!success)
+ if (!entity.empty())
{
- // An entry with that ID already exists. We keep the newest.
- if (it->second.timestamp < pose.timestamp)
+ ObjectPose pose = getLatestObjectPose(entity);
+ // Try to insert. Fails and returns false if an entry already exists.
+ const auto [it, success] = out.insert({pose.objectID, pose});
+ if (!success)
{
- it->second = pose;
+ // An entry with that ID already exists. We keep the newest.
+ if (it->second.timestamp < pose.timestamp)
+ {
+ it->second = pose;
+ }
}
}
- }
- });
+ });
}
-
- void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
+ void
+ Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
{
- entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
- {
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
+ entity.getLatestSnapshot().forEachInstance(
+ [&out](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
- fromAron(dto, out);
- });
+ fromAron(dto, out);
+ });
}
-
- arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
+ arondto::ObjectInstance
+ Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
@@ -592,7 +599,6 @@ namespace armarx::armem::server::obj::instance
return data;
}
-
::armarx::armem::articulated_object::ArticulatedObjects
Segment::getArticulatedObjects()
{
@@ -601,7 +607,7 @@ namespace armarx::armem::server::obj::instance
ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
::armarx::armem::articulated_object::ArticulatedObjects objects;
- for (const auto&[objectId, objectPose] : objectPoses)
+ for (const auto& [objectId, objectPose] : objectPoses)
{
armem::articulated_object::ArticulatedObject articulatedObject;
articulatedObject.config.jointMap = objectPose.objectJointValues;
@@ -625,7 +631,6 @@ namespace armarx::armem::server::obj::instance
fromAron(dto, description);
articulatedObject.description = description;
-
}
catch (...)
{
@@ -648,7 +653,6 @@ namespace armarx::armem::server::obj::instance
return objects;
}
-
std::map<DateTime, objpose::ObjectPose>
Segment::getObjectPosesInRange(const wm::Entity& entity,
const DateTime& start,
@@ -677,14 +681,14 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ std::optional<simox::OrientedBoxf>
+ Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
-
- objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ objpose::ProviderInfo
+ Segment::getProviderInfo(const std::string& providerName)
{
try
{
@@ -703,14 +707,13 @@ namespace armarx::armem::server::obj::instance
}
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
objpose::AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
+ output.success = false; // We are not successful until proven otherwise.
ObjectID objectID = armarx::fromIce(input.objectID);
@@ -718,7 +721,8 @@ namespace armarx::armem::server::obj::instance
if (not agent)
{
std::stringstream ss;
- ss << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ ss << "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: ";
for (const auto& [name, robot] : robots.loaded)
@@ -732,8 +736,8 @@ namespace armarx::armem::server::obj::instance
if (!agent->hasRobotNode(input.frameName))
{
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '"
+ << input.frameName << "' of agent '" << agent->getName() << "'.";
return output;
}
std::string frameName = input.frameName;
@@ -744,7 +748,8 @@ namespace armarx::armem::server::obj::instance
if (!objectEntity || objectEntity->empty())
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ << "' of agent '" << agent->getName()
+ << "', but object is currently not provided.";
return output;
}
arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
@@ -764,7 +769,8 @@ namespace armarx::armem::server::obj::instance
const auto timestamp = armarx::Clock::Now();
ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
- armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ armarx::FramedPose framed(
+ data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
if (frameName == armarx::GlobalFrame)
{
info.poseInFrame = framed.toGlobalEigen(agent);
@@ -779,21 +785,23 @@ namespace armarx::armem::server::obj::instance
// Store attachment in new entity snapshot.
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = objectEntity->id();
update.referencedTime = now;
{
arondto::ObjectInstance updated = data;
toAron(updated.pose.attachment, info);
updated.pose.attachmentValid = true;
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName
+ << "' "
<< "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
+ << "Object pose in frame: \n"
+ << info.poseInFrame;
output.success = true;
output.attachment = new objpose::data::ObjectAttachmentInfo();
@@ -804,10 +812,8 @@ namespace armarx::armem::server::obj::instance
return output;
}
-
objpose::DetachObjectFromRobotNodeOutput
- Segment::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input)
+ Segment::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
@@ -840,19 +846,20 @@ namespace armarx::armem::server::obj::instance
output.wasAttached = bool(attachment);
if (attachment)
{
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName
+ << "' from robot node '" << attachment->frameName << "' of agent '"
+ << attachment->agentName << "'.";
}
else
{
- ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName
+ << "' "
<< "from robot node, but it was not attached.";
}
return output;
}
-
objpose::DetachAllObjectsFromRobotNodesOutput
Segment::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input)
@@ -863,31 +870,31 @@ namespace armarx::armem::server::obj::instance
objpose::DetachAllObjectsFromRobotNodesOutput output;
output.numDetached = 0;
- segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity)
- {
- const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
- if (data.pose.attachmentValid)
+ segmentPtr->forEachEntity(
+ [this, now, &input, &output](wm::Entity& entity)
{
- ++output.numDetached;
- // Store non-attached pose in new snapshot.
- this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
- }
- });
+ const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ ++output.numDetached;
+ // Store non-attached pose in new snapshot.
+ this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
+ }
+ });
ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
return output;
}
-
- void Segment::storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose)
+ void
+ Segment::storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose)
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = entity.id();
update.referencedTime = now;
{
@@ -897,7 +904,8 @@ namespace armarx::armem::server::obj::instance
ObjectPose objectPose;
fromAron(data, objectPose);
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
bool agentSynchronized = false;
updateAttachement(objectPose, robot, agentSynchronized);
@@ -911,34 +919,34 @@ namespace armarx::armem::server::obj::instance
toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
}
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
-
std::optional<wm::EntityInstance>
Segment::findClassInstance(const ObjectID& objectID) const
{
- const ObjectID classID = { objectID.dataset(), objectID.className() };
+ const ObjectID classID = {objectID.dataset(), objectID.className()};
try
{
std::optional<wm::EntityInstance> result;
iceMemory.workingMemory->getCoreSegment("Class").forEachProviderSegment(
- [&classID, &result](const wm::ProviderSegment & provSeg)
- {
- if (provSeg.hasEntity(classID.str()))
+ [&classID, &result](const wm::ProviderSegment& provSeg)
{
- result = provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
- return false;
- }
- return true;
- });
+ if (provSeg.hasEntity(classID.str()))
+ {
+ result =
+ provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ return false;
+ }
+ return true;
+ });
if (not result.has_value())
{
- ARMARX_WARNING << deactivateSpam(120)
- << "No provider segment for classID " << classID.str() << " found";
+ ARMARX_WARNING << deactivateSpam(120) << "No provider segment for classID "
+ << classID.str() << " found";
}
return result;
}
@@ -950,12 +958,11 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
+ void
+ Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
-
ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
try
{
@@ -968,7 +975,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::string& filename)
{
@@ -982,7 +988,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::filesystem::path& path)
{
@@ -997,57 +1002,56 @@ namespace armarx::armem::server::obj::instance
}
}
-
const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
-
std::optional<std::filesystem::path>
Segment::resolveSceneFilepath(const std::string& _filename)
{
- std::string filepath = _filename;
+ std::stringstream log;
+ std::filesystem::path filepath = _filename;
- filepath = simox::alg::replace_all(filepath, timestampPlaceholder,
- Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
+ filepath = simox::alg::replace_all(
+ filepath, timestampPlaceholder, Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
if (not simox::alg::ends_with(filepath, ".json"))
{
filepath += ".json";
}
- // Try to interprete it as relative to 'Package/scenes/'.
- if (!finder)
+ if (filepath.is_absolute())
{
- finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
- if (not finder->packageFound())
- {
- ARMARX_WARNING << "Could not find CMake package " << p.sceneSnapshotsPackage << ".";
- }
+ return filepath;
}
- if (finder->packageFound())
+
+ armarx::PackagePath packagePath = [&filepath, this]()
{
- std::filesystem::path absDataDir = finder->getDataDir();
- std::filesystem::path absPath = absDataDir / p.sceneSnapshotsPackage
- / p.sceneSnapshotsDirectory / filepath;
- if (std::filesystem::is_regular_file(absPath))
+ if (filepath.has_parent_path())
{
- return absPath;
+ // Interpret the first component as package name.
+ std::string packageName = *filepath.begin();
+ return PackagePath(packageName, std::filesystem::relative(filepath, packageName));
}
- }
-
- // Try to interprete it as ArmarXDataPath.
- {
- std::string resolved = ArmarXDataPath::resolvePath(filepath);
- if (resolved != filepath)
+ else
{
- return resolved;
+ // Only a filename => Interprete it as relative to 'Package/scenes/'.
+ return PackagePath(p.sceneSnapshotsPackage, p.sceneSnapshotsDirectory / filepath);
}
- }
+ }();
- // Else: Fail
- return std::nullopt;
+ try
+ {
+ std::filesystem::path systemPath = packagePath.toSystemPath();
+ return systemPath;
+ }
+ catch (const armarx::PackageNotFoundException& e)
+ {
+ ARMARX_INFO << "Could not interpret '" << packagePath.serialize().package
+ << "' as ArmarX package name (" << e.what() << ").";
+ return std::nullopt;
+ }
}
-
- armarx::objects::Scene Segment::getSceneSnapshot() const
+ armarx::objects::Scene
+ Segment::getSceneSnapshot() const
{
using armarx::objects::SceneObject;
@@ -1060,36 +1064,42 @@ namespace armarx::armem::server::obj::instance
};
std::map<ObjectID, StampedSceneObject> objects;
- segmentPtr->forEachEntity([&objects](wm::Entity & entity)
- {
- const wm::EntityInstance* entityInstance = entity.findLatestInstance();
- if (entityInstance)
+ segmentPtr->forEachEntity(
+ [&objects](wm::Entity& entity)
{
- std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(*entityInstance);
- if (objectInstance)
+ const wm::EntityInstance* entityInstance = entity.findLatestInstance();
+ if (entityInstance)
{
- const ObjectID objectID = ObjectID::FromString(objectInstance->classID.entityName);
-
- auto it = objects.find(objectID);
- if (it == objects.end() or objectInstance->pose.timestamp > it->second.timestamp)
+ std::optional<arondto::ObjectInstance> objectInstance =
+ tryCast<arondto::ObjectInstance>(*entityInstance);
+ if (objectInstance)
{
- StampedSceneObject& stamped = objects[objectID];
- stamped.timestamp = objectInstance->pose.timestamp;
+ const ObjectID objectID =
+ ObjectID::FromString(objectInstance->classID.entityName);
- SceneObject& object = stamped.object;
- object.className = objectID.getClassID().str();
- object.instanceName = objectID.instanceName();
- object.collection = "";
+ auto it = objects.find(objectID);
+ if (it == objects.end() or
+ objectInstance->pose.timestamp > it->second.timestamp)
+ {
+ StampedSceneObject& stamped = objects[objectID];
+ stamped.timestamp = objectInstance->pose.timestamp;
- object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
- object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ SceneObject& object = stamped.object;
+ object.className = objectID.getClassID().str();
+ object.instanceName = objectID.instanceName();
+ object.collection = "";
- object.isStatic = objectInstance->pose.isStatic;
- object.jointValues = objectInstance->pose.objectJointValues;
+ object.position =
+ simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation =
+ simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+
+ object.isStatic = objectInstance->pose.isStatic;
+ object.jointValues = objectInstance->pose.objectJointValues;
+ }
}
}
- }
- });
+ });
armarx::objects::Scene scene;
for (const auto& [id, stamped] : objects)
@@ -1099,8 +1109,8 @@ namespace armarx::armem::server::obj::instance
return scene;
}
-
- void Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
+ void
+ Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
{
const Time now = Time::Now();
std::map<ObjectID, int> idCounters;
@@ -1117,11 +1127,9 @@ namespace armarx::armem::server::obj::instance
pose.objectType = objpose::ObjectType::KnownObject;
// If not specified, assume loaded objects are static.
pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
- pose.objectID = classID.withInstanceName(
- object.instanceName.empty()
- ? std::to_string(idCounters[classID]++)
- : object.instanceName
- );
+ pose.objectID = classID.withInstanceName(object.instanceName.empty()
+ ? std::to_string(idCounters[classID]++)
+ : object.instanceName);
pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
pose.objectPoseRobot = pose.objectPoseGlobal;
@@ -1142,8 +1150,8 @@ namespace armarx::armem::server::obj::instance
commitObjectPoses(objectPoses);
}
-
- void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ void
+ Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
{
std::stringstream ss;
ss << "Loading scene '" << filename << "' ...";
@@ -1156,7 +1164,7 @@ namespace armarx::armem::server::obj::instance
auto makeSceneName = [](const std::filesystem::path& path)
{
std::filesystem::path filename = path.filename();
- filename.replace_extension(); // Removes extension
+ filename.replace_extension(); // Removes extension
return filename.string();
};
std::string sceneName = makeSceneName(path.value());
@@ -1164,10 +1172,8 @@ namespace armarx::armem::server::obj::instance
// The check seems useless?
if (lockMemory)
{
- segmentPtr->doLocked([this,&snapshot, &sceneName]()
- {
- commitSceneSnapshot(snapshot.value(), sceneName);
- });
+ segmentPtr->doLocked([this, &snapshot, &sceneName]()
+ { commitSceneSnapshot(snapshot.value(), sceneName); });
}
else
{
@@ -1181,8 +1187,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::RemoteGui::setup(const Segment& data)
+ void
+ Segment::RemoteGui::setup(const Segment& data)
{
using namespace armarx::RemoteGui::Client;
@@ -1191,18 +1197,33 @@ namespace armarx::armem::server::obj::instance
infiniteHistory.setValue(data.properties.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
- storeLoadLine.setValue("Scene_" + timestampPlaceholder);
+ const std::string storeLoadPath = [&data]()
+ {
+ const std::vector<std::string> scenes = data.p.getSceneSnapshotsToLoad();
+ if (scenes.empty())
+ {
+ std::string storeLoadFilename = "Scene_" + timestampPlaceholder;
+ return data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsToLoad;
+ }
+ else
+ {
+ return scenes.front();
+ }
+ }();
+
+ storeLoadLine.setValue(storeLoadPath);
loadButton.setLabel("Load Scene");
storeButton.setLabel("Store Scene");
- HBoxLayout storeLoadLineLayout(
- {
- Label(data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsDirectory + "/"),
- storeLoadLine,
- Label(".json")
- });
+ HBoxLayout storeLoadLineLayout({storeLoadLine, Label(".json")});
HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
+ detachAllObjectsButton.setLabel("Detach All Objects");
+ detachAllObjectsCommitAttachedPoseCheckBox.setValue(true);
+ HBoxLayout detachAllObjectsCommitAttachedPoseLayout(
+ {Label("Commit Attached Pose"), detachAllObjectsCommitAttachedPoseCheckBox});
+
+
GridLayout grid;
int row = 0;
@@ -1215,15 +1236,19 @@ namespace armarx::armem::server::obj::instance
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});
+ grid.add(Label("Discard Snapshots while Attached"), {row, 0})
+ .add(discardSnapshotsWhileAttached, {row, 1});
row++;
+ grid.add(detachAllObjectsButton, {row, 0})
+ .add(detachAllObjectsCommitAttachedPoseLayout, {row, 1});
+
group.setLabel("Data");
group.addChild(grid);
}
-
- void Segment::RemoteGui::update(Segment& segment)
+ void
+ Segment::RemoteGui::update(Segment& segment)
{
if (loadButton.wasClicked())
{
@@ -1234,41 +1259,51 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
armarx::objects::Scene scene;
- segment.doLocked([&scene, &segment]()
- {
- scene = segment.getSceneSnapshot();
- });
+ segment.doLocked([&scene, &segment]() { scene = segment.getSceneSnapshot(); });
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged()
- || maxHistorySize.hasValueChanged()
- || discardSnapshotsWhileAttached.hasValueChanged())
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged() ||
+ discardSnapshotsWhileAttached.hasValueChanged())
{
- segment.doLocked([this, &segment]()
- {
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ segment.doLocked(
+ [this, &segment]()
{
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.segmentPtr)
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ segment.properties.maxHistorySize =
+ infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.segmentPtr)
+ {
+ segment.segmentPtr->setMaxHistorySize(
+ long(segment.properties.maxHistorySize));
+ }
}
- }
- segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- });
+ segment.p.discardSnapshotsWhileAttached =
+ discardSnapshotsWhileAttached.getValue();
+ });
}
- }
+ if (detachAllObjectsButton.wasClicked())
+ {
+ objpose::DetachAllObjectsFromRobotNodesInput input;
+ input.commitAttachedPose = detachAllObjectsCommitAttachedPoseCheckBox.getValue();
+
+ objpose::DetachAllObjectsFromRobotNodesOutput output = segment.doLocked(
+ [&segment, &input]() { return segment.detachAllObjectsFromRobotNodes(input); });
+ (void)output;
+ }
+ }
- VirtualRobot::RobotPtr Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
+ VirtualRobot::RobotPtr
+ Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
{
std::string robotName = _robotName;
if (robotName.empty())
{
- auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
+ auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
std::stringstream ss;
if (providerName.empty())
@@ -1285,8 +1320,7 @@ namespace armarx::armem::server::obj::instance
{
ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
<< "For these object poses, the object instance segment is not able "
- << "to provide transformed object poses (global and in robot root frame)."
- ;
+ << "to provide transformed object poses (global and in robot root frame).";
ARMARX_INFO << antiSpam << ss.str();
return nullptr;
@@ -1312,8 +1346,7 @@ namespace armarx::armem::server::obj::instance
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
VirtualRobot::RobotPtr robot = reader->getRobot(
- robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
@@ -1321,14 +1354,16 @@ namespace armarx::armem::server::obj::instance
if (not synchronized)
{
ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
- << " synchronized successfully (e.g., the global localization could be missing). "
+ << " synchronized successfully (e.g., the global localization "
+ "could be missing). "
<< "Make sure to synchronize it before use if necessary.";
}
// Store robot if valid.
loaded.emplace(robotName, robot);
}
- return robot; // valid or nullptr
+ return robot; // valid or nullptr
}
}
-}
+
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index c5e62d975ca161773c9ae4e8b7d7f5352f0f1922..b922f1211cbe769e89d9a55108b257b336c73642 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -9,24 +9,20 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
#include <RobotAPI/libraries/armem/core/Prediction.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "ArticulatedObjectVisu.h"
#include "Decay.h"
-
namespace armarx::armem::arondto
{
class ObjectInstance;
@@ -38,11 +34,11 @@ namespace armarx::armem::server::obj::instance
class Segment : public segment::SpecializedCoreSegment
{
public:
-
struct CommitStats
{
int numUpdated = 0;
};
+
using ObjectPose = objpose::ObjectPose;
using ObjectPoseSeq = objpose::ObjectPoseSeq;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
@@ -53,41 +49,46 @@ namespace armarx::armem::server::obj::instance
std::string robotNode = "Neck_2_Pitch";
float offset = 0.0f;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "calibration.");
};
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
void connect(viz::Client arviz);
-
- CommitStats commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
+ CommitStats commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses,
+ const std::string& providerName = "");
objpose::ObjectPoseMap getObjectPoses(const DateTime& now);
- objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, const DateTime& now);
+ objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName,
+ const DateTime& now);
- wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
+ wm::Entity* findObjectEntity(const ObjectID& objectID,
+ const std::string& providerName = "");
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
objpose::ProviderInfo getProviderInfo(const std::string& providerName);
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
+ objpose::AttachObjectToRobotNodeOutput
+ attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
+ objpose::DetachObjectFromRobotNodeOutput
+ detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
+ objpose::DetachAllObjectsFromRobotNodesOutput
+ detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
/**
@@ -100,7 +101,8 @@ namespace armarx::armem::server::obj::instance
*
* @param synchronized Indicates whether the agent is already synchronized to the current time.
*/
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ void updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
bool& synchronized) const;
@@ -120,34 +122,26 @@ namespace armarx::armem::server::obj::instance
getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
private:
-
ObjectPoseMap getLatestObjectPoses() const;
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now);
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
+ void updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now);
+ void updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
+ void updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
- void storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose);
+ void storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose);
std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
@@ -156,7 +150,6 @@ namespace armarx::armem::server::obj::instance
private:
-
void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
@@ -168,7 +161,6 @@ namespace armarx::armem::server::obj::instance
public:
-
/// Loaded robot models identified by the robot name.
struct RobotsCache : public armarx::Logging
{
@@ -177,8 +169,10 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, VirtualRobot::RobotPtr> loaded;
- VirtualRobot::RobotPtr get(const std::string& robotName, const std::string& providerName = "");
+ VirtualRobot::RobotPtr get(const std::string& robotName,
+ const std::string& providerName = "");
};
+
RobotsCache robots;
@@ -192,7 +186,6 @@ namespace armarx::armem::server::obj::instance
private:
-
struct Properties
{
bool discardSnapshotsWhileAttached = true;
@@ -201,7 +194,10 @@ namespace armarx::armem::server::obj::instance
std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
std::string sceneSnapshotsDirectory = "scenes";
std::string sceneSnapshotsToLoad = "";
+
+ std::vector<std::string> getSceneSnapshotsToLoad() const;
};
+
Properties p;
@@ -216,7 +212,6 @@ namespace armarx::armem::server::obj::instance
static const std::string timestampPlaceholder;
public:
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -229,15 +224,16 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox infiniteHistory;
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+ armarx::RemoteGui::Client::Button detachAllObjectsButton;
+ armarx::RemoteGui::Client::CheckBox detachAllObjectsCommitAttachedPoseCheckBox;
+
void setup(const Segment& data);
void update(Segment& data);
};
private:
-
std::unique_ptr<ArticulatedObjectVisu> visu;
-
};
-}
+} // namespace armarx::armem::server::obj::instance