From 55c23329dcb37cad56c358d9bec0186d3bef21ee Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Sat, 19 Mar 2022 10:37:11 +0100
Subject: [PATCH] Add extra utilities to aroncommon and armem/util
---
.../RobotAPI/libraries/armem/CMakeLists.txt | 1 +
source/RobotAPI/libraries/armem/util/util.cpp | 67 ++++++++-
source/RobotAPI/libraries/armem/util/util.h | 34 +++++
.../libraries/aron/common/CMakeLists.txt | 2 +
.../aron/common/util/object_finders.cpp | 60 ++++++++
.../aron/common/util/object_finders.h | 132 ++++++++++++++++++
.../data/visitor/variant/VariantVisitor.cpp | 25 ++++
.../data/visitor/variant/VariantVisitor.h | 6 +
8 files changed, 326 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/libraries/aron/common/util/object_finders.cpp
create mode 100644 source/RobotAPI/libraries/aron/common/util/object_finders.h
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 3e43604dd..6b105d8f6 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -9,6 +9,7 @@ set(LIBS
ArmarXCore
RemoteGui
aron
+ aroncommon
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp
index 34f0e2f56..fcaab64e0 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 a337bfafb..7ffdfbfdc 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/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index ea66ba9db..3930f1917 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 000000000..e6bba147d
--- /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 000000000..b55423fa9
--- /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>(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/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
index a91f1ac11..0f6f90a19 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 8a8cc2a90..389db80a5 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);
--
GitLab