From 76e93676f3e9425a6d06bc0622a0558081ae65b9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Sat, 23 Nov 2024 16:53:08 +0100
Subject: [PATCH] fix and improvement of articulated object reader
---
.../ArticulatedObjectReader.cpp | 74 +++++++++++++------
.../client/articulated_object/Reader.cpp | 23 +++---
.../client/articulated_object/Writer.cpp | 10 ++-
3 files changed, 74 insertions(+), 33 deletions(-)
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 a70285731..3bc68392d 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 e0ce31148..dfb2e57fe 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 d061c692e..6ec49b479 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();
--
GitLab