diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp index 4dbe9f32088c95623000bc40300d91454397aa7f..05bb57827346342762bf482f906d6a6e10b83d5a 100644 --- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp +++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp @@ -166,6 +166,10 @@ namespace armarx queryExampleData(); } if (true) + { + commitExamplesWithIDs(); + } + if (true) { commitExamplesWithLinks(); } @@ -363,6 +367,14 @@ namespace armarx } exampleDataProviderID = armem::MemoryID(addSegmentResult.segmentID); + addSegmentResult = memoryWriter.addSegment("LinkedData", getName()); + if (!addSegmentResult.success) + { + ARMARX_ERROR << addSegmentResult.errorMessage; + return; + } + linkedDataProviderID = armem::MemoryID(addSegmentResult.segmentID); + const armem::Time time = armem::Time::Now(); armem::Commit commit; @@ -373,7 +385,8 @@ namespace armarx update.timeCreated = time; armem::example::ExampleData data; - toAron(data.memoryLink, armem::MemoryID()); + toAron(data.memoryID, armem::MemoryID()); + toAron(data.memoryLink.memoryID, armem::MemoryID()); ARMARX_CHECK_NOT_NULL(data.toAron()); update.instancesData = { data.toAron() }; } @@ -417,7 +430,8 @@ namespace armarx data.the_3x1_vector = { 24, 42, 2442 }; data.the_4x4_matrix = 42 * Eigen::Matrix4f::Identity(); - toAron(data.memoryLink, armem::MemoryID()); // ////1/1 + toAron(data.memoryID, armem::MemoryID()); // ////1/1 + toAron(data.memoryLink.memoryID, armem::MemoryID()); simox::ColorMap cmap = simox::color::cmaps::plasma(); { @@ -465,10 +479,27 @@ namespace armarx update.instancesData = { data.toAron() }; } + // commit to linked data + { + armem::EntityUpdate& update = commit.add(); + update.entityID = linkedDataProviderID.withEntityName("yet_more_data"); + update.timeCreated = time; + + armem::example::LinkedData data; + data.yet_another_int = 42; + data.yet_another_string = "Hi! I'm from another core segment!"; + data.yet_another_object.element_int = 8349; + data.yet_another_object.element_float = -1e3; + data.yet_another_object.element_string = "I'm a nested object in some linked data."; + ARMARX_CHECK_NOT_NULL(data.toAron()); + update.instancesData = { data.toAron() }; + } + armem::CommitResult commitResult = memoryWriter.commit(commit); if (commitResult.allSuccess()) { theAnswerSnapshotID = commitResult.results.at(1).snapshotID; + yetMoreDataSnapshotID = commitResult.results.at(2).snapshotID; } else { @@ -500,7 +531,7 @@ namespace armarx } - void ExampleMemoryClient::commitExamplesWithLinks() + void ExampleMemoryClient::commitExamplesWithIDs() { ARMARX_IMPORTANT << "Committing multiple entity updates with links ..."; const armem::Time time = armem::Time::Now(); @@ -508,42 +539,46 @@ namespace armarx armem::Commit commit; { armem::EntityUpdate& update = commit.add(); - update.entityID = exampleDataProviderID.withEntityName("link to the_answer"); + update.entityID = exampleDataProviderID.withEntityName("id to the_answer"); update.timeCreated = time; armem::example::ExampleData data; - armem::toAron(data.memoryLink, theAnswerSnapshotID); + armem::toAron(data.memoryID, theAnswerSnapshotID); + armem::toAron(data.memoryLink.memoryID, armem::MemoryID()); update.instancesData = { data.toAron() }; } { armem::EntityUpdate& update = commit.add(); - update.entityID = exampleDataProviderID.withEntityName("link to self"); + update.entityID = exampleDataProviderID.withEntityName("id to self"); update.timeCreated = time; armem::example::ExampleData data; - armem::toAron(data.memoryLink, update.entityID.withTimestamp(time)); + armem::toAron(data.memoryID, update.entityID.withTimestamp(time)); + armem::toAron(data.memoryLink.memoryID, armem::MemoryID()); update.instancesData = { data.toAron() }; } { armem::EntityUpdate& update = commit.add(); - update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot"); + update.entityID = exampleDataProviderID.withEntityName("id to previous snapshot"); update.timeCreated = time - armem::Duration::Seconds(1); // 1 sec in the past armem::example::ExampleData data; - armem::toAron(data.memoryLink, armem::MemoryID()); // First entry - invalid link + armem::toAron(data.memoryID, armem::MemoryID()); // First entry - invalid link + armem::toAron(data.memoryLink.memoryID, armem::MemoryID()); update.instancesData = { data.toAron() }; } { armem::EntityUpdate& update = commit.add(); - update.entityID = exampleDataProviderID.withEntityName("link to previous snapshot"); + update.entityID = exampleDataProviderID.withEntityName("id to previous snapshot"); update.timeCreated = time; armem::example::ExampleData data; - armem::toAron(data.memoryLink, update.entityID.withTimestamp(time - armem::Duration::Seconds(1))); + armem::toAron(data.memoryID, update.entityID.withTimestamp(time - armem::Duration::Seconds(1))); + armem::toAron(data.memoryLink.memoryID, armem::MemoryID()); update.instancesData = { data.toAron() }; } @@ -583,6 +618,61 @@ namespace armarx } } + + void ExampleMemoryClient::commitExamplesWithLinks() + { + ARMARX_IMPORTANT << "Committing an entity update with a link..."; + + const armem::Time time = armem::Time::Now(); + + armem::Commit commit; + { + armem::EntityUpdate& update = commit.add(); + update.entityID = exampleDataProviderID.withEntityName("link to yet_more_data"); + update.timeCreated = time; + + armem::example::ExampleData data; + armem::toAron(data.memoryID, armem::MemoryID()); + armem::toAron(data.memoryLink.memoryID, yetMoreDataSnapshotID); + + update.instancesData = { data.toAron() }; + } + + ARMARX_CHECK_EQUAL(commit.updates.size(), 1); + armem::CommitResult commitResult = memoryWriter.commit(commit); + + if (!commitResult.allSuccess() || commitResult.results.size() != commit.updates.size()) + { + ARMARX_WARNING << commitResult.allErrorMessages(); + } + + + // Resolve memory IDs via memory name system (works for IDs from different servers). + ARMARX_IMPORTANT << "Resolving multiple memory IDs via Memory Name System:"; + { + std::vector<armem::MemoryID> ids; + for (armem::EntityUpdateResult& result : commitResult.results) + { + ids.push_back(result.snapshotID); + } + ARMARX_CHECK_EQUAL(ids.size(), commit.updates.size()); + + std::map<armem::MemoryID, armem::wm::EntityInstance> instances = + memoryNameSystem().resolveEntityInstances(ids); + ARMARX_CHECK_EQUAL(instances.size(), commit.updates.size()); + + std::stringstream ss; + for (const auto& [id, instance]: instances) + { + ss << "- Snapshot " << id << " " + << "\n--> Instance" << instance.id() + << " (# keys in data: " << instance.data()->childrenSize() << ")" + << "\n"; + } + ARMARX_INFO << ss.str(); + } + } + void ExampleMemoryClient::commitExampleImages() { const armem::Time time = armem::Time::Now(); @@ -616,7 +706,8 @@ namespace armarx update.timeCreated = time; armem::example::ExampleData data; - toAron(data.memoryLink, armem::MemoryID()); // ////1/1 + toAron(data.memoryID, armem::MemoryID()); // ////1/1 + toAron(data.memoryLink.memoryID, armem::MemoryID()); aron::data::DictPtr aron = data.toAron(); aron->addElement("unexpectedString", std::make_shared<aron::data::String>("unexpected value")); diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h index 0674aff6db1daa7421f6e4f83033618a09ece4e5..dcbcad65cb019296032664ee1c12fc14aee1d56e 100644 --- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h +++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h @@ -98,6 +98,7 @@ namespace armarx void commitExampleData(); void queryExampleData(); + void commitExamplesWithIDs(); void commitExamplesWithLinks(); void commitExampleImages(); @@ -122,6 +123,8 @@ namespace armarx armem::MemoryID exampleDataProviderID; armem::MemoryID theAnswerSnapshotID; + armem::MemoryID linkedDataProviderID; + armem::MemoryID yetMoreDataSnapshotID; armem::Time runStarted; diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp index 16c6b64c048d1614858f1cd7bdd57a0a99f63287..7a7d65f0740024f7ef1d7b43c667c2971962429b 100644 --- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp +++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp @@ -64,6 +64,7 @@ namespace armarx { // Usually, the memory server will specify a number of core segments with a specific aron type. workingMemory().addCoreSegment("ExampleData", armem::example::ExampleData::ToAronType()); + workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType()); // For illustration purposes, we add more segments (without types). bool trim = true; diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml index be9a9788fae8045e98d211d6a2b7397d6da455eb..4efce411b680ce9a91afceff7da5523b8f67d6cc 100644 --- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml +++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml @@ -14,6 +14,7 @@ An example data containing different native ARON types. <AronIncludes> <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" /> + <Include include="<RobotAPI/libraries/armem/aron/MemoryLink.xml>" autoinclude="true" /> </AronIncludes> <GenerateTypes> @@ -29,6 +30,21 @@ An example data containing different native ARON types. </ObjectChild> </Object> + <Object name='armarx::armem::example::LinkedData'> + + <ObjectChild key='yet_another_int'> + <Int /> + </ObjectChild> + <ObjectChild key='yet_another_string'> + <String /> + </ObjectChild> + + <ObjectChild key='yet_another_object'> + <armarx::armem::example::InnerClass /> + </ObjectChild> + + </Object> + <Object name='armarx::armem::example::ExampleData'> <ObjectChild key='the_int'> @@ -43,9 +59,9 @@ An example data containing different native ARON types. <ObjectChild key='the_double'> <Double /> </ObjectChild> - <ObjectChild key='the_string'> - <String /> - </ObjectChild> + <ObjectChild key='the_string'> + <String /> + </ObjectChild> <ObjectChild key='the_bool'> <Bool /> </ObjectChild> @@ -106,11 +122,16 @@ An example data containing different native ARON types. </Dict> </ObjectChild> - <ObjectChild key="memoryLink"> + <ObjectChild key="memoryID"> <armarx::armem::arondto::MemoryID /> </ObjectChild> - </Object> + <ObjectChild key="memoryLink"> + <armarx::armem::arondto::MemoryLink template="armarx::armem::example::LinkedData"/> + </ObjectChild> + + </Object> + </GenerateTypes> </AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt index afd231ffbbbf219b044331dc21f672fbac0274b1..6b105d8f67ed0ba87bfd5ae17a2fcf54fea79b70 100644 --- a/source/RobotAPI/libraries/armem/CMakeLists.txt +++ b/source/RobotAPI/libraries/armem/CMakeLists.txt @@ -168,6 +168,7 @@ armarx_add_library( "${LIBS}" ARON_FILES aron/MemoryID.xml + aron/MemoryLink.xml ) diff --git a/source/RobotAPI/libraries/armem/aron/MemoryLink.xml b/source/RobotAPI/libraries/armem/aron/MemoryLink.xml new file mode 100644 index 0000000000000000000000000000000000000000..3f04e393abf0c07747cdebd8288ca655f169e7c5 --- /dev/null +++ b/source/RobotAPI/libraries/armem/aron/MemoryLink.xml @@ -0,0 +1,19 @@ +<?xml version="1.0" encoding="UTF-8" ?> +<AronTypeDefinition> + <CodeIncludes> + <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" /> + </CodeIncludes> + <AronIncludes> + <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" /> + </AronIncludes> + <GenerateTypes> + <Object name="armarx::armem::arondto::MemoryLink" template="T"> + <ObjectChild key="memoryID"> + <armarx::armem::arondto::MemoryID /> + </ObjectChild> + <ObjectChild key="data"> + <T unique_ptr="true"/> + </ObjectChild> + </Object> + </GenerateTypes> +</AronTypeDefinition> diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp index b7a1c85dd5925353a360058cc5359a312f6e42c0..5bf6dcd0308cfad16c6401bed4c2ae940a972930 100644 --- a/source/RobotAPI/libraries/armem/client/Reader.cpp +++ b/source/RobotAPI/libraries/armem/client/Reader.cpp @@ -4,10 +4,14 @@ #include <ArmarXCore/core/logging/Logging.h> +#include <RobotAPI/libraries/armem/aron/MemoryLink.aron.generated.h> #include <RobotAPI/libraries/armem/core/MemoryID_operators.h> -#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h> +#include <RobotAPI/libraries/armem/core/aron_conversions.h> #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h> +#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h> #include <RobotAPI/libraries/armem/util/util.h> +#include <RobotAPI/libraries/aron/common/util/object_finders.h> +#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h> #include "query/Builder.h" #include "query/query_fns.h" @@ -81,6 +85,212 @@ namespace armarx::armem::client } + QueryResult Reader::query(const QueryInput& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth) const + { + return QueryResult::fromIce(query(input.toIce(), mns, recursionDepth)); + } + + + armem::query::data::Result Reader::query(const armem::query::data::Input& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth) const + { + auto makeErrorMsg = [](auto error) + { + std::stringstream sstream; + sstream << "Memory query failed.\nReason: " << error.what(); + return sstream.str(); + }; + + armem::query::data::Result result; + ARMARX_CHECK_NOT_NULL(memoryPrx); + try + { + result = memoryPrx->query(input); + QueryResult bObj = QueryResult::fromIce(result); + bObj.memory.forEachInstance( + [&bObj, &mns, recursionDepth](armem::wm::EntityInstance& instance) + { resolveMemoryLinks(instance, bObj.memory, mns, recursionDepth); }); + result = bObj.toIce(); + } + catch (const Ice::ConnectionRefusedException& e) + { + result.errorMessage = makeErrorMsg(e); + } + catch (const Ice::ConnectionLostException& e) + { + result.errorMessage = makeErrorMsg(e); + } + catch (const Ice::NotRegisteredException& e) + { + result.errorMessage = makeErrorMsg(e); + } + catch (const exceptions::local::ExpressionException& e) + { + std::stringstream sstream; + sstream << "Encountered malformed MemoryLink: " << e.what(); + result.errorMessage = sstream.str(); + } + + return result; + } + + + QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, // NOLINT + armem::client::MemoryNameSystem& mns, + int recursionDepth) const + { + return this->query(armem::query::data::MemoryQuerySeq{query}, mns, recursionDepth); + } + + + QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries, + armem::client::MemoryNameSystem& mns, + int recursionDepth) const + { + QueryInput input; + input.memoryQueries = queries; + return this->query(input, mns, recursionDepth); + } + + + QueryResult Reader::query(const QueryBuilder& queryBuilder, + armem::client::MemoryNameSystem& mns, + int recursionDepth) const + { + return this->query(queryBuilder.buildQueryInput(), mns, recursionDepth); + } + + + /** + * Get the MemoryID and data required to fill in the given MemoryLink. + * + * Returns nothing if the data could not be retrieved or its type does not + * match the MemoryLink's template type. + * + * @param linkData the data object of the MemoryLink + * @param linkType the type object of the MemoryLink + * @return a pair of memoryID and data to fill in a MemoryLink, or nothing if not available + */ + std::optional<std::pair<MemoryID, aron::data::VariantPtr>> + findDataForLink(const aron::data::VariantPtr& linkData, + const aron::type::VariantPtr& linkType, + armem::client::MemoryNameSystem& mns) + { + static const aron::Path memoryLinkIDPath{{"memoryID"}}; + static const aron::Path memoryLinkDataPath{{"data"}}; + armem::arondto::MemoryID dto; + auto memoryIDDict = + aron::data::Dict::DynamicCastAndCheck(linkData->navigateAbsolute(memoryLinkIDPath)); + dto.fromAron(memoryIDDict); + auto memoryID = aron::fromAron<armem::MemoryID>(dto); + if (!memoryID.isWellDefined() || !memoryID.hasEntityName()) + { + ARMARX_INFO << "Encountered unresolvable MemoryID '" << memoryID.str() + << "', ignoring."; + return {}; + } + + auto nextMemory = resolveID(mns, memoryID); + if (!nextMemory) + { + ARMARX_WARNING << "Could not retrieve data for " << memoryID.str() << ", skipping..."; + return {}; + } + auto nextDataAndType = extractDataAndType(nextMemory.value(), memoryID); + if (!nextDataAndType) + { + ARMARX_WARNING << "Data or type for " << memoryID.str() + << " not available in memory containing that MemoryID."; + return {}; + } + auto [nextData, nextType] = nextDataAndType.value(); + auto linkTemplate = + aron::type::Object::DynamicCastAndCheck(linkType)->getTemplateInstantiations().at(0); + std::string nextObjectName = nextType->getObjectNameWithTemplateInstantiations(); + if (nextType->getObjectName() != linkTemplate) + { + ARMARX_WARNING << "Linked object " << memoryID.str() << " is of the type " + << nextType->getObjectName() << ", but the link requires the type " + << linkTemplate; + return {}; + } + return {{memoryID, + // This is currently the only method to prefix the path to the data correctly. + aron::data::Variant::FromAronDTO( + nextData->toAronDTO(), + linkData->getPath().withElement(memoryLinkDataPath.toString()))}}; + } + + void + Reader::resolveMemoryLinks(armem::wm::EntityInstance& instance, + armem::wm::Memory& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth) + { + static const aron::Path memoryLinkDataPath{{"data"}}; + // MemoryID is used for the template instantiation as a placeholder - + // you could use any type here. + static const std::string linkType = + arondto::MemoryLink<arondto::MemoryID>::ToAronType()->getFullName(); + + std::set<armem::MemoryID> seenIDs{instance.id()}; + std::vector<aron::Path> currentPaths{{{""}}}; + std::vector<aron::Path> nextPaths; + int depth = 0; + + auto dataAndType = extractDataAndType(input, instance.id()); + if (!dataAndType) + { + ARMARX_INFO << "Instance '" << instance.id().str() << "' does not have a defined type."; + return; + } + auto [instanceData, instanceType] = dataAndType.value(); + while (!currentPaths.empty() && (recursionDepth == -1 || depth < recursionDepth)) + { + for (const auto& path : currentPaths) + { + aron::SubObjectFinder finder(linkType); + if (path.getFirstElement().empty()) + { + armarx::aron::data::visitRecursive(finder, instanceData, instanceType); + } + else + { + armarx::aron::data::visitRecursive(finder, + instanceData->navigateAbsolute(path), + instanceType->navigateAbsolute(path)); + } + + for (auto& [linkPathStr, linkDataAndType] : finder.getFoundObjects()) // NOLINT + { + auto [linkData, linkType] = linkDataAndType; + auto result = findDataForLink(linkData, linkType, mns); + if (result) + { + auto [memoryID, dataToAppend] = result.value(); + if (seenIDs.find(memoryID) != seenIDs.end()) + { + continue; + } + seenIDs.insert(memoryID); + + aron::data::Dict::DynamicCastAndCheck(linkData)->setElement( + memoryLinkDataPath.toString(), dataToAppend); + nextPaths.push_back( + linkData->getPath().withElement(memoryLinkDataPath.toString())); + } + } + } + currentPaths = std::move(nextPaths); + nextPaths.clear(); + ++depth; + } + } + + QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, DataMode dataMode) const { using namespace client::query_fns; diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h index e9ee1b69afef7d2befdaaf0169b0117a13edbad5..1b957fde328f8c09b1086dc7fa2f0011cecd694b 100644 --- a/source/RobotAPI/libraries/armem/client/Reader.h +++ b/source/RobotAPI/libraries/armem/client/Reader.h @@ -8,6 +8,7 @@ // RobotAPI #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h> #include <RobotAPI/interface/armem/server/StoringMemoryInterface.h> +#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h> #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h> #include "Query.h" @@ -41,9 +42,60 @@ namespace armarx::armem::client QueryResult query(const QueryBuilder& queryBuilder) const; + /** Perform a query with recursive memory link resolution. + * + * Resolves the links in the query result using the given MemoryNameSystem + * and inserts the data into the MemoryLink objects' data fields. + * If the inserted data also contains links, those will be resolved as well + * up to and including the given recursion depth. Link cycles are detected; + * the first repeated memory ID will not be inserted into the data. + * Setting the recursion depth to `-1` is equivalent to an infinite + * recursion depth. + * + * Standalone `MemoryID` subobjects are ignored; only MemoryIDs inside of + * MemoryLink subobjects are resolved. Empty or unresolvable MemoryIDs + * are ignored - this includes MemoryIDs whose Entity fields are not set. + * If the data associated with a MemoryID could not be retrieved or + * its type does not match the template type of the MemoryLink, + * it is ignored. + * + * @param input the query to perform + * @param mns the MemoryNameSystem to use when resolving MemoryLinks + * @param recursionDepth how many layers of MemoryLinks to resolve + */ + QueryResult query(const QueryInput& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth = -1) const; + /** + * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int) + */ + armem::query::data::Result query(const armem::query::data::Input& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth = -1) const; + + /** + * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int) + */ + QueryResult query(armem::query::data::MemoryQueryPtr query, + armem::client::MemoryNameSystem& mns, + int recursionDepth = -1) const; + /** + * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int) + */ + QueryResult query(const armem::query::data::MemoryQuerySeq& queries, + armem::client::MemoryNameSystem& mns, + int recursionDepth = -1) const; + + /** + * @sa Reader::query(const QueryInput&, armem::client::MemoryNameSystem, int) + */ + QueryResult query(const QueryBuilder& queryBuilder, + armem::client::MemoryNameSystem& mns, + int recursionDepth = -1) const; + /** - * @brief Qeury a specific set of memory IDs. + * @brief Query a specific set of memory IDs. * * Each ID can refer to an entity, a snapshot or an instance. When not * referring to an entity instance, the latest snapshot and first @@ -116,10 +168,16 @@ namespace armarx::armem::client return bool(memoryPrx); } + private: + static void resolveMemoryLinks(armem::wm::EntityInstance& instance, + armem::wm::Memory& input, + armem::client::MemoryNameSystem& mns, + int recursionDepth); + public: server::ReadingMemoryInterfacePrx memoryPrx; }; -} +} // namespace armarx::armem::client diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h index dfe5299f7bf37780a034b83f807496c3636728de..d77f34eb719b46c117c64a2e9ec09cd43780dad9 100644 --- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h +++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h @@ -3,6 +3,8 @@ #include <sstream> #include <stack> +#include <ArmarXCore/core/logging/Logging.h> + #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h> #include <RobotAPI/libraries/aron/core/data/variant/All.h> @@ -106,6 +108,12 @@ namespace armarx::armem::server this->addValueLabel(key, *aron::data::NDArray::DynamicCastAndCheck(array), "ND Array"); } + void visitUnknown(const aron::data::VariantPtr& unknown) override + { + ARMARX_IMPORTANT << "Unknown data encountered: " + << (unknown ? unknown->getFullName() : "nullptr"); + } + private: template <class DataT> diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp index 34f0e2f56d9d47ea7db8b54e198e6faa9d0b25d9..fcaab64e05b6eb35e9c516a9637f77a97b210952 100644 --- a/source/RobotAPI/libraries/armem/util/util.cpp +++ b/source/RobotAPI/libraries/armem/util/util.cpp @@ -1,3 +1,68 @@ #include "util.h" -// intentionally left blank \ No newline at end of file +#include <RobotAPI/libraries/armem/client.h> +#include <RobotAPI/libraries/armem/core/error/mns.h> + +namespace armarx::armem +{ + + std::optional<armarx::armem::wm::Memory> + resolveID(armarx::armem::client::MemoryNameSystem& mns, const armarx::armem::MemoryID& memoryID) + { + armarx::armem::client::Reader reader; + try + { + reader = mns.getReader(memoryID); + } + catch (const armarx::armem::error::CouldNotResolveMemoryServer& e) + { + ARMARX_WARNING << armarx::armem::error::CouldNotResolveMemoryServer::makeMsg(memoryID); + return {}; + } + + const armarx::armem::ClientQueryResult result = reader.queryMemoryIDs({memoryID}); + if (result.success) + { + return result.memory; + } + ARMARX_WARNING << "Could not query memory for ID " << memoryID << ", " + << result.errorMessage; + return {}; + } + + + std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>> + extractDataAndType(const armarx::armem::wm::Memory& memory, + const armarx::armem::MemoryID& memoryID) + { + const auto* instance = memory.findLatestInstance(memoryID); + if (instance == nullptr) + { + return {}; + } + + armarx::aron::data::DictPtr aronData = instance->data(); + armarx::aron::type::ObjectPtr aronType; + const auto* providerSegment = memory.findProviderSegment(memoryID); + if (providerSegment == nullptr) + { + return {}; + } + + if (!providerSegment->hasAronType()) + { + const auto* coreSegment = memory.findCoreSegment(memoryID); + if (coreSegment == nullptr || !coreSegment->hasAronType()) + { + return {}; + } + aronType = coreSegment->aronType(); + } + else + { + aronType = providerSegment->aronType(); + } + + return {{aronData, aronType}}; + } +} // namespace armarx::armem diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h index a337bfafb784725dbfdd7cef9e158361a8911c97..7ffdfbfdc4f874ce264d9a61e2ffb030291827d9 100644 --- a/source/RobotAPI/libraries/armem/util/util.h +++ b/source/RobotAPI/libraries/armem/util/util.h @@ -26,6 +26,7 @@ #include <ArmarXCore/core/logging/Logging.h> +#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h> #include <RobotAPI/libraries/armem/core/wm/memory_definitions.h> #include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronGeneratedClass.h> @@ -165,4 +166,37 @@ namespace armarx::armem return outV; } + /** + * @brief resolve a single MemoryID with the given MemoryNameSystem. + * + * Returns a Memory containing only the desired segment / entity if it was + * successfully queried. If the query was unsuccessful (the MNS could not + * resolve the memory server, the MemoryID does not exist, etc.), + * nothing is returned. + * + * @param mns the MemoryNameSystem to use for the query + * @param memoryID the MemoryID to query for + * @return memory containing the object referenced by the MemoryID if available + */ + std::optional<armarx::armem::wm::Memory> + resolveID(armarx::armem::client::MemoryNameSystem& mns, + const armarx::armem::MemoryID& memoryID); + + + /** + * @brief get the data and type of the given MemoryID in the given Memory. + * + * Returns the data and the type of the latest instance corresponding to + * the given MemoryID (whatever `memory.findLatestInstance(memoryID)` returns). + * If no such instance exists in the memory or no type for the instance is available, + * nothing is returned. + * + * @param memory the Memory to extract the data and type from + * @param memoryID the MemoryID of the instance to extract + * @return pair of data and type for the instance if available + */ + std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>> + extractDataAndType(const armarx::armem::wm::Memory& memory, + const armarx::armem::MemoryID& memoryID); + } // namespace armarx::armem diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp index 41a66a012763c3880498734b068361d08ddb38b8..849f6b2924f2d2c74ba1a75d49cd55ae4c647de7 100644 --- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp +++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp @@ -357,24 +357,41 @@ namespace armarx::armem::gui void - MemoryViewer::startDueQueries(std::map<std::string, Ice::AsyncResultPtr>& queries, - const std::map<std::string, armem::client::Reader>& readers) + MemoryViewer::startDueQueries( + std::map<std::string, std::future<armem::query::data::Result>>& queries, + const std::map<std::string, armem::client::Reader>& readers) { armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput(); + int recursionDepth = memoryGroup->queryWidget()->queryLinkRecursionDepth(); - for (auto& [name, reader] : readers) + // Can't use a structured binding here because you can't capture those in a lambda + // according to the C++ standard. + for (const auto& pair : readers) { + const auto& name = pair.first; + const auto& reader = pair.second; if (queries.count(name) == 0) { - queries[name] = reader.memoryPrx->begin_query(input.toIce()); + // You could pass the query function itself to async here, + // but that caused severe template headaches when I tried it. + queries[name] = + std::async(std::launch::async, + [&reader, input, recursionDepth, this]() + { + // Can't resolve MemoryLinks without data + return recursionDepth == 0 || input.dataMode == DataMode::NoData + ? reader.query(input.toIce()) + : reader.query(input.toIce(), mns, recursionDepth); + }); } } } std::map<std::string, client::QueryResult> - MemoryViewer::collectQueryResults(std::map<std::string, Ice::AsyncResultPtr>& queries, - const std::map<std::string, client::Reader>& readers) + MemoryViewer::collectQueryResults( + std::map<std::string, std::future<armem::query::data::Result>>& queries, + const std::map<std::string, client::Reader>& readers) { TIMING_START(tCollectQueryResults) @@ -382,16 +399,15 @@ namespace armarx::armem::gui for (auto it = queries.begin(); it != queries.end();) { const std::string& name = it->first; - Ice::AsyncResultPtr& queryPromise = it->second; + std::future<armem::query::data::Result>* queryPromise = &it->second; - if (queryPromise->isCompleted()) + if (queryPromise->wait_for(std::chrono::seconds(0)) == std::future_status::ready) { if (auto jt = memoryReaders.find(name); jt != readers.end()) { try { - results[name] = client::QueryResult::fromIce( - jt->second.memoryPrx->end_query(queryPromise)); + results[name] = client::QueryResult::fromIce(queryPromise->get()); } catch (const Ice::ConnectionRefusedException&) { @@ -401,7 +417,7 @@ namespace armarx::armem::gui // else: Server is gone (MNS new about it) => Skip result. // Promise is completed => Clean up in any case. - it = runningQueries.erase(it); + it = queries.erase(it); } else { diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h index 51cda4cb797911c3afcd1afaf43657b145b9a140..9b0d2a8fff364cd2f702c11a1769243b435ac0ce 100644 --- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h +++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h @@ -1,5 +1,6 @@ #pragma once +#include <future> #include <string> #include <QObject> @@ -120,12 +121,12 @@ namespace armarx::armem::gui /// Start a query for each entry in `memoryReaders` which is not in `runningQueries`. void startDueQueries( - std::map<std::string, Ice::AsyncResultPtr>& queries, + std::map<std::string, std::future<armem::query::data::Result>>& queries, const std::map<std::string, armem::client::Reader>& readers); std::map<std::string, client::QueryResult> collectQueryResults( - std::map<std::string, Ice::AsyncResultPtr>& queries, + std::map<std::string, std::future<armem::query::data::Result>>& queries, const std::map<std::string, armem::client::Reader>& readers); void applyQueryResults( @@ -142,7 +143,7 @@ namespace armarx::armem::gui std::map<std::string, armem::client::Writer> memoryWriters; std::map<std::string, armem::wm::Memory> memoryData; - std::map<std::string, Ice::AsyncResultPtr> runningQueries; + std::map<std::string, std::future<armem::query::data::Result>> runningQueries; /// Periodically triggers query result collection. QTimer* processQueryResultTimer; diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp index 5ed578979d2516f3c089685c1bf5bc44843ad157..27cb691cf870d1663305cb25a60f2541dc29e7c6 100644 --- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp +++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp @@ -18,7 +18,7 @@ namespace armarx::armem::gui::instance builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item) { auto child = data->getElement(key); - this->update(item, key, child); + this->update(item, key, child, data->getPath()); return true; }); @@ -27,20 +27,24 @@ namespace armarx::armem::gui::instance void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data) { - auto children = data->getChildren(); ListBuilder builder = getListBuilder(); - builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item) + builder.setUpdateItemFn([this, &data](size_t key, QTreeWidgetItem * item) { - this->update(item, std::to_string(key), children.at(key)); + auto child = data->getElement(key); + this->update(item, std::to_string(key), child, data->getPath()); return true; }); - builder.updateTreeWithContainer(parent, getIndex(children.size())); + builder.updateTreeWithContainer(parent, getIndex(data->childrenSize())); } - void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data) + void + DataTreeBuilder::update(QTreeWidgetItem* item, + const std::string& key, + const aron::data::VariantPtr& data, + const aron::Path& parentPath) { if (data) { @@ -59,8 +63,9 @@ namespace armarx::armem::gui::instance { // Optional data? this->setRowTexts(item, key, "(none)"); + auto empty = std::make_shared<aron::data::Dict>(parentPath.withElement(key)); + updateTree(item, empty); } - } - + } } diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h index 4ae61c3036195e08055582d5e7251857a30ca650..80773c86aa7074b88b962fa2ae44c7b4f54e65f1 100644 --- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h +++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h @@ -22,9 +22,9 @@ namespace armarx::armem::gui::instance protected: - - void update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data); - + void update(QTreeWidgetItem* item, + const std::string& key, + const aron::data::VariantPtr& data, + const aron::Path& parentPath); }; - } diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp index de85c11c08080c88c5f5d72f5b00900547a66fbc..70edd5a5149617618e8ab023a8ccb48f4a629dee 100644 --- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp +++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp @@ -213,31 +213,31 @@ namespace armarx::armem::gui::instance } } - if (data) + // We pass empty containers if data is null so that subitems of the data are deleted. + auto emptyDict = aron::data::Dict(type->getPath()); + auto emptyList = aron::data::List(type->getPath()); + if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type)) { - if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type)) - { - _updateTree(item, *t, *d); - } - else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type)) - { - _updateTree(item, *t, *d); - } - else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type)) - { - _updateTree(item, *t, *d); - } - else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type)) - { - _updateTree(item, *t, *d); - } - else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type)) - { - _updateTree(item, *t, *d); - } - // else??? - // phesch: else we stop here, since there's no container to recurse into. + _updateTree(item, *t, d ? *d : emptyDict); + } + else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type)) + { + _updateTree(item, *t, d ? *d : emptyDict); + } + else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type)) + { + _updateTree(item, *t, d ? *d : emptyList); + } + else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type)) + { + _updateTree(item, *t, d ? *d : emptyList); + } + else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type)) + { + _updateTree(item, *t, d ? *d : emptyList); } + // else??? + // phesch: else we stop here, since there's no container to recurse into. } diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp index 2bdef0d95aa45845d27170508f83aed51a3e7829..82a8cca0b365cbdb367d28cff620864c72c570f6 100644 --- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp +++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp @@ -1,12 +1,14 @@ #include "QueryWidget.h" -#include <QWidget> -#include <QTabWidget> -#include <QPushButton> #include <QCheckBox> +#include <QGroupBox> #include <QHBoxLayout> +#include <QLabel> +#include <QPushButton> +#include <QSpinBox> +#include <QTabWidget> #include <QVBoxLayout> -#include <QGroupBox> +#include <QWidget> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> @@ -23,14 +25,20 @@ namespace armarx::armem::gui _dataCheckBox = new QCheckBox("Get Data"); _dataCheckBox->setChecked(true); - _storeInLTMButton = new QPushButton("Store query result in LTM"); + _recursionDepthSpinner = new QSpinBox(); + _recursionDepthSpinner->setMinimum(-1); + _recursionDepthSpinner->setValue(0); + _recursionDepthSpinner->setEnabled(_dataCheckBox->isChecked()); - //_tabWidget = new QTabWidget(); + _recursionDepthLabel = new QLabel("Link resolution depth"); + + _storeInLTMButton = new QPushButton("Store query result in LTM"); _snapshotSelectorWidget = new SnapshotSelectorWidget(); - //_tabWidget->addTab(_snapshotSelectorWidget, QString("Snapshots")); hlayout1->addWidget(_dataCheckBox); + hlayout1->addWidget(_recursionDepthSpinner); + hlayout1->addWidget(_recursionDepthLabel); hlayout1->addWidget(_storeInLTMButton); hlayout2->addWidget(_snapshotSelectorWidget); @@ -44,6 +52,10 @@ namespace armarx::armem::gui // Public connections. connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM); + // Private connections. + connect( + _dataCheckBox, &QCheckBox::stateChanged, this, &This::setRecursionDepthSpinnerEnabled); + setLayout(vlayout); } @@ -55,6 +67,11 @@ namespace armarx::armem::gui : armem::DataMode::NoData; } + int QueryWidget::queryLinkRecursionDepth() const + { + return _recursionDepthSpinner->value(); + } + armem::client::QueryInput QueryWidget::queryInput() { armem::client::query::Builder qb(dataMode()); @@ -67,4 +84,18 @@ namespace armarx::armem::gui return qb.buildQueryInput(); } + void + QueryWidget::setRecursionDepthSpinnerEnabled(int state) + { + switch (state) + { + case Qt::Checked: + _recursionDepthSpinner->setEnabled(true); + break; + case Qt::Unchecked: + default: + _recursionDepthSpinner->setEnabled(false); + break; + } + } } diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h index 2bc7f223d5db52435b7571970bb4cd398aaeec49..3b950acf05899e1e187273d1a2d7da57def4de1e 100644 --- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h +++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h @@ -23,6 +23,8 @@ namespace armarx::armem::gui armem::DataMode dataMode() const; + int queryLinkRecursionDepth() const; + armem::client::QueryInput queryInput(); @@ -36,6 +38,7 @@ namespace armarx::armem::gui private slots: + void setRecursionDepthSpinnerEnabled(int state); signals: @@ -44,10 +47,10 @@ namespace armarx::armem::gui private: QCheckBox* _dataCheckBox; + QLabel* _recursionDepthLabel; + QSpinBox* _recursionDepthSpinner; QPushButton* _storeInLTMButton; - QTabWidget* _tabWidget; - SnapshotSelectorWidget* _snapshotSelectorWidget; }; diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt index 463f59dd1454c0f23fde9b304b5de1b29ae4345b..98cfb29ec529238b6e76c50c51e6563cb4fb4dcb 100644 --- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt +++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt @@ -18,6 +18,7 @@ armarx_add_library( aron_conversions/simox.h aron_conversions/stl.h aron_conversions/eigen.h + util/object_finders.h SOURCES aron_conversions/core.cpp @@ -25,6 +26,7 @@ armarx_add_library( aron_conversions/simox.cpp aron_conversions/stl.cpp aron_conversions/eigen.cpp + util/object_finders.cpp ) diff --git a/source/RobotAPI/libraries/aron/common/util/object_finders.cpp b/source/RobotAPI/libraries/aron/common/util/object_finders.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e6bba147d3f274652f3e173667013383d5d56697 --- /dev/null +++ b/source/RobotAPI/libraries/aron/common/util/object_finders.cpp @@ -0,0 +1,60 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @package RobotAPI::libraries::aron + * @author Philip Scherer ( ulila@student.kit.edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#include "object_finders.h" + +namespace armarx::aron +{ + SubObjectFinder::SubObjectFinder(const std::string& typeNamePrefix) : + typeNamePrefix(typeNamePrefix) + { + } + + const std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>>& + SubObjectFinder::getFoundObjects() + { + return foundSubObjects; + } + + void + SubObjectFinder::visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) + { + if (elementType && simox::alg::starts_with(elementType->getFullName(), typeNamePrefix)) + { + foundSubObjects.emplace(elementData->getPath().toString(), + std::make_pair(elementData, elementType)); + } + } + + void + SubObjectFinder::visitUnknown(DataInput& elementData, TypeInput& elementType) + { + // Ignore (the base class throws an exception here) + } + + SubObjectFinder::MapElements + SubObjectFinder::getObjectElements(DataInput& elementData, TypeInput& elementType) + { + return RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(elementData, + elementType); + } +} // namespace armarx::aron diff --git a/source/RobotAPI/libraries/aron/common/util/object_finders.h b/source/RobotAPI/libraries/aron/common/util/object_finders.h new file mode 100644 index 0000000000000000000000000000000000000000..ff67043de5cea4e608dd3f9d488130d80255005d --- /dev/null +++ b/source/RobotAPI/libraries/aron/common/util/object_finders.h @@ -0,0 +1,132 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @package RobotAPI::libraries::aron + * @author Philip Scherer ( ulila@student.kit.edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <map> +#include <vector> + +#include <SimoxUtility/algorithm/string.h> + +#include <RobotAPI/libraries/aron/common/aron_conversions/core.h> +#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h> +#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h> + +namespace armarx::aron +{ + /** + * Finds aron objects with a given type name prefix in aron variants. + * + * Construct it with the prefix you want to search for, + * then use `visitRecursive` to run the finder over the variant to search. + * Get a map of paths to data and type of the found objects using `getFoundObjects`. + */ + class SubObjectFinder : public armarx::aron::data::RecursiveConstTypedVariantVisitor + { + public: + SubObjectFinder(const std::string& typeNamePrefix); + + ~SubObjectFinder() override = default; + + /** + * Get the objects that have been found. + * + * The keys are strings instead of `aron::Path`s because those don't support comparisons. + * + * @return map of paths to pair of data and type variants + */ + const std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>>& + getFoundObjects(); + + void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override; + + void visitUnknown(DataInput& elementData, TypeInput& elementType) override; + + MapElements getObjectElements(DataInput& elementData, TypeInput& elementType) override; + + private: + std::string typeNamePrefix; + std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> + foundSubObjects; + }; + + /** + * Finds aron objects with a given type name prefix in aron variants and returns them as BOs. + * + * Construct it with the prefix you want to search for, + * then use `visitRecursive` to run the finder over the variant to search. + * Get a map of paths to your BOs using `getFoundObjects`. + * + * \tparam DTOType the _generated_ aron type of your data. + * \tparam BOType the type of your final BO. + */ + template <typename DTOType, typename BOType> + class BOSubObjectFinder : public armarx::aron::data::RecursiveConstTypedVariantVisitor + { + public: + BOSubObjectFinder() = default; + ~BOSubObjectFinder() override = default; + + /** + * Get the objects that have been found. + * + * The keys are strings instead of `aron::Path`s because those don't support comparisons. + * + * @return map of paths to BOs. + */ + const std::map<std::string, BOType>& + getFoundObjects() + { + return foundSubObjects; + } + + void + visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override + { + if (elementType->getFullName() == DTOType::ToAronType()->getFullName()) + { + auto dict = armarx::aron::data::Dict::DynamicCastAndCheck(elementData); + DTOType dto; + dto.fromAron(dict); + BOType boObj = armarx::aron::fromAron<BOType, DTOType>(dto); + foundSubObjects.emplace(elementData->getPath().toString(), boObj); + } + } + + void + visitUnknown(DataInput& elementData, TypeInput& elementType) override + { + // Ignore (the base class throws an exception here) + } + + MapElements + getObjectElements(DataInput& elementData, TypeInput& elementType) override + { + return RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(elementData, + elementType); + } + + private: + std::map<std::string, BOType> foundSubObjects; + }; + +} // namespace armarx::aron diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp index 8e437dd62935c3de5fadf1c93daf10d89270c813..4c0969f0de800c66a6a766fa4a4a80ebf21f6c64 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp @@ -211,7 +211,7 @@ namespace armarx::aron::codegenerator::cpp CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::eNone, const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path())", doc.str())); CppBlockPtr b = std::make_shared<CppBlock>(); - b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;"); + b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;"); std::string dummy; b->appendBlock(this->getWriteTypeBlock("", "", Path(), dummy)); @@ -228,7 +228,7 @@ namespace armarx::aron::codegenerator::cpp CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str())); CppBlockPtr b = std::make_shared<CppBlock>(); - b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;"); + b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;"); std::string dummy; b->appendBlock(this->getWriteBlock("", Path(), dummy)); @@ -245,8 +245,8 @@ namespace armarx::aron::codegenerator::cpp CppMethodPtr m = CppMethodPtr(new CppMethod("template<class ReaderT>\nvoid read(ReaderT& " + ARON_READER_ACCESSOR + ", typename ReaderT::InputType& input)", doc.str())); CppBlockPtr b = std::make_shared<CppBlock>(); - b->addLine("using T [[maybe_unused]] = typename ReaderT::InputType;"); - b->addLine("using TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;"); + b->addLine("using _Aron_T [[maybe_unused]] = typename ReaderT::InputType;"); + b->addLine("using _Aron_TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;"); b->addLine("this->resetSoft();"); b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))"); diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp index 16d73e0611df481b0139d895ab10529c2577387e..5d50b76c48b3aaf664722d97152e6390b8145b0e 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Dict.cpp @@ -72,7 +72,7 @@ namespace armarx::aron::codegenerator::cpp::generator variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor; const std::string elementsAccessor = variantAccessor + "_dictElements"; - block_if_data->addLine("std::map<std::string, T> " + elementsAccessor + ";"); + block_if_data->addLine("std::map<std::string, _Aron_T> " + elementsAccessor + ";"); std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_key"; std::string accessor_iterator_val = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_value"; @@ -107,7 +107,7 @@ namespace armarx::aron::codegenerator::cpp::generator std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictValue"; std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictKey"; - block_if_data->addLine("std::map<std::string, TNonConst> " + elements_accessor + ";"); + block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + elements_accessor + ";"); block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " + elements_accessor + ");"); block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_value + "] : " + elements_accessor + ")"); { diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp index a69f1767565aa2ea8f242c27a4fd03b4450567c3..76094035446a7f90a3d39afe85a8d1bd16ae3e23 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/List.cpp @@ -71,7 +71,7 @@ namespace armarx::aron::codegenerator::cpp::generator variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor; const std::string elementsAccessor = variantAccessor + "_listElements"; - block_if_data->addLine("std::vector<T> " + elementsAccessor + ";"); + block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";"); std::string accessor_iterator = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_it"; @@ -102,7 +102,7 @@ namespace armarx::aron::codegenerator::cpp::generator std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listElements"; std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listValue"; - block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";"); + block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";"); block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + variantAccessor + ", " + elements_accessor + ");"); block_if_data->addLine("for (const auto& " + accessor_iterator_value + " : " + elements_accessor + ")"); { diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp index 4f7cde8f0582a191037c5cd49532f785221394e9..d4edd5476373c0c49029c39ccc7537259116192f 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Pair.cpp @@ -114,7 +114,7 @@ namespace armarx::aron::codegenerator::cpp::generator std::string escaped_accessor = EscapeAccessor(cppAccessor); std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_pairElements"; - block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";"); + block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";"); block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor); auto child_s1 = FromAronType(*type.getFirstAcceptedType()); diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp index 5f2c241cb9024f721f867a54c1bad9a30db96759..84db8b60aab51848000f36d38cc85c8ad6889151 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/container/Tuple.cpp @@ -60,7 +60,7 @@ namespace armarx::aron::codegenerator::cpp::generator variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor; const std::string acceptedTypesAccessor = variantAccessor + "_tupleAcceptedTypes"; - block_if_data->addLine("std::vector<T> " + acceptedTypesAccessor + ";"); + block_if_data->addLine("std::vector<_Aron_T> " + acceptedTypesAccessor + ";"); unsigned int i = 0; for (const auto& type : type.getAcceptedTypes()) @@ -87,7 +87,7 @@ namespace armarx::aron::codegenerator::cpp::generator variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor; const std::string elementsAccessor = variantAccessor + "_tupleElements"; - block_if_data->addLine("std::vector<T> " + elementsAccessor + ";"); + block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";"); unsigned int i = 0; for (const auto& type : type.getAcceptedTypes()) @@ -114,7 +114,7 @@ namespace armarx::aron::codegenerator::cpp::generator std::string resolved_accessor = resolveMaybeAccessor(cppAccessor); std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_tupleElements"; - block_if_data->addLine("std::vector<TNonConst> " + elements_accessor + ";"); + block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";"); block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor); unsigned int i = 0; diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp index 70df7d108e582d0badc09abd7922296fba769dd5..cf15c169b4afec4e5fd833e40bb16b6e74672dc8 100644 --- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp +++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp @@ -97,7 +97,7 @@ namespace armarx::aron::codegenerator::cpp::generator static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends"; CppBlockPtr b = std::make_shared<CppBlock>(); - b->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";"); + b->addLine("std::map<std::string, _Aron_T> " + OBJECT_MEMBERS_ACCESSOR + ";"); if (type.getExtends() != nullptr) { @@ -152,9 +152,9 @@ namespace armarx::aron::codegenerator::cpp::generator CppBlockPtr block_if_data = std::make_shared<CppBlock>(); - block_if_data->addLine("std::map<std::string, T> " + OBJECT_MEMBERS_ACCESSOR + ";"); + block_if_data->addLine("std::map<std::string, _Aron_T> " + OBJECT_MEMBERS_ACCESSOR + ";"); - block_if_data->addLine("std::optional<T> " + OBJECT_EXTENDS_ACCESSOR + ";"); + block_if_data->addLine("std::optional<_Aron_T> " + OBJECT_EXTENDS_ACCESSOR + ";"); if (type.getExtends() != nullptr) { const auto extends_s = FromAronType(*type.getExtends()); @@ -187,7 +187,7 @@ namespace armarx::aron::codegenerator::cpp::generator static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends"; CppBlockPtr block_if_data = std::make_shared<CppBlock>(); - block_if_data->addLine("std::map<std::string, TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";"); + block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";"); if (type.getExtends() != nullptr) { diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h index 4ff55335a50edba6543b171a6bdc323dc50823e6..0be0b34898308d217d221659d3524d77b2cf8f5c 100644 --- a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h +++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h @@ -51,5 +51,7 @@ namespace armarx::aron::data::writer data::VariantPtr writeDouble(const double i, const Path& p = Path()) override; data::VariantPtr writeString(const std::string& i, const Path& p = Path()) override; data::VariantPtr writeBool(const bool i, const Path& p = Path()) override; + + using WriterInterface<data::VariantPtr>::writeDict; }; } diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp index a91f1ac118ba3525ee8a1030915711fb28448c5f..0f6f90a193a9391f943fca4466c294f6ac9d69e9 100644 --- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp +++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp @@ -536,6 +536,31 @@ namespace armarx::aron::data return ret; } + RecursiveConstTypedVariantVisitor::MapElements + RecursiveConstTypedVariantVisitor::GetObjectElementsWithNullType(DataInput& o, TypeInput& t) + { + std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> ret; + auto data = aron::data::Dict::DynamicCastAndCheck(o); + auto type = aron::type::Object::DynamicCastAndCheck(t); + + if (data) + { + for (const auto& [key, e] : data->getElements()) + { + if (type && type->hasMemberType(key)) + { + auto memberType = type->getMemberType(key); + ret.insert({key, {e, memberType}}); + } + else + { + ret.insert({key, {e, nullptr}}); + } + } + } + return ret; + } + RecursiveConstTypedVariantVisitor::MapElements RecursiveConstTypedVariantVisitor::GetDictElements(DataInput& o, TypeInput& t) { diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h index 8a8cc2a906bfc2e3e37734670468ff8207d308f7..389db80a53d43bdb0514f39cbd5276b8f71291bb 100644 --- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h +++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h @@ -201,6 +201,12 @@ namespace armarx::aron::data { type::Descriptor getDescriptor(DataInput& o, TypeInput& n) override; static MapElements GetObjectElements(DataInput& o, TypeInput& t); + /* This override exists for visitors that need to handle untyped members in the hierarchy. + * Using it instead of `GetObjectElements` will allow your visitor to visit objects with + * a nullptr as their type. However, you will have to handle nullptr types in your + * visitor's methods. + */ + static MapElements GetObjectElementsWithNullType(DataInput& o, TypeInput& t); static MapElements GetDictElements(DataInput& o, TypeInput& t); static ListElements GetListElements(DataInput& o, TypeInput& t); static PairElements GetPairElements(DataInput& o, TypeInput& t);