diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 2388c61e39a8e0582f63b11dec0ca45ec51c8d24..fac11b677edbf7fc26cd2538e453981601e94f13 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -328,5 +328,13 @@
propertyName="ObjectPoseStorageName"
propertyIsOptional="true"
propertyDefaultValue="ObjectMemory" />
+ <Proxy include="RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h"
+ humanName="Memory Name System"
+ typeName="armem::mns::MemoryNameSystemInterfacePrx"
+ memberName="MemoryNameSystem"
+ getterName="getMemoryNameSystem"
+ propertyName="MemoryNameSystemName"
+ propertyIsOptional="true"
+ propertyDefaultValue="MemoryNameSystem" />
</Lib>
</VariantInfo>
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 22981bc341684bcac781c01b26249ba6376c1ad2..094be533e812cf6a67c62d126afa77437d68ff57 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -92,6 +92,14 @@
# ArmarX.LoggingGroup = ""
+# ArmarX.ObjectMemory.:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory. = ""
+
+
# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
@@ -166,6 +174,41 @@
# ArmarX.ObjectMemory.mem.cls.CoreSegmentName = Class
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Environment/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Environment/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = true
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
# - Default: true
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 53da6a10a507a5a63c5205f22d8a6643bfb867c0..171da9c1a660b8257b2abc3f1b1f05ff6c0deb98 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -1,5 +1,7 @@
#include "ObjectPose.h"
+#include <SimoxUtility/math/pose/invert.h>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
@@ -100,6 +102,26 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+ void ObjectPose::setObjectPoseRobot(
+ const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
+ {
+ this->objectPoseRobot = objectPoseRobot;
+ if (updateObjectPoseGlobal)
+ {
+ this->objectPoseGlobal = robotPose * objectPoseRobot;
+ }
+ }
+
+ void ObjectPose::setObjectPoseGlobal(
+ const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot)
+ {
+ this->objectPoseGlobal = objectPoseGlobal;
+ if (updateObjectPoseRobot)
+ {
+ this->objectPoseRobot = simox::math::inverted_pose(robotPose) * objectPoseGlobal;
+ }
+ }
+
std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 556ac04be8c43a686e9d8f0e839fdb5bb13f7daa..023d6351f3bcf2533188e0df197b7f8e61806388 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -39,6 +39,9 @@ namespace armarx::objpose
void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
+ void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
+ void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
+
/// Name of the providing component.
std::string providerName;
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index d9cf1e4f39d48bbc6f4aeb1dd39bc448c3cd6cbf..011948cc29860ed26b63dfdf93e59358c8180e08 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -86,7 +86,7 @@ namespace armarx::armem::client
void
Reader::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
{
- for (const auto& [subscription, callback] : callbacks)
+ for (const auto& [subscription, callbacks] : this->callbacks)
{
std::vector<MemoryID> matchingSnapshotIDs;
@@ -100,7 +100,10 @@ namespace armarx::armem::client
if (not matchingSnapshotIDs.empty())
{
- callback(subscription, matchingSnapshotIDs);
+ for (auto& callback : callbacks)
+ {
+ callback(subscription, matchingSnapshotIDs);
+ }
}
}
}
@@ -124,7 +127,15 @@ namespace armarx::armem::client
void
Reader::subscribe(const MemoryID& id, callback callback)
{
- callbacks[id] = callback;
+ callbacks[id].push_back(callback);
+ }
+
+ void Reader::subscribe(const MemoryID& subscriptionID, callback_updated_only callback)
+ {
+ subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ callback(updatedSnapshotIDs);
+ });
}
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index cf0adbb45b087faf16188cb89a45bd22839dda97..5cbeff84a3b6f979cd39d87b8271ff7e81d0c01b 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -27,8 +27,13 @@ namespace armarx::armem::client
{
using callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using callback_updated_only = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+
template <class CalleeT>
using member_callback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ template <class CalleeT>
+ using member_callback_updated_only = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+
public:
@@ -54,6 +59,7 @@ namespace armarx::armem::client
data::StoreResult readAndStore(const data::StoreInput& input) const;
void subscribe(const MemoryID& subscriptionID, callback callback);
+ void subscribe(const MemoryID& subscriptionID, callback_updated_only callback);
/**
* Subscribe with a class member function:
* @code
@@ -69,6 +75,15 @@ namespace armarx::armem::client
};
subscribe(subscriptionID, cb);
}
+ template <class CalleeT>
+ void subscribe(const MemoryID& subscriptionID, CalleeT* callee, member_callback_updated_only<CalleeT> callback)
+ {
+ auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ };
+ subscribe(subscriptionID, cb);
+ }
/// Function handling updates from the MemoryListener ice topic.
void updated(const std::vector<MemoryID>& updatedIDs) const;
@@ -84,7 +99,7 @@ namespace armarx::armem::client
private:
- std::unordered_map<MemoryID, callback> callbacks;
+ std::unordered_map<MemoryID, std::vector<callback>> callbacks;
};
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index 42232a707d2e3e1a111662f97db641ecfca614d0..e7ce598a68d393d6da88a9e4ec52a801d10999c5 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -44,4 +44,60 @@ namespace armarx::armem::client::query
return _addChild(selector);
}
+ void Builder::allInCoreSegment(const MemoryID& coreSegmentID)
+ {
+ coreSegments().withName(coreSegmentID.coreSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().all();
+ }
+
+ void Builder::allLatestInCoreSegment(const MemoryID& coreSegmentID)
+ {
+ coreSegments().withName(coreSegmentID.coreSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ void Builder::allInProviderSegment(const MemoryID& providerSegmentID)
+ {
+ coreSegments().withName(providerSegmentID.coreSegmentName)
+ .providerSegments().withName(providerSegmentID.providerSegmentName)
+ .entities().all()
+ .snapshots().all();
+ }
+
+ void Builder::allLatestInProviderSegment(const MemoryID& providerSegmentID)
+ {
+ coreSegments().withName(providerSegmentID.coreSegmentName)
+ .providerSegments().withName(providerSegmentID.providerSegmentName)
+ .entities().all()
+ .snapshots().latest();
+ }
+
+ void Builder::allEntitySnapshots(const MemoryID& entityID)
+ {
+ coreSegments().withName(entityID.coreSegmentName)
+ .providerSegments().withName(entityID.providerSegmentName)
+ .entities().withName(entityID.entityName)
+ .snapshots().all();
+ }
+
+ void Builder::latestEntitySnapshot(const MemoryID& entityID)
+ {
+ coreSegments().withName(entityID.coreSegmentName)
+ .providerSegments().withName(entityID.providerSegmentName)
+ .entities().withName(entityID.entityName)
+ .snapshots().latest();
+ }
+
+ void Builder::singleEntitySnapshot(const MemoryID& snapshotID)
+ {
+ coreSegments().withName(snapshotID.coreSegmentName)
+ .providerSegments().withName(snapshotID.providerSegmentName)
+ .entities().withName(snapshotID.entityName)
+ .snapshots().atTime(snapshotID.timestamp);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 1741fbdb60d46aecacd217f07da414e2f3b7538d..7f4c2b7c63440f8182d2e489e74ea33ab98e8317 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -41,6 +41,24 @@ namespace armarx::armem::client::query
}
+ // Short hands for common queries
+
+ /// Get all snapshots from all entities in all provider segments in a core segment.
+ void allInCoreSegment(const MemoryID& coreSegmentID);
+ /// Get latest snapshots from all entities in all provider segments in a core segment.
+ void allLatestInCoreSegment(const MemoryID& coreSegmentID);
+
+ /// Get all snapshots from all entities in a provider segment.
+ void allInProviderSegment(const MemoryID& providerSegmentID);
+ /// Get latest snapshots from all entities in a provider segment.
+ void allLatestInProviderSegment(const MemoryID& providerSegmentID);
+
+ void allEntitySnapshots(const MemoryID& entityID);
+ void latestEntitySnapshot(const MemoryID& entityID);
+
+ void singleEntitySnapshot(const MemoryID& snapshotID);
+
+
QueryInput buildQueryInput() const;
armem::query::data::Input buildQueryInputIce() const;
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index ea04bb93945341c9dd8b433a7004a478f50581be..5c1946ed0f55591457c7ce7fcf7a8a5b9a325ee4 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -75,4 +75,15 @@ namespace armarx::armem
}, results);
}
+
+ EntityUpdate& Commit::add()
+ {
+ return updates.emplace_back();
+ }
+
+ EntityUpdate& Commit::add(const EntityUpdate& update)
+ {
+ return updates.emplace_back(update);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index ff3215ab7ea6d01de892eec9f99ec911e6e667b8..48707600ab242ad3a624c6e26965acdef18839fa 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -85,6 +85,9 @@ namespace armarx::armem
*/
std::vector<EntityUpdate> updates;
+ EntityUpdate& add();
+ EntityUpdate& add(const EntityUpdate& update);
+
friend std::ostream& operator<<(std::ostream& os, const Commit& rhs);
};
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index f5e6d3e0d10cb7b3f1ed51bcc8f665cb48ec3cbf..ed2197e66fc41d424cc793980a771684e12d3de1 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -20,3 +20,10 @@ void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
dto.timestamp = bo.timestamp.toMicroSeconds();
dto.instanceIndex = bo.instanceIndex;
}
+
+std::ostream& armarx::armem::arondto::operator<<(std::ostream& os, const MemoryID& rhs)
+{
+ armem::MemoryID bo;
+ fromAron(rhs, bo);
+ return os << bo;
+}
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
index 5d105fd9761f3ff1d8eaa58fabf4aa257719478b..8688c3b3fdf5dcb2ce9684960ab3a98449c8c13a 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -9,3 +9,7 @@ namespace armarx::armem
void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
void toAron(arondto::MemoryID& dto, const MemoryID& bo);
}
+namespace armarx::armem::arondto
+{
+ std::ostream& operator<<(std::ostream& os, const MemoryID& rhs);
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 6ea66c74b5216c9e4a92c4e719e01193e39b5ae1..ead811e5a744b9208a30eb7761c381b040cd7e90 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -140,7 +140,16 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Add the missing provider segment (with this core segment's type).
+ ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
+ return provSeg.update(update);
+ }
+ else
+ {
+ throw armem::error::MissingEntry("provider segment", update.entityID.providerSegmentName, getLevelName(), this->getKeyString());
+ }
}
}
@@ -230,13 +239,20 @@ namespace armarx::armem::base
{
Base::_copySelf(other);
other.aronType() = _aronType;
+ other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
}
virtual void _copySelfEmpty(DerivedT& other) const override
{
Base::_copySelfEmpty(other);
other.aronType() = _aronType;
+ other._addMissingProviderSegmentDuringUpdate = _addMissingProviderSegmentDuringUpdate;
}
+
+ private:
+
+ bool _addMissingProviderSegmentDuringUpdate = true;
+
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 53b398a284bc8ebb1151d5266f81ec7a43010cea..713ba580089443b9fc2ca6f982e72970aa28dde7 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -108,7 +108,7 @@ namespace armarx::armem::base
this->_checkContainerName(id.memoryName, this->name());
if (id.hasCoreSegmentName())
{
- return getCoreSegment(id.providerSegmentName).findEntity(id);
+ return getCoreSegment(id.coreSegmentName).findEntity(id);
}
else
{
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
index 986612d1e788bee74e572f20b89dbc78fb99ea58..ede63a494a7aff26c54ba1d7149200c91be7d987 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
@@ -19,62 +19,82 @@ namespace armarx::armem::wm
bool Visitor::applyTo(Memory& memory)
{
+ bool cont = true;
+ visitEnter(memory);
for (auto& [_, coreSeg] : memory)
{
if (!applyTo(coreSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(memory);
+ return cont;
}
bool Visitor::applyTo(CoreSegment& coreSegment)
{
+ bool cont = true;
+ visitEnter(coreSegment);
for (auto& [_, provSeg] : coreSegment)
{
if (!applyTo(provSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(coreSegment);
+ return cont;
}
bool Visitor::applyTo(ProviderSegment& providerSegment)
{
+ bool cont = true;
+ visitEnter(providerSegment);
for (auto& [_, entity] : providerSegment)
{
if (!applyTo(entity))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(providerSegment);
+ return cont;
}
bool Visitor::applyTo(Entity& entity)
{
+ bool cont = true;
+ visitEnter(entity);
for (auto& [_, snapshot] : entity)
{
if (!applyTo(snapshot))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(entity);
+ return cont;
}
bool Visitor::applyTo(EntitySnapshot& snapshot)
{
+ bool cont = true;
+ visitEnter(snapshot);
for (auto& instance : snapshot)
{
if (!applyTo(instance))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(snapshot);
+ return cont;
}
bool Visitor::applyTo(EntityInstance& instance)
@@ -85,62 +105,82 @@ namespace armarx::armem::wm
bool Visitor::applyTo(const Memory& memory)
{
+ bool cont = true;
+ visitEnter(memory);
for (const auto& [_, coreSeg] : memory)
{
if (!applyTo(coreSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(memory);
+ return cont;
}
bool Visitor::applyTo(const CoreSegment& coreSegment)
{
+ bool cont = true;
+ visitEnter(coreSegment);
for (const auto& [_, provSeg] : coreSegment)
{
if (!applyTo(provSeg))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(coreSegment);
+ return cont;
}
bool Visitor::applyTo(const ProviderSegment& providerSegment)
{
+ bool cont = true;
+ visitEnter(providerSegment);
for (const auto& [_, entity] : providerSegment)
{
if (!applyTo(entity))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(providerSegment);
+ return cont;
}
bool Visitor::applyTo(const Entity& entity)
{
+ bool cont = true;
+ visitEnter(entity);
for (const auto& [_, snapshot] : entity)
{
if (!applyTo(snapshot))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(entity);
+ return cont;
}
bool Visitor::applyTo(const EntitySnapshot& snapshot)
{
+ bool cont = true;
+ visitEnter(snapshot);
for (const auto& instance : snapshot)
{
if (!applyTo(instance))
{
- return false;
+ cont = false;
+ break;
}
}
- return true;
+ visitExit(snapshot);
+ return cont;
}
bool Visitor::applyTo(const EntityInstance& instance)
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 6895cf4e5b27ecec5872a38890d8111b41405fa7..c1ae33fbe624fe2558c8f2dfb11d2ae380fb192a 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -99,9 +99,30 @@ namespace armarx::armem::server
MemoryToIceAdapter::commit(const data::Commit& commitIce, Time timeArrived)
{
ARMARX_CHECK_NOT_NULL(workingMemory);
+ auto handleException = [](const std::string & what)
+ {
+ data::CommitResult result;
+ data::EntityUpdateResult& r = result.results.emplace_back();
+ r.success = false;
+ r.errorMessage = what;
+ return result;
+ };
armem::Commit commit;
- armem::fromIce(commitIce, commit, timeArrived);
+ try
+ {
+ armem::fromIce(commitIce, commit, timeArrived);
+ }
+ catch (const aron::error::AronNotValidException& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
+ catch (const Ice::Exception& e)
+ {
+ throw;
+ return handleException(e.what());
+ }
armem::CommitResult result = this->commit(commit);
data::CommitResult resultIce;
@@ -147,6 +168,16 @@ namespace armarx::armem::server
result.success = false;
result.errorMessage = e.what();
}
+ catch (const aron::error::AronException& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
+ catch (const Ice::Exception& e)
+ {
+ result.success = false;
+ result.errorMessage = e.what();
+ }
}
if (publishUpdates)
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index a410033a4dcdda6b0e17f0de7ab35e4f9a044f12..4dc890e600b5419db408d3110455a19fd96f2e68 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -34,7 +34,7 @@ namespace armarx::armem::gui::memory
header()->setMinimumSectionSize(25);
header()->resizeSection(int(Columns::KEY), 250);
- header()->resizeSection(int(Columns::SIZE), 25);
+ header()->resizeSection(int(Columns::SIZE), 30);
header()->setTextElideMode(Qt::TextElideMode::ElideRight);
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 859f3d601024ba97dd0e59e938afaba2718a95a1..39ee8bccc7bc774bd0ec200819578307983c9278 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -25,3 +25,11 @@ void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPo
}
+armarx::armem::MemoryID
+armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
+{
+ return MemoryID("Object/Instance")
+ .withProviderSegmentName(objectPose.providerName)
+ .withEntityName(objectPose.objectID.str())
+ .withTimestamp(objectPose.timestamp);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 7ea733e99c75895ff7aba463f36a0f53d0c4bd38..feeed6834096a92af81e5904d291cc7dc49b4b05 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -12,4 +12,14 @@ namespace armarx::armem
void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+
+}
+
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::obj
+{
+ /// Make a Memory ID for the object instance snapshot representing this pose.
+ MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
}
diff --git a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
index c4a4e407fd7ce3aca8cdca5aacc991f05645023f..c7527c2cfc3e862f45bf5f69a039b27c96eb5d50 100644
--- a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
@@ -5,6 +5,7 @@ The ARON DTO of simox::OrientedBoxf.
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
</CodeIncludes>
<GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
index 55a30cff91afbd74bbf4cbae769df34992843096..e99a7f91658f88611d9884d9795fb8e36b2a8485 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
@@ -41,6 +41,33 @@ namespace armarx::aron::visitor
return true;
}
+ template <class ToT, class FromT>
+ ToT cast(const FromT& from)
+ {
+ ToT to;
+ to.setValue(from.getValue());
+ return to;
+ }
+ template <class FromDataT>
+ std::optional<datanavigator::LongNavigator> castToLong(const FromDataT& data)
+ {
+ if (auto d = dynamic_cast<const datanavigator::IntNavigator*>(&data))
+ {
+ return cast<datanavigator::LongNavigator>(*d);
+ }
+ return std::nullopt;
+ }
+ template <class FromDataT>
+ std::optional<datanavigator::IntNavigator> castToInt(const FromDataT& data)
+ {
+ if (auto d = dynamic_cast<const datanavigator::LongNavigator*>(&data))
+ {
+ return cast<datanavigator::IntNavigator>(*d);
+ }
+ return std::nullopt;
+ }
+
+
bool TypedDataVisitor::applyTo(const std::string& key, TypeNavigator& type, DataNavigator& data)
{
if (auto t = dynamic_cast<ObjectTypeNavigator*>(&type))
@@ -63,70 +90,103 @@ namespace armarx::aron::visitor
ARMARX_CHECK_EQUAL(type.childrenSize(), 0) << simox::meta::get_type_name(type);
ARMARX_CHECK_EQUAL(data.childrenSize(), 0) << simox::meta::get_type_name(data);
- if (auto t = dynamic_cast<BoolTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<BoolDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<DoubleTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<DoubleDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<FloatTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<FloatDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<IntTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<IntDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<LongTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<StringTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ try
{
- return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
- }
- if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<EigenQuaternionTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<PoseTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
- }
- else if (auto t = dynamic_cast<PositionTypeNavigator*>(&type))
- {
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ if (auto t = dynamic_cast<BoolTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<BoolDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<DoubleTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<DoubleDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<FloatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<FloatDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<IntTypeNavigator*>(&type))
+ {
+ if (auto cast = castToInt(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<IntDataNavigator&>(data));
+ }
+ }
+ else if (auto t = dynamic_cast<LongTypeNavigator*>(&type))
+ {
+ if (auto cast = castToLong(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
+ }
+ else if (auto t = dynamic_cast<StringTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ {
+ if (auto cast = castToLong(data))
+ {
+ return visit(*t, key, cast.value());
+ }
+ else
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
+ }
+
+ if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenQuaternionTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<PoseTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<PositionTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OrientationTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+
+ ARMARX_CHECK(false) << "Unhandled AronTypeNavigatorType '" << simox::meta::get_type_name(type) << "'."
+ << "\n(Data: '" << simox::meta::get_type_name(data) << "')";
}
- else if (auto t = dynamic_cast<OrientationTypeNavigator*>(&type))
+ catch (const std::bad_cast& e)
{
- return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ std::stringstream msg;
+ msg << "Got ARON data '" << simox::meta::get_type_name(data) << "'"
+ << " incompatible to ARON type '" << simox::meta::get_type_name(type) << "'"
+ << " (got std::bad_cast: '" << e.what() << "').";
+ throw error::AronException("TypedDataVisitor", __FUNCTION__, msg.str());
}
-
- ARMARX_CHECK(false) << "Unhandled AronTypeNavigatorType '" << simox::meta::get_type_name(type) << "'."
- << "\n(Data: '" << simox::meta::get_type_name(data) << "')";
}
bool TypedDataVisitor::applyToChildren(ObjectTypeNavigator& type, DictDataNavigator& data)
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 99495ecee5a9b0ce1402f6fb182eaa63341f80a4..91f15876447e836e6a7e5f883747ae613765a0e5 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -199,7 +199,7 @@ namespace armarx
return this->_robot;
}
- std::string RemoteRobot::getName()
+ std::string RemoteRobot::getName() const
{
return _robot->getName();
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 42970b7e78c7f62fbc7d5ab42ec4a5107a6e6487..b687f0e0fc8cc1eac7b967b10408ec8b63440f56 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -168,7 +168,7 @@ namespace armarx
/// Use this method to share the robot instance over Ice.
SharedRobotInterfacePrx getSharedRobot() const;
- std::string getName() override;
+ std::string getName() const override;
/*!
Creates a local robot that is synchronized once but not updated any more. You can use the synchronizeLocalClone method to update the joint values.