From 2e93f83cc0712a172450cf22f5837c1603f4dece Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 12 Apr 2021 17:22:48 +0200
Subject: [PATCH] minor fixes
---
.../RobotAPI/libraries/armem/CMakeLists.txt | 4 +
source/RobotAPI/libraries/armem/util/util.cpp | 3 +
source/RobotAPI/libraries/armem/util/util.h | 142 ++++++++++++++
.../MemoryConnector.cpp | 2 +
.../MemoryConnector.h | 13 +-
.../armem_robot_mapping/MappingDataReader.cpp | 173 +++++++-----------
.../armem_robot_mapping/MappingDataReader.h | 2 +-
.../armem_robot_mapping/MappingDataWriter.h | 2 +-
8 files changed, 231 insertions(+), 110 deletions(-)
create mode 100644 source/RobotAPI/libraries/armem/util/util.cpp
create mode 100644 source/RobotAPI/libraries/armem/util/util.h
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 0dca18cdb..84bb291a7 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -66,6 +66,8 @@ set(LIB_FILES
mns/MemoryNameSystem.cpp
mns/ClientPlugin.cpp
mns/ComponentPlugin.cpp
+
+ util/util.cpp
)
set(LIB_HEADERS
@@ -135,6 +137,8 @@ set(LIB_HEADERS
mns/ClientPlugin.h
mns/ComponentPlugin.h
+ util/util.h
+
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp
new file mode 100644
index 000000000..34f0e2f56
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/util.cpp
@@ -0,0 +1,3 @@
+#include "util.h"
+
+// intentionally left blank
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
new file mode 100644
index 000000000..8dcb4b997
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -0,0 +1,142 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+#include <optional>
+
+#include <RobotAPI/libraries/armem/core/Entity.h>
+#include <RobotAPI/libraries/armem/core/EntityInstance.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
+
+namespace armarx::armem {
+
+ /**
+ * @brief Tries to cast a armem::EntityInstance to AronClass
+ *
+ * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
+ * @param item
+ * @return std::optional<AronClass>
+ */
+ template <typename AronClass>
+ std::optional<AronClass> tryCast(const EntityInstance& item)
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ try
+ {
+ AronClass t;
+ t.fromAron(item.data());
+ return t;
+ }
+ catch (const armarx::aron::error::AronException&)
+ {
+ return std::nullopt;
+ }
+ }
+
+ /**
+ * @brief Returns all entities that can be cast to AronClass
+ *
+ * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
+ * @param entities collection of entities
+ * @return std::vector<AronClass>
+ */
+ template <typename AronClass>
+ std::vector<AronClass>
+ allOfType(const std::map<std::string, Entity>& entities)
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ std::vector<AronClass> outV;
+
+ // loop over all entities and their snapshots
+ for (const auto &[s, entity] : entities)
+ {
+ for (const auto &[ss, entitySnapshot] : entity.history)
+ {
+ for (const auto& entityInstance : entitySnapshot.instances)
+ {
+ const auto o = tryCast<AronClass>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(*o);
+ }
+ }
+ }
+ }
+
+ return outV;
+ }
+
+ /**
+ * @brief filter + transform for entities.
+ *
+ * Can be used instead of
+ *
+ * std::vector<Bar> ret;
+ *
+ * const auto allOf = allOfType<Foo>(entities);
+ * std::transform(allOf.begin(), allOf.end(), std::back_inserter(ret), pred)
+ *
+ * This function has the benefit that the transform function will be applied directly.
+ * No intermediate vector has to be created (e.g. "allOf" in the example above).
+ *
+ * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
+ * @param entities collection of entities
+ * @param pred binary predicate function, applied to all entity instances
+ * @return vector of "pred"-transformed elements that can be cast to AronClass
+ */
+ template <typename AronClass>
+ auto transformAllOfType(const std::map<std::string, Entity>& entities,
+ auto pred) -> std::vector<decltype(pred(AronClass()))>
+ {
+ static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass,
+ AronClass>::value);
+
+ std::vector<decltype(pred(AronClass()))> outV;
+
+ // loop over all entities and their snapshots
+ for (const auto &[s, entity] : entities)
+ {
+ for (const auto &[ss, entitySnapshot] : entity.history)
+ {
+ for (const auto& entityInstance : entitySnapshot.instances)
+ {
+ const auto o = tryCast<AronClass>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(pred(*o));
+ }
+ }
+ }
+ }
+
+ return outV;
+ }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
index c73dbdd19..4fadf6f65 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
@@ -10,6 +10,8 @@ namespace armarx::armem
MemoryConnector::MemoryConnector(ManagedIceObject& component) : component(component) {}
+ MemoryConnector::~MemoryConnector() = default;
+
void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
{
const std::string prefix = getPropertyPrefix();
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
index 68da96eb0..d3a9f288d 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
@@ -36,16 +36,19 @@ namespace armarx
namespace armarx::armem
{
-
- // TODO(fabian.reister): add
- // class PropertyDefinitionsPtr;
-
+ /**
+ * @brief The MemoryConnector class simplifies connecting to the ArMem memory.
+ *
+ * Use this as the base class of any class that needs to connect to the memory.
+ *
+ *
+ */
class MemoryConnector
{
public:
MemoryConnector(ManagedIceObject& component);
- virtual ~MemoryConnector() = default;
+ virtual ~MemoryConnector();
protected:
armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 7797a72db..126c6b003 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -1,19 +1,62 @@
#include "MappingDataReader.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "RobotAPI/libraries/armem/core/EntityInstance.h"
-#include "RobotAPI/libraries/armem/core/EntitySnapshot.h"
-#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
-#include "RobotAPI/libraries/armem_robot_mapping/types.h"
-#include "RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h"
+#include "RobotAPI/libraries/armem_robot_localization/MemoryConnector.h"
+
+#include <vector>
#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/core/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/EntitySnapshot.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_mapping/types.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-#include <vector>
namespace armarx::armem
{
- armem::client::query::Builder MappingDataReader::buildQuery(const Query& query) const
+ MappingDataReader::MappingDataReader(ManagedIceObject& component) : armarx::armem::MemoryConnector(component) {}
+
+ MappingDataReader::~MappingDataReader() = default;
+
+
+ void MappingDataReader::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+ MemoryConnector::registerPropertyDefinitions(def);
+
+ const std::string prefix = getPropertyPrefix();
+
+ def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void MappingDataReader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
+ << properties.memoryName << "' ...";
+ auto result = useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "TransformReader: Connected to memory '"
+ << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ armem::client::query::Builder
+ MappingDataReader::buildQuery(const Query& query) const
{
armem::client::query::Builder qb;
@@ -35,125 +78,49 @@ namespace armarx::armem
qbEntities.snapshots().timeRange(
IceUtil::Time::microSeconds(query.timeRange.min),
- IceUtil::Time::microSeconds(query.timeRange.max)
- );
+ IceUtil::Time::microSeconds(query.timeRange.max));
return qb;
}
- template <typename AronClass>
- std::optional<AronClass> tryCast(const EntityInstance& item)
- {
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass, AronClass>::value);
-
- try
- {
- AronClass t;
- t.fromAron(item.data());
- return t;
- }
- catch (const armarx::aron::error::AronException&)
- {
- return std::nullopt;
- }
- }
-
- template <typename AronClass>
- std::vector<AronClass> allOfType(const std::map<std::string, Entity>& entities)
- {
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass, AronClass>::value);
-
- std::vector<AronClass> outV;
-
- // loop over all entities and their snapshots
- for (const auto& [s, entity] : entities)
- {
- for (const auto& [ss, entitySnapshot] : entity.history)
- {
- for (const auto& entityInstance : entitySnapshot.instances)
- {
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
- {
- outV.push_back(*o);
- }
- }
- }
- }
-
- return outV;
- }
- template <typename AronClass>
- auto transformAllOfType(const std::map<std::string, Entity>& entities, auto pred)
- {
- static_assert(std::is_base_of<armarx::aron::cppcodegenerator::AronCppClass, AronClass>::value);
-
- std::vector<decltype(pred(AronClass()))> outV;
-
- // loop over all entities and their snapshots
- for (const auto& [s, entity] : entities)
- {
- for (const auto& [ss, entitySnapshot] : entity.history)
- {
- for (const auto& entityInstance : entitySnapshot.instances)
- {
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
- {
- outV.push_back(pred(*o));
- }
- }
- }
- }
-
- return outV;
- }
-
-
- MappingDataReader::Result MappingDataReader::queryData(const Query& query) const
+ MappingDataReader::Result
+ MappingDataReader::queryData(const Query& query) const
{
auto qb = buildQuery(query);
ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- return
- {
- .laserScans = {},
- .status = Result::Status::Error,
- .errorMessage = qResult.errorMessage
- };
+ return {.laserScans = {},
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
}
// now create result from memory
- const auto& entities = qResult.memory
- .getCoreSegment(properties.mappingMemoryName)
- .getProviderSegment(query.agent)
- .entities;
-
-
- auto laserScans = transformAllOfType<aron::LaserScanStamped>(entities, [](const aron::LaserScanStamped & aronLaserScan) -> LaserScanStamped
+ const auto& entities =
+ qResult.memory.getCoreSegment(properties.mappingMemoryName)
+ .getProviderSegment(query.agent)
+ .entities;
+
+ auto laserScans = transformAllOfType<aron::LaserScanStamped>(
+ entities,
+ [](const aron::LaserScanStamped & aronLaserScan) -> LaserScanStamped
{
LaserScanStamped laserScan;
fromAron(aronLaserScan, laserScan);
return laserScan;
});
- return
- {
- .laserScans = std::move(laserScans),
- .status = Result::Status::Success,
- .errorMessage = ""
- };
-
+ return {.laserScans = std::move(laserScans),
+ .status = Result::Status::Success,
+ .errorMessage = ""};
}
} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
index b5307d0de..3ccfc02ff 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -61,7 +61,7 @@ namespace armarx::armem
public:
MappingDataReader(ManagedIceObject& component);
- ~MappingDataReader() override;
+ virtual ~MappingDataReader();
void connect();
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
index 09ff4a1f3..5d5f21185 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
@@ -50,7 +50,7 @@ namespace armarx::armem
{
public:
MappingDataWriter(ManagedIceObject& component);
- ~MappingDataWriter();
+ virtual ~MappingDataWriter();
void connect();
--
GitLab