diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0b67edc552c9bd77aa0f8ca78d95c2f2a5659282..8c14b9de7a9d80bcf2b09d45ae3253b94a2c8b5b 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -35,6 +35,7 @@
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <mutex>
#include <optional>
namespace armarx
@@ -71,17 +72,15 @@ namespace armarx
void
RobotHealth::monitorHealthUpdateTaskClb()
{
- std::shared_lock lock(updateMutex);
+ const std::scoped_lock lock(updateMutex);
ARMARX_TRACE;
auto now = armarx::core::time::DateTime::Now();
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
ARMARX_TRACE;
- UpdateEntry& e = updateEntries.at(i);
-
- std::unique_lock elock(e.mutex);
+ const std::scoped_lock elock(e.mutex);
if (!e.enabled)
{
@@ -129,12 +128,10 @@ namespace armarx
// get aggregated status
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
ARMARX_TRACE;
- UpdateEntry& e = updateEntries.at(i);
-
- std::shared_lock elock(e.mutex);
+ std::scoped_lock elock(e.mutex);
// trim history
while (e.history.size() > 20)
@@ -218,11 +215,11 @@ namespace armarx
{
ARMARX_TRACE;
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & updateEntrie : updateEntries)
{
- if (updateEntries.at(i).name == name)
+ if (updateEntrie.name == name)
{
- return &updateEntries.at(i);
+ return &updateEntrie;
}
}
@@ -234,11 +231,11 @@ namespace armarx
{
ARMARX_TRACE;
{
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & updateEntrie : updateEntries)
{
- if (updateEntries.at(i).name == name)
+ if (updateEntrie.name == name)
{
- return {false, updateEntries.at(i)};
+ return {false, updateEntrie};
}
}
}
@@ -254,12 +251,12 @@ namespace armarx
RobotHealth::signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current)
{
ARMARX_TRACE;
- std::unique_lock lockU(updateMutex);
+ std::scoped_lock lockU(updateMutex);
auto e = findOrCreateUpdateEntry(args.identifier);
{
- std::unique_lock lock(e.second.mutex);
+ std::scoped_lock lock(e.second.mutex);
if (e.first)
{
@@ -294,7 +291,7 @@ namespace armarx
// We hold a reference to 'o' which is an element in a vector.
// If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
- std::shared_lock lockU(updateMutex);
+ std::scoped_lock lockU(updateMutex);
auto* const entry = findUpdateEntry(identifier);
if (entry == nullptr)
@@ -306,7 +303,7 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(entry);
- std::unique_lock lock(entry->mutex);
+ std::scoped_lock lock(entry->mutex);
if (not entry->enabled)
{
@@ -325,21 +322,18 @@ namespace armarx
}
void
- armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
+ armarx::RobotHealth::unregister(const std::string& identifier, const Ice::Current&)
{
ARMARX_TRACE;
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
bool found = false;
{
- std::shared_lock lock(updateMutex);
-
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
- auto& e = updateEntries.at(i);
- if (e.name == componentName)
+ if (e.name == identifier)
{
- std::unique_lock elock(e.mutex);
+ std::scoped_lock elock(e.mutex);
e.required = false;
e.enabled = false;
found = true;
@@ -351,11 +345,11 @@ namespace armarx
if (found)
{
- ARMARX_INFO << "Component " << componentName << " disabled from heartbeat";
+ ARMARX_INFO << "Component " << identifier << " disabled from heartbeat";
}
else
{
- ARMARX_INFO << "Could not unregister component " << componentName << ". Not found.";
+ ARMARX_INFO << "Could not unregister component " << identifier << ". Not found.";
}
updateRequiredElements();
@@ -366,7 +360,7 @@ namespace armarx
const std::vector<std::string>& tags,
const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// add newly requested tags
for(const auto& tag : tags)
@@ -429,7 +423,7 @@ namespace armarx
const std::vector<std::string>& tags,
const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// update the required tags list / map
for(const auto& tag : tags)
@@ -443,7 +437,7 @@ namespace armarx
void
armarx::RobotHealth::resetRequiredTags(const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// treat the special provider (see onInitComponent()) appropriately
ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
@@ -457,7 +451,7 @@ namespace armarx
RobotHealthInfo
RobotHealth::getSummary(const Ice::Current&)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
ARMARX_TRACE;
RobotHealthInfo ret;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index a27f864e8f639983650ece96ac8d101f709b8618..1e45f31116382a9761b879eaffb80375541fdaf7 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -70,6 +70,29 @@ namespace armarx
return "RobotHealth";
}
+ // RobotHealthInterface interface
+ void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
+ void unregister(const std::string& identifier, const Ice::Current&) override;
+
+
+ void addRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void removeRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void resetRequiredTags(const Ice::Current& current) override;
+
+
+ void heartbeat(const std::string& identifier,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current) override;
+
+ std::string getTopicName(const Ice::Current& current) override;
+
+ RobotHealthInfo getSummary(const Ice::Current& current) override;
+
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -116,7 +139,7 @@ namespace armarx
bool required = false;
bool enabled = false;
- mutable std::shared_mutex mutex;
+ mutable std::mutex mutex;
struct TimeInfo
{
@@ -140,36 +163,13 @@ namespace armarx
void reportDebugObserver();
- // RobotHealthInterface interface
- public:
- void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
- void unregister(const std::string& identifier, const Ice::Current&) override;
-
-
- void addRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current) override;
- void removeRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current) override;
- void resetRequiredTags(const Ice::Current& current) override;
-
-
- void heartbeat(const std::string& identifier,
- const core::time::dto::DateTime& referenceTime,
- const Ice::Current& current) override;
-
- std::string getTopicName(const Ice::Current& current) override;
-
- // RobotHealthComponentInterface interface
- public:
- RobotHealthInfo getSummary(const Ice::Current& current) override;
-
- private:
+
void updateRequiredElements();
std::set<std::string> requestedTags() const;
- mutable std::shared_mutex updateMutex;
+ // Mutex to restrict access to all interface / public methods. This ensures that all requests are
+ // handled sequentially.
+ mutable std::mutex updateMutex;
std::deque<UpdateEntry> updateEntries;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 1b7f92ef951ef1b1223489b3e80fb09a84efa981..87ce13bc529cbc44c114171337e43192b83bd503 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -67,8 +67,7 @@ armarx::KinematicSubUnit::setupData(
auto nodes = robot->getRobotNodes();
for (auto& node : nodes)
{
- if ((node->isRotationalJoint() || node->isTranslationalJoint()) &&
- !devs.count(node->getName()))
+ if (node->isJoint() && !devs.count(node->getName()))
{
sensorLessJoints.push_back(node);
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 2a1f02718bb8b1fa9465445086238a4e04ba92dd..a89af6168a5f66f366a4df3a36b59c01906a0695 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,6 +24,7 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -45,9 +46,9 @@ HokuyoLaserUnit::onInitComponent()
offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName");
- topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
- offeringTopic(topicName);
- ARMARX_INFO << "Going to report on topic " << topicName;
+ laserScannerListenerProxyName = getProperty<std::string>("LaserScannerListenerName").getValue();
+ usingProxyFromProperty("LaserScannerListenerName");
+ ARMARX_INFO << "Going to report to listener " << laserScannerListenerProxyName;
updatePeriod = getProperty<int>("UpdatePeriod").getValue();
angleOffset = getProperty<float>("AngleOffset").getValue();
@@ -92,7 +93,7 @@ HokuyoLaserUnit::onConnectComponent()
{
ARMARX_TRACE;
- topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
+ getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
debugObserver = getTopic<DebugObserverInterfacePrx>(
getProperty<std::string>("DebugObserverName").getValue());
@@ -133,7 +134,7 @@ HokuyoLaserUnit::onConnectComponent()
info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
- device.scanTopic = topic;
+ device.listenerPrx = listenerPrx;
device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
@@ -181,7 +182,7 @@ HokuyoLaserUnit::createPropertyDefinitions()
std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
- return topicName;
+ return laserScannerListenerProxyName;
}
LaserScannerInfoSeq
@@ -230,6 +231,8 @@ HokuyoLaserScanDevice::run()
ARMARX_TRACE;
while (!task->isStopped())
{
+ ARMARX_INFO << deactivateSpam() << "Running update loop for laserscanner device " << ip;
+
IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10)
@@ -272,15 +275,16 @@ HokuyoLaserScanDevice::run()
errorCounter = 0;
- if (scanTopic)
+ if (listenerPrx)
{
- scanTopic->reportSensorValues(ip, frame, scan, now);
+
+ listenerPrx->reportSensorValues(ip, frame, scan, now);
}
else
{
- ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
+ ARMARX_WARNING << "No proxy available: IP: " << ip << ", Port: " << port;
}
- IceUtil::Time time_topicSensor = TimeUtil::GetTime();
+ IceUtil::Time time_proxyReport = TimeUtil::GetTime();
if (robotHealthPlugin != nullptr)
{
@@ -302,10 +306,10 @@ HokuyoLaserScanDevice::run()
new Variant((time_measure - time_start).toMilliSecondsDouble());
durations["update_ms"] =
new Variant((time_update - time_measure).toMilliSecondsDouble());
- durations["topic_sensor_ms"] =
- new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
+ durations["proxy_report_ms"] =
+ new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] =
- new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
+ new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
debugObserver->setDebugChannel(
"LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
@@ -318,9 +322,9 @@ HokuyoLaserScanDevice::run()
<< "Update: "
<< (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
<< "TopicSensor: "
- << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
+ << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
<< "TopicHeart: "
- << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()
+ << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
<< "ms\n";
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 0b8ad3985e9e3b1825054fe1339cb41a5c59f4cd..98da02fce21736694803272e4e76e046080d2cd4 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -47,7 +47,7 @@ namespace armarx
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ "LaserScannerListenerName", "CartographerMappingAndLocalization", "Name of the laser scan listener.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset",
-2.3561944902,
@@ -87,7 +87,7 @@ namespace armarx
LaserScan scan;
std::string componentName;
- LaserScannerUnitListenerPrx scanTopic;
+ LaserScannerUnitListenerPrx listenerPrx;
armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
@@ -150,8 +150,8 @@ namespace armarx
LaserScannerInfoSeq getConnectedDevices(const Ice::Current& c) const override;
private:
- std::string topicName;
- LaserScannerUnitListenerPrx topic;
+ std::string laserScannerListenerProxyName;
+ LaserScannerUnitListenerPrx listenerPrx;
int updatePeriod = 25;
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
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/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
index b0f94d03e4868e9051525ad5864c82fbe37a5b13..47b8601280e5191e99a7f1a4d56fec5aeaf75ab5 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -1,5 +1,7 @@
#include "LocationLoader.h"
+#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/eigen/ice_conversions.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -67,7 +69,12 @@ namespace armarx::priorknowledge::util
.get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
armarx::FramedPose fp(pose, frame, agent);
- Location loc(dataset, locationIdStr, fp);
+
+ // escape locationIdStr
+ auto locationIdStrSplitted = simox::alg::split(locationIdStr, "#");
+ std::string locationIdStrEscaped = simox::alg::trim_copy(locationIdStrSplitted[0]);
+
+ Location loc{{dataset, fp.agent, locationIdStrEscaped}, fp};
ret.push_back(loc);
}
return ret;
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
index 843d7b6eea739b23861d0ac22a097b9921757b5a..ea08e20229bb09989f2b4239a529d9b107a3e0f2 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
@@ -6,23 +6,20 @@
namespace armarx::priorknowledge::util
{
-
- LocationId::LocationId(const std::string& sourceId, const std::string& name) :
- sourceId(sourceId), name(name)
- {
- }
-
std::string
LocationId::toString() const
{
- return sourceId + "/" + name;
+ return sourceId + "/" + toStringWithoutSourceId();
}
- Location::Location(const std::string& sourceId,
- const std::string& name,
- const armarx::FramedPose& p) :
- id(sourceId, name), pose(p)
+ std::string
+ LocationId::toStringWithoutSourceId() const
{
+ if (! prefix.empty())
+ {
+ return prefix + "/" + name;
+ }
+ return name;
}
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index c85502812b77c4d443c70885ad955cd13ef416df..202b7734a2b7dea90e37d1df1f4a6886ba162b18 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -9,18 +9,16 @@ namespace armarx::priorknowledge::util
struct LocationId
{
std::string sourceId;
+ std::string prefix;
std::string name;
- LocationId(const std::string& sourceId, const std::string& name);
-
std::string toString() const;
+ std::string toStringWithoutSourceId() const;
};
struct Location
{
LocationId id;
armarx::FramedPose pose;
-
- Location(const std::string& sourceId, const std::string& name, const armarx::FramedPose& p);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index 7c63d33b767f10ace65279b9be98e9426e5d8644..0bc460c8bc27fea0f3b2dcdfe9ba46921680291a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx::plugins
{
static const std::string ARVIZ_TOPIC_PROPERTY_NAME = "ArVizTopicName";
@@ -11,28 +10,31 @@ namespace armarx::plugins
static const std::string ARVIZ_STORAGE_PROPERTY_NAME = "ArVizStorageName";
static const std::string ARVIZ_STORAGE_PROPERTY_DEFAULT = "ArVizStorage";
-
- std::string ArVizComponentPlugin::getTopicName()
+ std::string
+ ArVizComponentPlugin::getTopicName()
{
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)) :
- ARVIZ_TOPIC_PROPERTY_DEFAULT;
+ return parentDerives<Component>() ? parent<Component>().getProperty<std::string>(
+ makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME))
+ : ARVIZ_TOPIC_PROPERTY_DEFAULT;
}
- std::string ArVizComponentPlugin::getStorageName()
+ std::string
+ ArVizComponentPlugin::getStorageName()
{
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME)) :
- ARVIZ_STORAGE_PROPERTY_DEFAULT;
+ return parentDerives<Component>() ? parent<Component>().getProperty<std::string>(
+ makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME))
+ : ARVIZ_STORAGE_PROPERTY_DEFAULT;
}
- void ArVizComponentPlugin::preOnInitComponent()
+ void
+ ArVizComponentPlugin::preOnInitComponent()
{
parent().offeringTopic(getTopicName());
parent().usingProxy(getStorageName());
}
- void ArVizComponentPlugin::preOnConnectComponent()
+ void
+ ArVizComponentPlugin::preOnConnectComponent()
{
if (parentDerives<ArVizComponentPluginUser>())
{
@@ -40,7 +42,8 @@ namespace armarx::plugins
}
}
- void ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)))
{
@@ -58,11 +61,12 @@ namespace armarx::plugins
}
}
- viz::Client ArVizComponentPlugin::createClient()
+ viz::Client
+ ArVizComponentPlugin::createClient()
{
return viz::Client(parent(), getTopicName(), getStorageName());
}
-}
+} // namespace armarx::plugins
namespace armarx
{
@@ -71,8 +75,9 @@ namespace armarx
addPlugin(plugin);
}
- viz::Client ArVizComponentPluginUser::createArVizClient()
+ viz::Client
+ ArVizComponentPluginUser::createArVizClient()
{
return plugin->createClient();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 14aceaef8913538df4af2e5272abd125150ca352..3ab4e0530b54331305cdbc8f8ad05603607b0ac9 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -20,13 +20,13 @@
* GNU General Public License
*/
+#include "RobotUnitDataStreamingReceiver.h"
+
#include <Ice/ObjectAdapter.h>
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/ArmarXManager.h>
-#include "RobotUnitDataStreamingReceiver.h"
-
namespace armarx::detail::RobotUnitDataStreamingReceiver
{
class Receiver :
@@ -34,23 +34,37 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
virtual public RobotUnitDataStreaming::Receiver
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnitDataStreamingReceiver";
}
+
~Receiver()
{
std::lock_guard g{_data_mutex};
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
- void onExitComponent() override {}
-
- void update_async(
- const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
- const RobotUnitDataStreaming::TimeStepSeq& data,
- Ice::Long msgSequenceNbr,
- const Ice::Current&) override
+
+ void
+ onInitComponent() override
+ {
+ }
+
+ void
+ onConnectComponent() override
+ {
+ }
+
+ void
+ onExitComponent() override
+ {
+ }
+
+ void
+ update_async(const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
+ const RobotUnitDataStreaming::TimeStepSeq& data,
+ Ice::Long msgSequenceNbr,
+ const Ice::Current&) override
{
ptr->ice_response();
if (_discard_data)
@@ -65,32 +79,31 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
_data[seq] = data;
}
- std::atomic_bool _discard_data = false;
- std::mutex _data_mutex;
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
- Ice::Identity _identity;
+ std::atomic_bool _discard_data = false;
+ std::mutex _data_mutex;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
+ Ice::Identity _identity;
};
-}
+} // namespace armarx::detail::RobotUnitDataStreamingReceiver
namespace armarx
{
RobotUnitDataStreamingReceiver::RobotUnitDataStreamingReceiver(
const ManagedIceObjectPtr& obj,
const RobotUnitInterfacePrx& ru,
- const RobotUnitDataStreaming::Config& cfg
- ) : _obj{obj}, _ru{ru}
+ const RobotUnitDataStreaming::Config& cfg) :
+ _obj{obj}, _ru{ru}
{
ARMARX_CHECK_NOT_NULL(_obj);
ARMARX_CHECK_NOT_NULL(_ru);
_receiver = make_shared<detail::RobotUnitDataStreamingReceiver::Receiver>();
- _receiver->_identity.name =
- _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
- std::to_string(clock_t::now().time_since_epoch().count());
+ _receiver->_identity.name = _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
+ std::to_string(clock_t::now().time_since_epoch().count());
auto adapter = _obj->getArmarXManager()->getAdapter();
_proxy = RobotUnitDataStreaming::ReceiverPrx::uncheckedCast(
- adapter->add(_receiver, _receiver->_identity));
+ adapter->add(_receiver, _receiver->_identity));
_description = _ru->startDataStreaming(_proxy, cfg);
}
@@ -120,14 +133,15 @@ namespace armarx
ARMARX_INFO << deactivateSpam() << "waiting until receiver is removed from ice";
}
}
- _proxy = nullptr;
+ _proxy = nullptr;
_receiver = nullptr;
}
- std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
+ std::deque<RobotUnitDataStreaming::TimeStep>&
+ RobotUnitDataStreamingReceiver::getDataBuffer()
{
ARMARX_CHECK_NOT_NULL(_receiver);
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
{
std::lock_guard g{_receiver->_data_mutex};
std::swap(data, _receiver->_data);
@@ -169,14 +183,12 @@ namespace armarx
_tmp_data_buffer_seq_id = it->first;
for (auto& step : it->second)
{
- if (
- _last_iteration_id != -1 &&
- _last_iteration_id + 1 != step.iterationId
- )
+ if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
{
- ARMARX_VERBOSE << "Missing Iterations or iterations out of order! "
- << "This should not happen. "
- << VAROUT(_last_iteration_id) << ", " << VAROUT(step.iterationId);
+ ARMARX_INFO << deactivateSpam(10)
+ << "Missing Iterations or iterations out of order! "
+ << "This should not happen. " << VAROUT(_last_iteration_id) << ", "
+ << VAROUT(step.iterationId);
}
_last_iteration_id = step.iterationId;
_data_buffer.emplace_back(std::move(step));
@@ -186,12 +198,14 @@ namespace armarx
return _data_buffer;
}
- const RobotUnitDataStreaming::DataStreamingDescription& RobotUnitDataStreamingReceiver::getDataDescription() const
+ const RobotUnitDataStreaming::DataStreamingDescription&
+ RobotUnitDataStreamingReceiver::getDataDescription() const
{
return _description;
}
- std::string RobotUnitDataStreamingReceiver::getDataDescriptionString() const
+ std::string
+ RobotUnitDataStreamingReceiver::getDataDescriptionString() const
{
std::stringstream str;
const auto& entr = getDataDescription().entries;
@@ -202,4 +216,4 @@ namespace armarx
}
return str.str();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index 22ae30fbd8489aff451fda127a170f58548f5fcc..45b26e7c9e8c4dd124b762d297d2fb0dca29a317 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -54,6 +54,10 @@ namespace armarx::armem::client::util
virtual void connect();
const Properties& properties() const;
+ void setProperties(const Properties& p)
+ {
+ props = p;
+ }
protected:
diff --git a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
index 22d418142d784080f8f4f99410a25a272d5c628f..38d927aa828a2e1d554492527093c0cb21b09035 100644
--- a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
@@ -11,12 +11,20 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
+
+ armem_objects
+ armem_robot_state
+
# aronjsonconverter
SOURCES
./aron_conversions.cpp
+ ./frame_conversions.cpp
+ ./client/Reader.cpp
HEADERS
./aron_conversions.h
+ ./frame_conversions.h
+ ./client/Reader.h
)
armarx_enable_aron_file_generation_for_target(
diff --git a/source/RobotAPI/libraries/armem_locations/aron/Location.xml b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
index 9a0539d5d40a3b3b91dc32ad2034077a815437ff..d53f51c2c8f75bacae2868ae9bc155aad717379e 100644
--- a/source/RobotAPI/libraries/armem_locations/aron/Location.xml
+++ b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
@@ -1,41 +1,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/>
- </AronIncludes>
<GenerateTypes>
- <!--
- ToDo: Model regions. Ideas:
- - Polygon (convex, non-convex)
- -
- -->
-
-
- <Object name='armarx::navigation::location::arondto::ObjectRelativeLocation'>
-
- <ObjectChild key='objectInstanceID'>
- <armarx::armem::arondto::MemoryID />
- </ObjectChild>
-
- <ObjectChild key='relativeRobotPose'>
- <Pose />
- </ObjectChild>
-
- </Object>
-
-
<Object name='armarx::navigation::location::arondto::Location'>
- <ObjectChild key='globalRobotPose'>
- <Pose />
- </ObjectChild>
-
- <ObjectChild key='relativeToObject'>
- <armarx::navigation::location::arondto::ObjectRelativeLocation optional="true" />
+ <ObjectChild key='framedPose'>
+ <FramedPose />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0fd0c6a1b5258d7960e9179c649ab0286b26be5a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -0,0 +1,108 @@
+#include "Reader.h"
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
+#include "../frame_conversions.h"
+
+namespace armarx::armem::locations::client
+{
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem),
+ objReader(memoryNameSystem),
+ robotReader(memoryNameSystem)
+ {
+ }
+
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ const std::string prefix = propertyPrefix;
+
+ def->optional(p.memoryName, prefix + "MemoryName");
+ objReader.registerPropertyDefinitions(def);
+ robotReader.registerPropertyDefinitions(def);
+ }
+
+ void
+ Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
+ try
+ {
+ // simply wait until memory is ready. Do nothing with reader but get prx
+ locationReader = memoryNameSystem.useReader(p.memoryName);
+
+ ARMARX_IMPORTANT << "Connected to Memory '" << p.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ objReader.connect();
+ robotReader.connect();
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ Reader::getAllLocations()
+ {
+ auto mId = armarx::armem::MemoryID{p.memoryName, "Location"};
+ auto res = locationReader.getLatestSnapshotsIn(mId);
+
+ if (res.success)
+ {
+ std::map<std::string, armarx::navigation::location::arondto::Location> ret;
+ res.memory.forEachEntity(
+ [&ret](const armarx::armem::wm::Entity& e)
+ {
+ auto i = e.findLatestInstance();
+ if (i)
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
+ }
+ });
+ return ret;
+ }
+
+ throw error::ArMemError(res.errorMessage);
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ Reader::getAllLocationsInGlobalFrame()
+ {
+ auto locations = this->getAllLocations();
+ auto objects = objReader.queryLatestObjectInstances();
+ auto robotDecs = robotReader.queryDescriptions(armarx::core::time::DateTime::Now());
+
+ for (auto& [locName, location] : locations)
+ {
+ (void) locName;
+ if (location.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ location.framedPose.header.agent = ""; //sanity set
+ continue;
+ }
+ // check if we have an object that matches the relative location
+ for (const auto& [objectInstanceId, object] : objects)
+ {
+ if (simox::alg::starts_with(objectInstanceId, location.framedPose.header.agent))
+ {
+ toGlobal(location, object);
+ }
+ }
+
+ // TODO: check if we have an robot that matches the relative location
+ }
+
+ return locations;
+ }
+
+
+} // namespace armarx::armem::locations::client
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.h b/source/RobotAPI/libraries/armem_locations/client/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad895c269e52fd2e4ec1b6e6bbda037fbefedb04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+
+namespace armarx::armem::locations::client
+{
+ class Reader
+ {
+ public:
+ struct Properties
+ {
+ std::string memoryName = "Navigation";
+ };
+
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ Properties
+ getProperties()
+ {
+ return this->p;
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location> getAllLocations();
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ getAllLocationsInGlobalFrame();
+
+ private:
+ Properties p;
+
+ const std::string propertyPrefix = "mem.nav.location";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armarx::armem::obj::instance::Reader objReader;
+ armarx::armem::robot_state::RobotReader robotReader;
+
+ armarx::armem::client::Reader locationReader;
+ };
+
+} // namespace armarx::armem::locations::client
diff --git a/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp b/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50ef19ce29d8432970e47071ac3dc4358cef6745
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::armem
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_locations/frame_conversions.h b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..876e6f9529fb83c88c5f7575daed6c7437f4b3f3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+namespace armarx::armem::locations
+{
+ void
+ toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
+ const Eigen::Matrix4f& framedToGlobal)
+ {
+ if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ // No need to change to global
+ framedLoc.framedPose.header.agent = ""; // sanity set
+ return;
+ }
+
+ framedLoc.framedPose.pose = framedToGlobal * framedLoc.framedPose.pose;
+ framedLoc.framedPose.header.frame = armarx::GlobalFrame;
+ framedLoc.framedPose.header.agent = "";
+ }
+
+ void
+ toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
+ const armarx::objpose::ObjectPose& objPose)
+ {
+ if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ // No need to change to global
+ framedLoc.framedPose.header.agent = ""; // sanity set
+ return;
+ }
+
+ if (simox::alg::starts_with(objPose.objectID.str(), framedLoc.framedPose.header.agent))
+ {
+ ARMARX_CHECK(framedLoc.framedPose.header.frame ==
+ "root"); // TODO: Right now we only allow root
+ toGlobal(framedLoc, objPose.objectPoseGlobal);
+ return;
+ }
+
+ throw armarx::LocalException(
+ "Could not convert a framedLocation because the used object is wrong!");
+ }
+
+} // namespace armarx::armem::locations
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 66bd99afbcf294f533a180bfe8727b1e73bc01a5..b2b740b735e933245a44bbfd3ce8d1c437ff30b2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -88,7 +88,13 @@ namespace armarx::armem::obj::instance
auto requestResult = objPoseStorage->requestObjects(req);
- return requestResult.results.at(requestObject).result.success;
+ if (requestResult.results.count(requestObject))
+ {
+ return requestResult.results.at(requestObject).result.success;
+ }
+ return false;
+
+
}
std::optional<objpose::ObjectPose>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 0a77d4d0fe8e77557a5fb4659b52aa9b1be23035..81a0fc03092bbbe67c476d3e5f2146a28d62517a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -74,6 +74,11 @@ namespace armarx::armem::obj::instance
return this->p;
}
+ objpose::ObjectPoseStorageInterfacePrx getObjectPoseStorage() const
+ {
+ return objPoseStorage;
+ }
+
private:
Properties p;
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
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index e808a4b2e31f7d3763b0a0a8052502eb77260f27..c163ff7f68ca6fae8b4bccdf6ecfce72187f10fa 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -1,4 +1,5 @@
#include "TransformHelper.h"
+
#include <optional>
#include <string>
@@ -6,55 +7,53 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose/interpolate.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/core/exceptions/LocalException.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::common::robot_state::localization
{
- template <class ...Args>
+ template <class... Args>
TransformResult
TransformHelper::_lookupTransform(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "' for robot `" + query.header.agent + "`."};
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
- const std::optional<armem::Time> sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ const std::optional<armem::Time> sanitizedTimestamp =
+ _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
- if(not sanitizedTimestamp.has_value())
+ if (not sanitizedTimestamp.has_value())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::Error,
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::Error,
.errorMessage = "Error: Issue with timestamp"};
}
@@ -66,79 +65,69 @@ namespace armarx::armem::common::robot_state::localization
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
header.timestamp = sanitizedTimestamp.value();
-
+
if (transforms.empty())
{
- ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
+ ARMARX_INFO << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
" -> " + query.header.frame +
"'. No memory data in time range."};
}
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
+ const Eigen::Affine3f transform = std::accumulate(
+ transforms.begin(), transforms.end(), Eigen::Affine3f::Identity(), std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
return {.transform = {.header = header, .transform = transform},
- .status = TransformResult::Status::Success};
+ .status = TransformResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
TransformChainResult
TransformHelper::_lookupTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- ARMARX_VERBOSE << "TF chain is empty";
- return
- {
- .header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "' for robot `" + query.header.agent + "`."
- };
+ ARMARX_DEBUG << "TF chain is empty";
+ return {.header = query.header,
+ .transforms = std::vector<Eigen::Affine3f>{},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
if (transforms.empty())
{
- ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ ARMARX_INFO << deactivateSpam(1) << "No transform available.";
return
{
.header = query.header,
- .transforms = {},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range."
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame + "'. No memory data in time range."
};
}
ARMARX_DEBUG << "Found valid transform";
- return
- {
- .header = query.header,
- .transforms = transforms,
- .status = TransformChainResult::Status::Success
- };
+ return {.header = query.header,
+ .transforms = transforms,
+ .status = TransformChainResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<std::string>
TransformHelper::_buildTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -148,14 +137,15 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain;
- const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
const std::vector<std::string> tfs = agentProviderSegment.getEntityNames();
// lookup from robot root to global
std::map<std::string, std::string> tfLookup;
- for(const std::string& tf: tfs)
+ for (const std::string& tf : tfs)
{
const auto frames = simox::alg::split(tf, ",");
ARMARX_CHECK_EQUAL(frames.size(), 2);
@@ -165,60 +155,66 @@ namespace armarx::armem::common::robot_state::localization
std::string currentFrame = query.header.parentFrame;
chain.push_back(currentFrame);
- while(tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
+ while (tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
{
currentFrame = tfLookup.at(currentFrame);
chain.push_back(currentFrame);
}
-
+
ARMARX_DEBUG << VAROUT(chain);
if (chain.empty() or chain.back() != query.header.frame)
{
- ARMARX_WARNING << deactivateSpam(60) << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "' for robot `" + query.header.agent + "`.";
+ ARMARX_INFO << deactivateSpam(60) << "Cannot create tf lookup chain '"
+ << query.header.parentFrame << " -> " << query.header.frame
+ << "' for robot `" + query.header.agent + "`.";
return {};
}
std::vector<std::string> frameChain;
- for(size_t i = 0; i < (chain.size() - 1); i++)
+ for (size_t i = 0; i < (chain.size() - 1); i++)
{
- frameChain.push_back(chain.at(i) + "," + chain.at(i+1));
+ frameChain.push_back(chain.at(i) + "," + chain.at(i + 1));
}
return frameChain;
}
- template <class ...Args>
+ template <class... Args>
std::optional<armarx::core::time::DateTime>
- TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
+ TransformHelper::_obtainTimestamp(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const armem::Time& timestamp)
{
-
+
// first we check which the newest timestamp is
std::optional<int64_t> timeSinceEpochUs = std::nullopt;
- localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
- auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
-
- if(snapshot == nullptr)
+ localizationCoreSegment.forEachEntity(
+ [&timeSinceEpochUs, ×tamp](const auto& entity)
{
- return;
- }
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
- if(not snapshot->hasInstance(0))
- {
- return;
- }
+ if (snapshot == nullptr)
+ {
+ return;
+ }
+
+ if (not snapshot->hasInstance(0))
+ {
+ return;
+ }
- const armem::wm::EntityInstance& item = snapshot->getInstance(0);
- const auto tf = _convertEntityToTransform(item);
-
- const auto& dataTs = tf.header.timestamp;
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
- timeSinceEpochUs = std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
- });
+ const auto& dataTs = tf.header.timestamp;
- if(not timeSinceEpochUs.has_value())
+ timeSinceEpochUs =
+ std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
+ });
+
+ if (not timeSinceEpochUs.has_value())
{
return std::nullopt;
}
@@ -226,11 +222,11 @@ namespace armarx::armem::common::robot_state::localization
// then we ensure that the timestamp is not more recent than the query timestamp
timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
+ return armarx::core::time::DateTime(
+ armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<Eigen::Affine3f>
TransformHelper::_obtainTransforms(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -250,10 +246,9 @@ namespace armarx::armem::common::robot_state::localization
std::transform(tfChain.begin(),
tfChain.end(),
std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return _obtainTransform(entityName, agentProviderSegment, timestamp);
- });
+ [&](const std::string& entityName) {
+ return _obtainTransform(entityName, agentProviderSegment, timestamp);
+ });
return transforms;
}
catch (const armem::error::MissingEntry& missingEntryError)
@@ -276,8 +271,7 @@ namespace armarx::armem::common::robot_state::localization
return {};
}
-
- template <class ...Args>
+ template <class... Args>
Eigen::Affine3f
TransformHelper::_obtainTransform(
const std::string& entityName,
@@ -327,7 +321,6 @@ namespace armarx::armem::common::robot_state::localization
return transforms.front().transform;
}
-
::armarx::armem::robot_state::Transform
TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
@@ -344,18 +337,13 @@ namespace armarx::armem::common::robot_state::localization
findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
const armem::Time& timestamp)
{
- const auto comp = [](const armem::Time & timestamp, const auto & transform)
- {
- return transform.header.timestamp < timestamp;
- };
+ const auto comp = [](const armem::Time& timestamp, const auto& transform)
+ { return transform.header.timestamp < timestamp; };
const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
- auto timestampBeyond =
- [timestamp](const ::armarx::armem::robot_state::Transform & transform)
- {
- return transform.header.timestamp > timestamp;
- };
+ auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform& transform)
+ { return transform.header.timestamp > timestamp; };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
@@ -374,7 +362,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Entering";
ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
+ << "The queue has to contain at least two items to perform a lookup";
ARMARX_DEBUG << "Entering ... "
<< "Q front " << queue.front().header.timestamp << " "
@@ -384,11 +372,11 @@ namespace armarx::armem::common::robot_state::localization
// TODO(fabian.reister): sort queue.
ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
+ << "Cannot perform lookup into the future!";
// ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
+ << "Cannot perform lookup. Timestamp too old";
// => now we know that there is an element right after and before the timestamp within our queue
ARMARX_DEBUG << "Entering 2";
@@ -402,36 +390,39 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "deref";
// the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
+ const double t =
+ (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
+ (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
ARMARX_DEBUG << "interpolate";
- return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
+ return simox::math::interpolatePose(
+ posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
}
-
- TransformResult TransformHelper::lookupTransform(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformResult
+ TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
- TransformResult TransformHelper::lookupTransform(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+
+ TransformResult
+ TransformHelper::lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
-
- TransformChainResult TransformHelper::lookupTransformChain(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformChainResult
+ TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransformChain(localizationCoreSegment, query);
}
- TransformChainResult TransformHelper::lookupTransformChain(
+
+ TransformChainResult
+ TransformHelper::lookupTransformChain(
const armem::server::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index b8b3190d1bae586226c60b490bb399abe5799272..41eaf6e2ec2f2bcead697ec6ca7e86868bca44c4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -117,6 +117,24 @@ namespace armarx::armem::server::robot_state
}
}
+ void Visu::visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ {
+
+ std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
+
+ for (const auto& [robotName, robotFrames] : frames)
+ {
+ int i = 0;
+ for (const auto& frame : robotFrames)
+ {
+ layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+ }
+ }
+ }
+
+
void Visu::visualizeRun()
{
@@ -213,6 +231,12 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+ {
+ viz::Layer layerFrames = arviz.layer("FramesIndividual");
+ visualizeFramesIndividual(layerFrames, frames);
+ layers.push_back(layerFrames);
+ }
+
// Commit layers.
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00..99321b656be9977a23096e3b9bdd9ed4ba62a101 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -71,6 +71,11 @@ namespace armarx::armem::server::robot_state
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ static
+ void visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 53021dc7d88f9362a967f395b1381d07934f271e..34260f51e36951f7b9a0a68369cf8317e8a4dedc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -3,7 +3,6 @@
#include <filesystem>
#include <istream>
-
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -11,13 +10,11 @@
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
RobotUnitReader::RobotUnitReader() = default;
-
void
RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
@@ -48,7 +45,6 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
void
RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
@@ -56,14 +52,22 @@ namespace armarx::armem::server::robot_state::proprioception
while (task and not task->isStopped())
{
+ auto start = std::chrono::high_resolution_clock::now();
+
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
// will lock a mutex
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]",
+ commit->timestamp.toMicroSecondsSinceEpoch());
+
dataBuffer.push(std::move(commit.value()));
}
-
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
if (debugObserver)
{
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t run [ms]",
+ duration.count() / 1000.f);
debugObserver->sendDebugObserverBatch();
}
@@ -71,13 +75,24 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
ARMARX_CHECK_NOT_NULL(converterProprioception);
- const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
+
+ std::optional<RobotUnitDataStreaming::TimeStep> data;
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ data = fetchLatestData();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Fetch [ms]",
+ duration.count() / 1000.f);
+ }
+ }
if (not data.has_value())
{
return std::nullopt;
@@ -88,16 +103,16 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitData result;
- if(converterProprioception != nullptr)
+ if (converterProprioception != nullptr)
{
result.proprioception = converterProprioception->convert(data.value(), description);
}
- if(converterExteroception != nullptr)
+ if (converterExteroception != nullptr)
{
result.exteroception = converterExteroception->convert(data.value(), description);
}
-
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
@@ -114,7 +129,6 @@ namespace armarx::armem::server::robot_state::proprioception
return result;
}
-
std::optional<RobotUnitDataStreaming::TimeStep>
RobotUnitReader::fetchLatestData()
{
@@ -122,6 +136,14 @@ namespace armarx::armem::server::robot_state::proprioception
if (debugObserver)
{
debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+ if (data.size())
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | RT Timestamp USec",
+ data.back().timestampUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | RT Timestamp Since Last Iteration",
+ data.back().timesSinceLastIterationUSec);
+ }
}
if (data.empty())
{
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index 006016c5daa24d555c8a5d9537b8bfbbd0c96229..33340476f9568dce65fbd43bcd3f745f80df32f6 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -13,3 +13,5 @@ add_subdirectory(StatechartExecutionGroup)
add_subdirectory(ProsthesisKinestheticTeachIn)
add_subdirectory(DebugDrawerToArVizGroup)
+
+add_subdirectory(ObjectMemoryGroup)
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a751e89b2566ed34fadcfa9c04943088955c1d5e
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt
@@ -0,0 +1,31 @@
+# Modern CMake way:
+#armarx_add_statechart(ObjectMemoryGroup
+# SOURCES
+# ObjectMemoryGroupRemoteStateOfferer.cpp
+# HEADERS
+# ObjectMemoryGroupRemoteStateOfferer.h
+# DEPENDENCIES
+#)
+
+# Legacy CMake way:
+
+armarx_component_set_name("ObjectMemoryGroup")
+
+set(COMPONENT_LIBS
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+ # RobotAPI
+ RobotAPIArmarXObjects
+ armem_objects
+)
+
+set(SOURCES
+ ObjectMemoryGroupRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+ ObjectMemoryGroupRemoteStateOfferer.h
+ ObjectMemoryGroup.scgxml
+)
+
+armarx_generate_statechart_cmake_lists()
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..8f7d00053ef37819464e9d1b7bc4171cf4caf72b
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="ObjectMemoryGroup" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPIInterfaces.MemoryNameSystem"/>
+ </Proxies>
+ <Folder basename="test">
+ <State filename="RequestObjectsTest.xml" visibility="public"/>
+ </Folder>
+ <State filename="RequestObjects.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1324dfff62f4a6ec3df04e61d451f062125a50c8
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
@@ -0,0 +1,29 @@
+#include "ObjectMemoryGroupRemoteStateOfferer.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+ // DO NOT EDIT NEXT LINE
+ ObjectMemoryGroupRemoteStateOfferer::SubClassRegistry ObjectMemoryGroupRemoteStateOfferer::Registry(ObjectMemoryGroupRemoteStateOfferer::GetName(), &ObjectMemoryGroupRemoteStateOfferer::CreateInstance);
+
+ ObjectMemoryGroupRemoteStateOfferer::ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > (reader)
+ {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+
+ // DO NOT EDIT NEXT FUNCTION
+ std::string ObjectMemoryGroupRemoteStateOfferer::GetName()
+ {
+ return "ObjectMemoryGroupRemoteStateOfferer";
+ }
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateOffererFactoryBasePtr ObjectMemoryGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+ {
+ return XMLStateOffererFactoryBasePtr(new ObjectMemoryGroupRemoteStateOfferer(reader));
+ }
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..882627aec09b55c16b4b0f07fe313c0138392368
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "ObjectMemoryGroupStatechartContext.generated.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+ class ObjectMemoryGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..562ddabd56b6d07428f8ce2b386fa682ac95dd1f
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
@@ -0,0 +1,133 @@
+#include "RequestObjects.h"
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include "ObjectMemoryGroupStatechartContext.generated.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+ // DO NOT EDIT NEXT LINE
+ RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(), &RequestObjects::CreateInstance);
+
+ RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate<RequestObjects>(stateData), RequestObjectsGeneratedBase<RequestObjects>(stateData)
+ {
+ }
+
+ void RequestObjects::onEnter()
+ {
+
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+ }
+
+ void RequestObjects::run()
+ {
+ if (not in.getEnable())
+ {
+ emitSuccess();
+ return;
+ }
+
+ using Reader = armarx::armem::obj::instance::Reader;
+
+ const std::string provider = in.isProviderSet() ? in.getProvider() : "";
+
+ const std::vector<std::string> objectIdsString = in.getObjectIds();
+ const armarx::Duration relativeTimeout = armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
+
+ std::stringstream info;
+ std::stringstream warn;
+
+ std::vector<armarx::ObjectID> objectIds;
+ for (const std::string& idString : objectIdsString)
+ {
+ try
+ {
+ armarx::ObjectID id = armarx::ObjectID::FromString(idString);
+ objectIds.push_back(id);
+ info << "Requesting object " << id << "\n";
+ }
+ catch (const armarx::LocalException& e)
+ {
+ warn << "\nGiven object ID '" << idString << "' could not parsed as ObjectID: " << e.what();
+ }
+ }
+
+ auto context = getContext<ObjectMemoryGroupStatechartContext>();
+ armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context);
+
+ Reader reader{mns};
+ reader.connect();
+ objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage();
+
+ armarx::objpose::observer::RequestObjectsInput input;
+ input.provider = provider;
+ for (const armarx::ObjectID& id : objectIds)
+ {
+ armarx::toIce(input.request.objectIDs.emplace_back(), id);
+ }
+ input.request.relativeTimeoutMS = relativeTimeout.toMilliSeconds();
+
+
+ objpose::observer::RequestObjectsOutput output;
+ try
+ {
+ output = storage->requestObjects(input);
+ }
+ catch (const Ice::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to request object localization: " << e.what();
+ emitFailure();
+ return;
+ }
+
+ for (const auto& [id, result] : output.results)
+ {
+ if (result.result.success)
+ {
+ info << "Requested object " << id << " via provider '" << result.providerName << "'.\n";
+ }
+ else
+ {
+ warn << "\nFailed to request the object " << id << " for localization.";
+ }
+ }
+
+ if (not info.str().empty())
+ {
+ ARMARX_INFO << info.str();
+ }
+ if (not warn.str().empty())
+ {
+ ARMARX_WARNING << "The following issues occurred whhen requesting objects for localization:"
+ << warn.str();
+ }
+
+ emitSuccess();
+ }
+
+ //void RequestObjects::onBreak()
+ //{
+ // // put your user code for the breaking point here
+ // // execution time should be short (<100ms)
+ //}
+
+ void RequestObjects::onExit()
+ {
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+ }
+
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateFactoryBasePtr RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
+ {
+ return XMLStateFactoryBasePtr(new RequestObjects(stateData));
+ }
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
new file mode 100644
index 0000000000000000000000000000000000000000..4390865434749d591d6082df003c55e3b8560cd6
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.generated.h>
+
+namespace armarx::ObjectMemoryGroup
+{
+ class RequestObjects :
+ public RequestObjectsGeneratedBase < RequestObjects >
+ {
+ public:
+ RequestObjects(const XMLStateConstructorParams& stateData);
+
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8b32c20109a1bee77bcd3ee1203716d3abb02f0d
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="RequestObjects" uuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" width="800" height="600" type="Normal State">
+ <InputParameters>
+ <Parameter name="Enable" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no"/>
+ <Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="yes"/>
+ <Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no"/>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml b/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..edb16e98297feb19d1769ca6c54ab28901da7ad1
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml
@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="RequestObjectsTest" uuid="E74E1D8D-2E97-4846-9282-73AC4F49FAEE" width="493.167" height="355.278" type="Normal State">
+ <InputParameters>
+ <Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/spraybottle"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/handwash-paste"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/mounting-adhesive"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/multipurpose-grease"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="Maintenance/spraybottle\nMaintenance/handwash-paste\nMaintenance/mounting-adhesive\nMaintenance/multipurpose-grease"/>
+ </Parameter>
+ <Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"sim_track"}}' docValue="sim_track"/>
+ </Parameter>
+ <Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":10000}}' docValue="10000"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates>
+ <EndState name="Failure" event="Failure" left="363.167" top="140.056" boundingSquareSize="99.6636"/>
+ <LocalState name="RequestObjects" refuuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" left="114.167" top="196.278" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="363.167" top="250.278" boundingSquareSize="99.6636"/>
+ </Substates>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ </Events>
+ <StartState substateName="RequestObjects">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Parent" from="ObjectIds" to="ObjectIds"/>
+ <ParameterMapping sourceType="Parent" from="Provider" to="Provider"/>
+ <ParameterMapping sourceType="Parent" from="RelativeTimeoutMilliseconds" to="RelativeTimeoutMilliseconds"/>
+ </ParameterMappings>
+ <SupportPoints>
+ <SupportPoint posX="67.1067" posY="233.278"/>
+ <SupportPoint posX="76.5839" posY="233.278"/>
+ <SupportPoint posX="88.7241" posY="233.278"/>
+ <SupportPoint posX="101.186" posY="233.278"/>
+ </SupportPoints>
+ </StartState>
+ <Transitions>
+ <Transition from="RequestObjects" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.588" posY="221.9"/>
+ <SupportPoint posX="253.892" posY="213.031"/>
+ <SupportPoint posX="308.683" posY="200.669"/>
+ <SupportPoint posX="350.786" posY="191.175"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="RequestObjects" to="Success" eventName="Success">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.358" posY="252.153"/>
+ <SupportPoint posX="221.884" posY="254.613"/>
+ <SupportPoint posX="229.602" posY="256.935"/>
+ <SupportPoint posX="237" posY="258.833"/>
+ <SupportPoint posX="274.158" posY="268.372"/>
+ <SupportPoint posX="316.529" posY="275.551"/>
+ <SupportPoint posX="350.428" posY="280.424"/>
+ </SupportPoints>
+ </Transition>
+ </Transitions>
+</State>
+