diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index a702857316a46fb59fd581c95e92b8b2413d371e..3bc68392d3f072327b49eeb8c35bc0cae9de7b4a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -7,6 +7,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -32,39 +33,68 @@ namespace armarx::armem::articulated_object
const std::string& instanceName,
VirtualRobot::RobotIO::RobotDescription loadMode)
{
- const auto descriptions = queryDescriptions(timestamp, providerName);
+ ARMARX_INFO << "Getting `" << typeName + "/" + instanceName << "`";
- ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ const std::optional<ArticulatedObject> articulatedObjectDescription =
+ get(typeName + "/" + instanceName, timestamp, providerName);
+ ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
+ << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
- const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
- { return desc.name == typeName; });
-
- if (it == descriptions.end())
- {
- ARMARX_WARNING << "Description for articulate object with type <" << typeName
- << "> not (yet) available!";
- return nullptr;
- }
-
- ARMARX_DEBUG << "Description for articulate object with type <" << typeName
- << "> available!";
-
- auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+ auto obj = VirtualRobot::RobotIO::loadRobot(
+ articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
if (not obj)
{
- ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
- << ">!";
+ ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
+ << instanceName << "` from file `"
+ << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
+
return nullptr;
}
obj->setName(instanceName);
- obj->setType(it->name);
+ obj->setType(typeName);
return obj;
+
+
+ // const auto descriptions = queryDescriptions(timestamp, providerName);
+
+ // ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ // for (const auto& desc : descriptions)
+ // {
+ // ARMARX_INFO << "- " << desc.name;
+ // }
+
+ // const auto it = std::find_if(
+ // descriptions.begin(),
+ // descriptions.end(),
+ // [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
+ // { return desc.name == typeName; });
+
+ // if (it == descriptions.end())
+ // {
+ // ARMARX_WARNING << "Description for articulate object with type <" << typeName
+ // << "> not (yet) available!";
+ // return nullptr;
+ // }
+
+ // ARMARX_DEBUG << "Description for articulate object with type <" << typeName
+ // << "> available!";
+
+ // auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+
+ // if (not obj)
+ // {
+ // ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
+ // << ">!";
+ // return nullptr;
+ // }
+
+ // obj->setName(instanceName);
+ // obj->setType(it->name);
+
+ // return obj;
}
bool
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index e0ce311486d06c5dc859422976f4c96aa9d037dc..dfb2e57fee2806c177da25e31a9fdbfeab12e232 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -171,6 +171,7 @@ namespace armarx::armem::articulated_object
if (providerName.has_value()) // query single provider
{
+ ARMARX_VERBOSE << "Single provider query";
ARMARX_CHECK_NOT_EMPTY(providerName.value());
// clang-format off
@@ -183,6 +184,8 @@ namespace armarx::armem::articulated_object
}
else // query all providers
{
+ ARMARX_VERBOSE << "All provider query";
+
// clang-format off
qb
.coreSegments().withName(objects::constants::CoreClassSegmentName)
@@ -192,7 +195,9 @@ namespace armarx::armem::articulated_object
// clang-format on
}
+ ARMARX_VERBOSE << "Before query";
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ ARMARX_VERBOSE << "After query";
ARMARX_DEBUG << "Lookup result in reader: " << qResult;
@@ -201,6 +206,8 @@ namespace armarx::armem::articulated_object
return {};
}
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "getRobotDescriptions";
return getRobotDescriptions(qResult.memory);
}
@@ -380,20 +387,18 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(objects::constants::CoreClassSegmentName);
std::vector<robot_state::description::RobotDescription> descriptions;
- coreSegment.forEachEntity(
- [&descriptions](const wm::Entity& entity)
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
{
- if (not entity.empty())
+ ARMARX_INFO << "Converting ...";
+ if (const auto robotDescription = convertRobotDescription(instance))
{
- const auto robotDescription =
- convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
- if (robotDescription)
- {
- descriptions.push_back(*robotDescription);
- }
+ descriptions.push_back(*robotDescription);
}
});
+ ARMARX_INFO << descriptions.size() << " descriptions";
+
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index d061c692e3399b07279a6cd5420c295396d34e38..6ec49b479ccccca948d46230e900efb02da5c6ff 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -218,7 +218,7 @@ namespace armarx::armem::articulated_object
arondto::ObjectInstance objectInstance;
toAron(objectInstance, obj.config);
- const auto classId = storeOrGetClass(obj);
+ const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
if (not classId)
{
@@ -229,10 +229,16 @@ namespace armarx::armem::articulated_object
// install memory link
toAron(objectInstance.classID, *classId);
+ // set object instance id
+ const MemoryID memoryInstanceId = classId->withEntityName(entityName);
+
armem::MemoryID id;
- id.setEntityID(classId->getEntityID());
+ id.setEntityID(memoryInstanceId.getEntityID());
armarx::ObjectID objectId(id.entityName);
+ ARMARX_INFO << "Object ID: " << objectId;
+
+ ARMARX_CHECK_NOT_EMPTY(objectId.instanceName()) << "An object instance name must be provided!";
armarx::arondto::ObjectID cs;
cs.className = objectId.className();