diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index af25e698a9aa3820dbc68c346a57aa895f97505c..88113c1408093d1bd0bd42f4d703bfb7f67ae663 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,6 +1,9 @@
# ArMem
/source/RobotAPI/components/armem/ @RainerKartmann @dreher
+
/source/RobotAPI/libraries/armem/ @RainerKartmann @dreher
+/source/RobotAPI/libraries/armem_robot_localization/ @FabianReister
+/source/RobotAPI/libraries/armem_robot_mapping/ @FabianReister
+
/source/RobotAPI/interface/armem.ice @RainerKartmann @dreher
-/source/RobotAPI/interface/armem/ @RainerKartmann @dreher
diff --git a/source/RobotAPI/components/ArViz/Client/Layer.h b/source/RobotAPI/components/ArViz/Client/Layer.h
index 56056eabf3fbd031d0aeb0b25bee28956c85ffba..27e056d78eb767637ab9b7a96951c01e3035e902 100644
--- a/source/RobotAPI/components/ArViz/Client/Layer.h
+++ b/source/RobotAPI/components/ArViz/Client/Layer.h
@@ -46,6 +46,11 @@ namespace armarx::viz
(add(std::forward<Ts>(elems)), ...);
}
+ std::size_t size() const noexcept
+ {
+ return data_.elements.size();
+ }
+
data::LayerUpdate data_;
};
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index 4f7c222f5c8ebb3f6a5f65062033218e058a8ddb..b763c4301703bd83fe3af4b06a8fa9d96e4b6b1d 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -85,6 +85,11 @@ namespace armarx::viz
return this->position(position).orientation(orientation);
}
+ DerivedT& pose(const Eigen::Affine3f& pose)
+ {
+ return this->position(pose.translation()).orientation(pose.linear());
+ }
+
Eigen::Matrix4f pose() const
{
auto& p = data_->pose;
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index e102426c44a33acb156a10203e77193689a97f92..85a862f4cd815168d6163d2d085426d71b298068 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -33,6 +33,15 @@ module armarx
string errorMessage;
};
+ dictionary<string, server::MemoryInterface*> MemoryInterfaces;
+ struct GetAllRegisteredMemoriesResult
+ {
+ bool success;
+ string errorMessage;
+
+ MemoryInterfaces proxies;
+ };
+
struct ResolveMemoryNameInput
{
string name;
@@ -69,6 +78,8 @@ module armarx
data::RegisterMemoryResult registerMemory(data::RegisterMemoryInput input);
data::RemoveMemoryResult removeMemory(data::RemoveMemoryInput input);
+ data::GetAllRegisteredMemoriesResult getAllRegisteredMemories();
+
data::ResolveMemoryNameResult resolveMemoryName(data::ResolveMemoryNameInput input);
data::WaitForMemoryResult waitForMemory(data::WaitForMemoryInput input);
};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 369ad1d5504d233fdfac46ccfae3ab90d0b512a5..06c12b18a610b8b05998daea8bc2100cb6839078 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -20,6 +20,7 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
add_subdirectory(armem_robot_localization)
+add_subdirectory(armem_robot_mapping)
add_subdirectory(aron)
add_subdirectory(NJointControllerGuiPluginUtility)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 0dca18cdbbbb8d1c4302c26666d7755a3d671673..84bb291a72f5d9b86ed932b1e1422b5a89252ceb 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/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index cc00c9febfc9927d7a4985a63b1dabf2d8621300..328d1f59d793cbcc9a7d0febc0951775d200013a 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -34,4 +34,4 @@ namespace armarx::armem
*/
Time timeFromStringMicroSeconds(const std::string& microSeconds);
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp
index 0e5c206b636e6504975736d7c6c81a0ff40255d8..79e58300a6968dfda3f60d89ff0511f9ec72f267 100644
--- a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.cpp
@@ -37,6 +37,15 @@ namespace armarx::armem::mns
}
+ armem::data::GetAllRegisteredMemoriesResult
+ ComponentPluginUser::getAllRegisteredMemories(const Ice::Current&)
+ {
+ std::scoped_lock lock(mnsMutex);
+ armem::data::GetAllRegisteredMemoriesResult result = mns.getAllRegisteredMemories();
+ return result;
+ }
+
+
armem::data::ResolveMemoryNameResult ComponentPluginUser::resolveMemoryName(const armem::data::ResolveMemoryNameInput& input, const Ice::Current&)
{
std::scoped_lock lock(mnsMutex);
diff --git a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h
index ade1351d7f24c5315b55056d822fb7b26ef4eec8..1347c197e6d4f560b5773abfed30b92e8f8a7604 100644
--- a/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/mns/ComponentPlugin.h
@@ -40,6 +40,7 @@ namespace armarx::armem::mns
public:
armem::data::RegisterMemoryResult registerMemory(const armem::data::RegisterMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
armem::data::RemoveMemoryResult removeMemory(const armem::data::RemoveMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+ armem::data::GetAllRegisteredMemoriesResult getAllRegisteredMemories(const Ice::Current&) override;
armem::data::ResolveMemoryNameResult resolveMemoryName(const armem::data::ResolveMemoryNameInput& input, const Ice::Current& = Ice::emptyCurrent) override;
armem::data::WaitForMemoryResult waitForMemory(const armem::data::WaitForMemoryInput& input, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index c3cff5bb45c2dfbfa793a32d1ff432a51c740203..ece0fa74306ed77465ce0bb0c643b781ec704178 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -82,6 +82,30 @@ namespace armarx::armem::mns
}
+ data::GetAllRegisteredMemoriesResult
+ MemoryNameSystem::getAllRegisteredMemories()
+ {
+ data::GetAllRegisteredMemoriesResult result;
+ result.success = true;
+ result.errorMessage = "";
+
+ for (const auto& [name, info] : memoryMap)
+ {
+ try
+ {
+ info.proxy->ice_ping();
+ result.proxies[name] = info.proxy;
+ }
+ catch (const Ice::Exception&)
+ {
+ ;
+ }
+ }
+
+ return result;
+ }
+
+
armem::data::ResolveMemoryNameResult MemoryNameSystem::resolveMemoryName(const armem::data::ResolveMemoryNameInput& input)
{
armem::data::ResolveMemoryNameResult result;
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
index 891e78f4acf072e49670336b3d95afb4b766695a..7372246d7e8059787f97f117931830d12c9dc082 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.h
@@ -35,6 +35,8 @@ namespace armarx::armem::mns
*/
data::RemoveMemoryResult removeMemory(const data::RemoveMemoryInput& input);
+ data::GetAllRegisteredMemoriesResult getAllRegisteredMemories();
+
/**
* @brief Gets a memory entry, if it is available.
*/
diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..34f0e2f56d9d47ea7db8b54e198e6faa9d0b25d9
--- /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 0000000000000000000000000000000000000000..4fa457ab679a0f970974101a53ba04a92b3fc241
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -0,0 +1,156 @@
+/*
+ * 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 "ArmarXCore/core/logging/Logging.h"
+#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;
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ }
+
+ // loop over all entities and their snapshots
+ for (const auto &[s, entity] : entities)
+ {
+ if (entity.history.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << s;
+ }
+
+ ARMARX_INFO << "History size: " << entity.history.size();
+
+ 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_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 2cff7aa28476ba530a8b72febb11d563e5101b0b..78097af1a7a40ca0e8c46f1e75d8ce72783e06ab 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -9,6 +9,8 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
#include <QBoxLayout>
#include <QDialog>
#include <QCheckBox>
@@ -47,8 +49,8 @@ namespace armarx::armem::gui
ARMARX_CHECK_NULL(_instanceGroupBox);
- connect(this, &This::connected, this, &This::updateMemory);
- connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemory);
+ connect(this, &This::connected, this, &This::updateMemories);
+ connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::updateMemories);
connect(this, &This::memoryDataChanged, this, &This::updateMemoryTree);
connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::selectedItemChanged, this, &This::updateInstanceTree);
@@ -64,9 +66,9 @@ namespace armarx::armem::gui
void MemoryViewer::onInit(ManagedIceObject& component)
{
- if (memoryName.size() > 0)
+ if (mnsName.size() > 0)
{
- component.usingProxy(memoryName);
+ component.usingProxy(mnsName);
}
if (debugObserverName.size() > 0)
{
@@ -78,10 +80,15 @@ namespace armarx::armem::gui
void MemoryViewer::onConnect(ManagedIceObject& component)
{
- if (!memoryName.empty())
+ if (!mnsName.empty())
{
- component.getProxy(memory, memoryName);
- memoryReader = armem::client::Reader(memory);
+ component.getProxy(mns, mnsName);
+ auto res = mns->getAllRegisteredMemories();
+ for (auto& [name, proxy] : res.proxies)
+ {
+ armem::client::Reader memoryReader{proxy};
+ memoryReaders[name] = memoryReader;
+ }
}
// DebugObserver is optional (check for null on every call)
if (!debugObserverName.empty())
@@ -101,52 +108,78 @@ namespace armarx::armem::gui
}
- void MemoryViewer::updateMemory()
+ void MemoryViewer::updateMemories()
{
- if (!memoryReader)
+ memoryReaders.clear();
+ memoryData.clear();
+
+ for (auto& [name, proxy] : mns->getAllRegisteredMemories().proxies)
{
- return;
+ armem::client::Reader memoryReader{proxy};
+ memoryReaders[name] = memoryReader;
}
- TIMING_START(MemoryQuery);
+ bool dataChanged = false;
+
+ for (auto& [name, reader] : memoryReaders)
{
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- armem::client::QueryResult result = memoryReader.query(input);
- if (result)
+ TIMING_START(MemoryQuery);
+ {
+ armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
+ armem::client::QueryResult result = reader.query(input);
+ if (result)
+ {
+ memoryData[name] = std::move(result.memory);
+ }
+ else
+ {
+ if (statusLabel)
+ {
+ statusLabel->setText(QString::fromStdString(result.errorMessage));
+ }
+ }
+ }
+ TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
+
+ if (debugObserver)
{
- this->memoryData = std::move(result.memory);
+ debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
+ }
+
+ if (memoryData[name])
+ {
+ dataChanged = true;
}
else
{
if (statusLabel)
{
- statusLabel->setText(QString::fromStdString(result.errorMessage));
+ statusLabel->setText("No query result.");
}
}
}
- TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
- }
- if (this->memoryData)
+ if (dataChanged)
{
emit memoryDataChanged();
}
- else
- {
- if (statusLabel)
- {
- statusLabel->setText("No query result.");
- }
- }
}
+
void MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
- if (memoryData)
+ if (memoryData.find(selectedID.memoryName) == memoryData.end())
+ {
+ std::stringstream ss;
+ ss << "Memory name '" << selectedID.memoryName << "' is unknown. Known are: "
+ << simox::alg::get_keys(memoryData);
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ return;
+ }
+
+ const std::optional<armem::Memory>& data = memoryData.at(selectedID.memoryName);
+
+ if (data)
{
if (!selectedID.hasEntityName())
{
@@ -156,7 +189,7 @@ namespace armarx::armem::gui
const armem::EntitySnapshot* snapshot = nullptr;
if (!id.hasTimestamp())
{
- const armem::Entity& entity = memoryData->getEntity(id);
+ const armem::Entity& entity = data->getEntity(id);
if (entity.empty())
{
return;
@@ -170,7 +203,7 @@ namespace armarx::armem::gui
{
try
{
- snapshot = &memoryData->getEntitySnapshot(id);
+ snapshot = &data->getEntitySnapshot(id);
}
catch (const armem::error::ArMemError& e)
{
@@ -187,19 +220,29 @@ namespace armarx::armem::gui
}
if (id.hasInstanceIndex())
{
- instanceGroup->view->update(id, *memoryData);
+ instanceGroup->view->update(id, *data);
}
}
}
void MemoryViewer::updateMemoryTree()
{
- if (!memoryData)
+ std::map<std::string, const armem::Memory*> convMap;
+ for (auto& [name, data] : memoryData)
+ {
+ if (data.has_value())
+ {
+ convMap[name] = &data.value();
+ }
+ }
+
+ if (convMap.empty())
{
return;
}
+
TIMING_START(GuiUpdate);
- memoryGroup->tree()->update(*memoryData);
+ memoryGroup->tree()->update(convMap);
TIMING_END_STREAM(GuiUpdate, ARMARX_VERBOSE);
if (debugObserver)
@@ -209,34 +252,29 @@ namespace armarx::armem::gui
}
- const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryName";
+ const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryNameSystem";
const static std::string CONFIG_KEY_DEBUG_OBSERVER = "MemoryViewer.DebugObserverName";
void MemoryViewer::loadSettings(QSettings* settings)
{
- memoryName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "Memory").toString().toStdString();
+ mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "ArMemMemoryNameSystem").toString().toStdString();
debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver").toString().toStdString();
}
void MemoryViewer::saveSettings(QSettings* settings)
{
- settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY), QString::fromStdString(memoryName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY), QString::fromStdString(mnsName));
settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), QString::fromStdString(debugObserverName));
}
void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armem::server::MemoryInterfacePrx>({CONFIG_KEY_MEMORY, "Memory", "*Memory"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "ArMemMemoryNameSystem", "*"});
dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
- memoryName = dialog->getProxyName(CONFIG_KEY_MEMORY);
+ mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
debugObserverName = dialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
}
-
-
-
-
-
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index df66ad86fa603443119744d0ee152f5f126e805a..5c8f4b2f182f798a3c076a89e6a30ff24d10050c 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -7,7 +7,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
@@ -15,7 +15,6 @@
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
-
class QBoxLayout;
class QDialog;
class QGroupBox;
@@ -63,7 +62,7 @@ namespace armarx::armem::gui
public slots:
- void updateMemory();
+ void updateMemories();
void updateInstanceTree(const armem::MemoryID& selectedID);
@@ -95,11 +94,11 @@ namespace armarx::armem::gui
public:
- std::string memoryName;
- armem::server::MemoryInterfacePrx memory;
- armem::client::Reader memoryReader;
+ std::string mnsName;
+ armem::mns::MemoryNameSystemInterfacePrx mns;
- std::optional<armem::Memory> memoryData;
+ std::map<std::string, armem::client::Reader> memoryReaders;
+ std::map<std::string, std::optional<armem::Memory>> memoryData;
QLayout* updateWidgetLayout;
@@ -121,5 +120,3 @@ namespace armarx::armem::gui
};
}
-
-
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
index c66b568c89a28e714bffbe4df1fcba00ccdfd666..4fadf6f65ebb0ade9b91d49d36860bf6e9b0ff64 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp
@@ -1,20 +1,24 @@
#include "MemoryConnector.h"
#include <ArmarXCore/core/ManagedIceObject.h>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx
+namespace armarx::armem
{
MemoryConnector::MemoryConnector(ManagedIceObject& component) : component(component) {}
+ MemoryConnector::~MemoryConnector() = default;
+
void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
{
- ARMARX_INFO << "Memory connector: registerPropertyDefinitions";
-
const std::string prefix = getPropertyPrefix();
+ ARMARX_INFO << "Memory connector: registerPropertyDefinitions with prefix " << prefix;
+
+
def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
}
@@ -40,4 +44,10 @@ namespace armarx
return result;
}
-} // namespace armarx
+ const std::string& MemoryConnector::getPropertyPrefix() const
+ {
+ return propertyPrefix;
+ }
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
index 1af5ec5d889f520c62e0dc5e4d89948cba268d3d..d3a9f288da15cefb71eb8bf5ecabf9bf7e250c4b 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h
@@ -30,17 +30,25 @@
namespace armarx
{
-
class ManagedIceObject;
+}
- // TODO(fabian.reister): add
- // class PropertyDefinitionsPtr;
+namespace armarx::armem
+{
+ /**
+ * @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();
protected:
armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
@@ -48,15 +56,14 @@ namespace armarx
void waitForMemory();
- virtual std::string getPropertyPrefix()
- {
- return "mem.";
- }
+ virtual const std::string& getPropertyPrefix() const;
private:
ManagedIceObject& component;
armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
+
+ const std::string propertyPrefix;
};
-} // namespace armarx
\ No newline at end of file
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h b/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
index cb353f57d73561dfdda4a96ce60137e988e5db09..6962593814414267635efc425637266c7fbcd34a 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
@@ -46,60 +46,66 @@ namespace armarx
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
};
- struct TransformResult
+ namespace armem
{
- Transform transform;
- enum class Status
+ struct TransformResult
{
- success,
- errorLookupIntoFuture,
- errorFrameNotAvailable
- } status;
+ Transform transform;
- explicit operator bool() const
- {
- return status == Status::success;
- }
+ enum class Status
+ {
+ Success,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
- std::string errorMessage = "";
- };
+ explicit operator bool() const
+ {
+ return status == Status::Success;
+ }
- struct TransformQuery
- {
- TransformHeader header;
+ std::string errorMessage = "";
+ };
- // bool exact;
- };
+ struct TransformQuery
+ {
+ TransformHeader header;
- class TransformInterface
- {
- public:
- virtual ~TransformInterface() = default;
+ // bool exact;
+ };
- virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
- };
+ class TransformInterface
+ {
+ public:
+ virtual ~TransformInterface() = default;
- class TransformReaderInterface : virtual public TransformInterface
- {
- public:
- virtual ~TransformReaderInterface() = default;
+ virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
+ virtual void connect() = 0;
+ };
- virtual TransformResult getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const std::int64_t& timestamp) const = 0;
+ class TransformReaderInterface : virtual public TransformInterface
+ {
+ public:
+ virtual ~TransformReaderInterface() = default;
- virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
- // waitForTransform()
- };
+ virtual TransformResult
+ getGlobalPose(const std::string& agentName, const std::string& robotRootFrame,
+ const std::int64_t& timestamp) const = 0;
- class TransformWriterInterface : virtual public TransformInterface
- {
- public:
- ~TransformWriterInterface() override = default;
+ virtual TransformResult
+ lookupTransform(const TransformQuery& query) const = 0;
+ // waitForTransform()
+ };
- virtual bool commitTransform(const Transform& transform) = 0;
- };
+ class TransformWriterInterface : virtual public TransformInterface
+ {
+ public:
+ ~TransformWriterInterface() override = default;
+
+ virtual bool commitTransform(const Transform& transform) = 0;
+ };
+
+ } // namespace armem
} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
index 80b242b1204a28b255283939cd64588f1c251c01..b14f1a711e490c2bcae4eca0ca81976ca888a6ea 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
@@ -56,7 +56,7 @@
#include <RobotAPI/libraries/armem_robot_localization/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-namespace armarx
+namespace armarx::armem
{
TransformReader::TransformReader(ManagedIceObject& component) : MemoryConnector(component) {}
@@ -65,7 +65,7 @@ namespace armarx
void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
- ARMARX_INFO << "TransformReader: registerPropertyDefinitions";
+ ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
MemoryConnector::registerPropertyDefinitions(def);
const std::string prefix = getPropertyPrefix();
@@ -131,7 +131,7 @@ namespace armarx
std::vector<std::string> TransformReader::buildTransformChain(const armem::Memory& memory,
const TransformQuery& query) const
{
- ARMARX_INFO << "Building transform chain";
+ ARMARX_DEBUG << "Building transform chain";
auto join = [](const std::string & parentFrame, const std::string & frame)
{
@@ -229,17 +229,17 @@ namespace armarx
{
ARMARX_TRACE;
- ARMARX_INFO << "Entering";
+ ARMARX_DEBUG << "Entering";
ARMARX_CHECK(not queue.empty())
<< "The queue has to contain at least two items to perform a lookup";
- ARMARX_INFO << "Entering ... "
- << "Q front " << queue.front().header.timestamp << " "
- << "Q back " << queue.back().header.timestamp << " "
- << "query timestamp " << timestamp;
+ ARMARX_DEBUG << "Entering ... "
+ << "Q front " << queue.front().header.timestamp << " "
+ << "Q back " << queue.back().header.timestamp << " "
+ << "query timestamp " << timestamp;
// TODO(fabian.reister): sort queue.
@@ -247,26 +247,26 @@ namespace armarx
ARMARX_CHECK(queue.back().header.timestamp > timestamp)
<< "Cannot perform lookup into the future!";
- // ARMARX_INFO << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
+ // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
ARMARX_CHECK(queue.front().header.timestamp < timestamp)
<< "Cannot perform lookup. Timestamp too old";
// => now we know that there is an element right after and before the timestamp within our queue
- ARMARX_INFO << "Entering 2";
+ ARMARX_DEBUG << "Entering 2";
const auto poseNextIt = findFirstElementAfter(queue, timestamp);
- ARMARX_INFO << "it ari";
+ ARMARX_DEBUG << "it ari";
const auto posePreIt = poseNextIt - 1;
- ARMARX_INFO << "deref";
+ ARMARX_DEBUG << "deref";
// the time fraction [0..1] of the lookup wrt to posePre and poseNext
const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
(poseNextIt->header.timestamp - posePreIt->header.timestamp);
- ARMARX_INFO << "interpolate";
+ ARMARX_DEBUG << "interpolate";
return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
}
@@ -300,13 +300,16 @@ namespace armarx
return convertEntityToTransform(entity.getInstance(0));
});
- ARMARX_INFO << "obtaining transform";
+ ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1)
{
- ARMARX_INFO << "More than one snapshots received: " << transforms.size();
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
+
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_INFO << "Done interpolating transform";
+ ARMARX_DEBUG << "Done interpolating transform";
return p;
}
@@ -314,12 +317,12 @@ namespace armarx
// accept this to fail (will raise armem::error::MissingEntry)
if (transforms.empty())
{
- ARMARX_INFO << "empty transform";
+ ARMARX_DEBUG << "empty transform";
throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
}
- ARMARX_INFO << "single transform";
+ ARMARX_DEBUG << "single transform";
return transforms.front().transform;
@@ -377,7 +380,7 @@ namespace armarx
const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
- ARMARX_INFO << "Looking up transform at timestamp " << timestamp;
+ ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -395,7 +398,7 @@ namespace armarx
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_INFO << "Lookup result in reader: " << qResult;
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
if (not qResult.success)
{
@@ -403,7 +406,7 @@ namespace armarx
{
.header = query.header,
},
- .status = TransformResult::Status::errorFrameNotAvailable,
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
query.header.frame + "' : " + qResult.errorMessage};
}
@@ -413,7 +416,7 @@ namespace armarx
if (tfChain.empty())
{
return {.transform = {.header = query.header},
- .status = TransformResult::Status::errorFrameNotAvailable,
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Cannot create tf lookup chain '" +
query.header.parentFrame + " -> " + query.header.frame +
"'"};
@@ -426,7 +429,7 @@ namespace armarx
{
ARMARX_WARNING << "No transform available.";
return {.transform = {.header = query.header},
- .status = TransformResult::Status::errorFrameNotAvailable,
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in TF loookup: '" +
query.header.parentFrame + " -> " + query.header.frame +
"'. No memory data in time range."};
@@ -437,10 +440,10 @@ namespace armarx
Eigen::Affine3f::Identity(),
std::multiplies<>());
- ARMARX_INFO << "Found valid transform";
+ ARMARX_DEBUG << "Found valid transform";
return {.transform = {.header = query.header, .transform = transform},
- .status = TransformResult::Status::success};
+ .status = TransformResult::Status::Success};
}
-} // namespace armarx
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
index 458ce5d89a7ec56a5f34f3d8b49a608158e69fde..061499f7b69fd1a8a1b372efe590229f5722c73e 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
@@ -27,7 +27,7 @@
#include "MemoryConnector.h"
#include "TransformInterfaces.h"
-namespace armarx
+namespace armarx::armem
{
/**
* @defgroup Component-ExampleClient ExampleClient
@@ -41,8 +41,8 @@ namespace armarx
* Detailed description of class ExampleClient.
*/
class TransformReader :
- virtual public armarx::TransformReaderInterface,
- virtual public armarx::MemoryConnector
+ virtual public ::armarx::armem::TransformReaderInterface,
+ virtual public ::armarx::armem::MemoryConnector
{
public:
TransformReader(ManagedIceObject& component);
@@ -56,11 +56,11 @@ namespace armarx
const std::int64_t& timestamp) const override;
TransformResult lookupTransform(const TransformQuery& query) const override;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
- std::string getPropertyPrefix() override
+ const std::string& getPropertyPrefix() const override
{
- return "mem.read.";
+ return propertyPrefix;
}
private:
@@ -82,5 +82,8 @@ namespace armarx
std::string memoryName = "RobotState";
std::string localizationMemoryName = "Localization";
} properties;
+
+
+ const std::string propertyPrefix = "mem.localization.read.";
};
-} // namespace armarx
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
index 55100c09eeee08293fac3ca455a04ba31bcbeaea..2db61f978e45fb2f8582532454d6423026b9fcfe 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
@@ -49,7 +49,7 @@
#include "aron_conversions.h"
-namespace armarx
+namespace armarx::armem
{
TransformWriter::TransformWriter(ManagedIceObject& component) : MemoryConnector(component) {}
@@ -58,7 +58,7 @@ namespace armarx
void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
- ARMARX_INFO << "TransformWriter: registerPropertyDefinitions";
+ ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
MemoryConnector::registerPropertyDefinitions(def);
@@ -121,10 +121,10 @@ namespace armarx
update.instancesData = {aronTransform.toAron()};
update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
- ARMARX_INFO << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+ ARMARX_DEBUG << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
- ARMARX_INFO << updateResult;
+ ARMARX_DEBUG << updateResult;
if (not updateResult.success)
{
@@ -134,4 +134,10 @@ namespace armarx
return updateResult.success;
}
-} // namespace armarx
+ const std::string& TransformWriter::getPropertyPrefix() const
+ {
+ return propertyPrefix;
+ }
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
index cc7be8737647cd86a19109ab77da3b5ee2ea2afc..2804fbc53e7a0f61c937dc0a1b6c65d558196e1c 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
@@ -22,13 +22,14 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/Writer.h>
#include <mutex>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
#include "MemoryConnector.h"
#include "TransformInterfaces.h"
-namespace armarx
+namespace armarx::armem
{
/**
@@ -43,8 +44,8 @@ namespace armarx
* Detailed description of class ExampleClient.
*/
class TransformWriter :
- virtual public armarx::TransformWriterInterface,
- virtual public armarx::MemoryConnector
+ virtual public ::armarx::armem::TransformWriterInterface,
+ virtual public ::armarx::armem::MemoryConnector
{
public:
TransformWriter(ManagedIceObject& component);
@@ -60,10 +61,7 @@ namespace armarx
bool commitTransform(const Transform& transform) override;
- std::string getPropertyPrefix() override
- {
- return "mem.write.";
- }
+ const std::string& getPropertyPrefix() const override;
private:
armem::client::Writer memoryWriter;
@@ -76,5 +74,7 @@ namespace armarx
} properties;
std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.localization.write.";
};
-} // namespace armarx
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
index bca16f71ea33e944fdbf77de9751361895882e33..6a5379fe759f73fd03ad7e274a9a2211ba89d846 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
@@ -8,7 +8,7 @@ namespace armarx
namespace aron
{
struct Transform;
- }
+ } // namespace aron
void fromAron(const aron::Transform& aronTransform, Transform& transform);
void toAron(const Transform& transform, aron::Transform& aronTransform);
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5712d4c15f338783ac913f3416d6b8fcdb900003
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
@@ -0,0 +1,36 @@
+set(LIB_NAME armem_robot_mapping)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPICore
+ RobotAPI::libraries::armem
+ # TODO(fabian.reister): remove this dependency!
+ RobotAPI::armem_robot_localization
+ # System / External
+ Eigen3::Eigen
+ HEADERS
+ ./aron_conversions.h
+ ./MappingDataWriter.h
+ ./MappingDataReader.h
+ SOURCES
+ ./aron_conversions.cpp
+ ./MappingDataWriter.cpp
+ ./MappingDataReader.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/LaserScan.xml
+)
+
+
+add_library(RobotAPI::armem_robot_mapping ALIAS armem_robot_mapping)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aae0106dab35ac2252e90f78ee5dd79a37c9188c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -0,0 +1,208 @@
+#include "MappingDataReader.h"
+
+#include <cstring>
+#include <vector>
+#include <algorithm>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+
+#include <IceUtil/Time.h>
+#include <IceUtil/Handle.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/ProviderSegment.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/EntityInstance.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_mapping/types.h>
+
+namespace armarx::armem
+{
+ class Entity;
+} // namespace armarx
+
+namespace armarx::armem
+{
+
+ 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;
+
+ ARMARX_INFO << "Query for agent: " << query.agent
+ << " memory name: " << properties.mappingMemoryName;
+
+ if (query.sensorList.empty()) // all sensors
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.mappingMemoryName)
+ .providerSegments().withName(query.agent)
+ .entities().all()
+ .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ // clang-format on
+ }
+ else
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.mappingMemoryName)
+ .providerSegments().withName(query.agent)
+ .entities().withNames(query.sensorList)
+ .snapshots().timeRange(query.timeRange.min,query.timeRange.max);
+ // clang-format on
+ }
+
+ return qb;
+
+ }
+
+ std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, Entity>& entities)
+ {
+ std::vector<LaserScanStamped> outV;
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ }
+
+ const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const EntityInstance & ei) -> LaserScanStamped
+ {
+ LaserScanStamped laserScanStamped;
+ fromAron(aronLaserScanStamped, laserScanStamped);
+
+ const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+
+ ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
+ ARMARX_IMPORTANT << "4";
+
+
+ return laserScanStamped;
+
+ };
+
+ // loop over all entities and their snapshots
+ for (const auto &[s, entity] : entities)
+ {
+ if (entity.history.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << s;
+ }
+
+ ARMARX_INFO << "History size: " << entity.history.size();
+
+ for (const auto &[ss, entitySnapshot] : entity.history)
+ {
+ for (const auto& entityInstance : entitySnapshot.instances)
+ {
+ const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ }
+ }
+ }
+
+ return outV;
+ }
+
+ MappingDataReader::Result
+ MappingDataReader::queryData(const Query& query) const
+ {
+ const auto qb = buildQuery(query);
+
+ ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult =
+ memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: "
+ << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const auto& entities =
+ qResult.memory.getCoreSegment(properties.mappingMemoryName)
+ .getProviderSegment(query.agent)
+ .entities;
+
+ const auto laserScans = asLaserScans(entities);
+ const auto sensors = simox::alg::get_keys(entities);
+
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
+ .errorMessage = ""};
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..359b88fe8550d9b64f066fe942a5c6fda7dce172
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -0,0 +1,129 @@
+/*
+ * 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/>.
+ *
+ * @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 <stdint.h>
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+// TODO(fabian.reister): move MemoryConnector to armem library
+#include <RobotAPI/libraries/armem_robot_localization/MemoryConnector.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem_robot_mapping/types.h>
+
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+
+namespace armarx::armem
+{
+
+ struct TimeRange
+ {
+ Time min;
+ Time max;
+ };
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class MappingDataReader :
+ virtual public armarx::armem::MemoryConnector
+ {
+ public:
+ MappingDataReader(ManagedIceObject& component);
+
+ virtual ~MappingDataReader();
+
+ void connect();
+
+
+ struct Query
+ {
+ std::string agent;
+
+ TimeRange timeRange;
+
+ // if empty, all sensors will be queried
+ std::vector<std::string> sensorList;
+
+ };
+
+ struct Result
+ {
+
+ std::vector<LaserScanStamped> laserScans;
+
+ std::vector<std::string> sensors;
+
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+
+ std::string errorMessage;
+
+ };
+
+ Result queryData(const Query& query) const;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+ const std::string& getPropertyPrefix() const override
+ {
+ return propertyPrefix;
+ }
+
+ client::query::Builder buildQuery(const Query& query) const ;
+
+
+ private:
+
+ armem::client::Reader memoryReader;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "RobotState";
+ std::string mappingMemoryName = "Mapping";
+ } properties;
+
+
+ const std::string propertyPrefix = "mem.mapping.read.";
+ };
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd0299ece229eaac2fb4f44758fae23f763f612d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
@@ -0,0 +1,99 @@
+#include "MappingDataWriter.h"
+
+#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+
+
+namespace armarx::armem
+{
+
+ MappingDataWriter::MappingDataWriter(ManagedIceObject& component)
+ : MemoryConnector(component) {}
+
+ MappingDataWriter::~MappingDataWriter() = default;
+
+ void MappingDataWriter::registerPropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "TransformWriter: 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 MappingDataWriter::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
+
+ memoryWriter.setWritingMemory(result.proxy);
+ }
+
+
+ bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const std::int64_t& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ const auto result =
+ memoryWriter.addSegment(properties.mappingMemoryName, agentName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto iceTimestamp = Time::microSeconds(timestamp);
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID =
+ providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ update.timeCreated = armem::Time::now();
+
+ arondto::LaserScanStamped aronSensorData;
+ // currently only sets the header
+ toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
+
+ auto dict = aronSensorData.toAron();
+ dict->addElement("scan", toAron(laserScan));
+
+ update.instancesData = {dict};
+ update.timeCreated = iceTimestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d5f21185ba6bfad4cb319bef81853ef3bd551f5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
@@ -0,0 +1,88 @@
+/*
+ * 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::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 <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+// TODO(fabian.reister): move MemoryConnector to armem lib
+#include "RobotAPI/libraries/armem_robot_localization/MemoryConnector.h"
+
+
+namespace armarx::armem
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class MappingDataWriter :
+ virtual public ::armarx::armem::MemoryConnector
+ {
+ public:
+ MappingDataWriter(ManagedIceObject& component);
+ virtual ~MappingDataWriter();
+
+ void connect();
+
+ // MappingDataWriterInterface
+ /// to be called in Component::onConnectComponent
+ // void connect() override;
+
+ /// to be called in Component::addPropertyDefinitions
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+
+ bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+
+ const std::string& getPropertyPrefix() const override
+ {
+ return propertyPrefix;
+ }
+
+ private:
+ armem::client::Writer memoryWriter;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "RobotState";
+ std::string mappingMemoryName = "Mapping";
+ } properties;
+
+ std::mutex memoryWriterMutex;
+
+
+ const std::string propertyPrefix = "mem.mapping.write.";
+ };
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
new file mode 100644
index 0000000000000000000000000000000000000000..659df4a1536b0f81b81ba64f77e4049ba42fdd5c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
@@ -0,0 +1,54 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::arondto::LaserScannerInfo'>
+ <ObjectChild key='device'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='minAngle'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='maxAngle'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='stepSize'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::SensorHeader">
+ <ObjectChild key="agent">
+ <string/>
+ </ObjectChild>
+ <ObjectChild key="frame">
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::arondto::LaserScanStamped'>
+ <ObjectChild key="header">
+ <armarx::arondto::SensorHeader />
+ </ObjectChild>
+
+ <!--
+ <ObjectChild key='data'>
+ <NdArray />
+ </ObjectChild> -->
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40471a632c66708ecb8085ea63a65acf6013d5a2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
@@ -0,0 +1,100 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+#include "types.h"
+
+
+namespace armarx
+{
+
+ /************ fromAron ************/
+
+ armem::Time timeFromAron(const int64_t timestamp)
+ {
+ return armem::Time::microSeconds(timestamp);
+ }
+
+ SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
+ {
+
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
+ .timestamp = timeFromAron(aronSensorHeader.timestamp)};
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScanStamped& laserScan)
+ {
+ laserScan.header = fromAron(aronLaserScan.header);
+ // laserScan.data = fromAron(aronLaserScan.data);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
+ std::int64_t& timestamp, std::string& frame,
+ std::string& agentName)
+ {
+ const auto header = fromAron(aronLaserScan.header);
+
+ // laserScan = fromAron(aronLaserScan.data);
+
+ timestamp = header.timestamp.toMicroSeconds();
+ frame = header.frame;
+ agentName = header.agent;
+ }
+
+
+ /************ toAron ************/
+
+
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
+
+ int64_t toAron(const armem::Time& timestamp)
+ {
+ return timestamp.toMicroSeconds();
+ }
+
+ arondto::SensorHeader toAron(const SensorHeader& sensorHeader)
+ {
+ arondto::SensorHeader aronSensorHeader;
+
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.timestamp = toAron(sensorHeader.timestamp);
+
+ return aronSensorHeader;
+ }
+
+ void toAron(const LaserScanStamped& laserScanStamped,
+ arondto::LaserScanStamped& aronLaserScanStamped)
+ {
+ aronLaserScanStamped.header = toAron(laserScanStamped.header);
+ // toAron(laserScanStamped.data, aronLaserScanStamped.data);
+ }
+
+ void toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScanStamped)
+ {
+ const SensorHeader header
+ {
+ .agent = agentName, .frame = frame, .timestamp = timestamp};
+
+ const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+
+ toAron(laserScanStamped, aronLaserScanStamped);
+ }
+
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e0d690a48da10f3d07290f6bf32281251a877e5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
@@ -0,0 +1,70 @@
+/*
+ * 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/>.
+ *
+ * @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 <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+namespace armarx
+{
+
+ namespace arondto
+ {
+ struct LaserScanStamped;
+ } // namespace aron
+
+ // struct LaserScan;
+ struct LaserScanStamped;
+
+ void fromAron(
+ const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName);
+
+
+ template<typename T>
+ auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+ void toAron(
+ const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScan);
+
+ inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
+ {
+ return aron::datanavigator::NDArrayNavigator::FromVector(laserScan);
+ }
+
+
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_robot_mapping/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..d822597e103177b0833014d25f0530e25f2b2075
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_mapping/types.h
@@ -0,0 +1,43 @@
+/*
+ * 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/>.
+ *
+ * @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 <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx
+{
+
+ struct SensorHeader
+ {
+ std::string agent;
+ std::string frame;
+ armem::Time timestamp;
+ };
+
+ struct LaserScanStamped
+ {
+ SensorHeader header;
+ LaserScan data;
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/converter/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
index 637345f2b3a4792f269dfd1cee0aed7861db323c..a99bfccfd49d736dc8d4c1ff55e41decb06029ee 100644
--- a/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/CMakeLists.txt
@@ -1,4 +1,19 @@
+add_subdirectory(common)
add_subdirectory(ivt)
add_subdirectory(pcl)
add_subdirectory(eigen)
add_subdirectory(opencv)
+
+
+add_library(AronConverter INTERFACE)
+
+target_link_libraries(AronConverter
+ INTERFACE
+ RobotAPI::aron::converter::common
+ RobotAPI::aron::converter::ivt
+ RobotAPI::aron::converter::pcl
+ RobotAPI::aron::converter::eigen
+ RobotAPI::aron::converter::opencv
+)
+
+add_library(RobotAPI::aron::converter ALIAS AronConverter)
diff --git a/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ad1ddc3669b4672d4d9a2f9def8cb4643b8d6821
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/CMakeLists.txt
@@ -0,0 +1,23 @@
+set(LIB_NAME aroncommonconverter)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ aron
+)
+
+set(LIB_FILES
+ Converter.cpp
+ VectorConverter.cpp
+)
+
+set(LIB_HEADERS
+ Converter.h
+ VectorConverter.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+
+add_library(RobotAPI::aron::converter::common ALIAS aroncommonconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/common/Converter.cpp b/source/RobotAPI/libraries/aron/converter/common/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5161e17c00a2ab5dbe0d893a83ef29d46ac7da9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/Converter.cpp
@@ -0,0 +1 @@
+#include "Converter.h"
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/converter/common/Converter.h b/source/RobotAPI/libraries/aron/converter/common/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..570344ab618f8a353327a716aaa9ba5f850eaca8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/Converter.h
@@ -0,0 +1,71 @@
+/*
+ * 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/>.
+ *
+ * @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 <algorithm>
+#include <vector>
+
+namespace armarx::aron
+{
+
+ /**
+ * @brief Converter function for vector of aron elements to plain cpp type
+ *
+ * You have to provide a converter function for the element with the signature
+ *
+ * PlainCppType fromAron(const AronType&)
+ *
+ * @tparam T the aron vector element
+ * @param v the vector of elements
+ * @return the vector of aron elements
+ */
+ template <typename T>
+ auto fromAron(const std::vector<T>& v) -> std::vector<decltype(fromAron(T()))>
+ {
+ std::vector<decltype(fromAron(T()))> r;
+ r.reserve(v.size());
+
+ std::transform(v.begin(), v.end(), std::back_inserter(r),
+ [](const T & t)
+ {
+ return fromAron(t);
+ });
+
+ return r;
+ }
+
+
+ template <typename T> auto toAron(const std::vector<T>& v)
+ {
+ std::vector<decltype(toAron(T()))> r;
+ r.reserve(v.size());
+
+ std::transform(v.begin(), v.end(), std::back_inserter(r),
+ [](const T & t)
+ {
+ return toAron(t);
+ });
+
+ return r;
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f5406cc2be80d443623d0d406865b89279ad00e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.cpp
@@ -0,0 +1 @@
+#include "VectorConverter.h"
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..17475a3b9df0f39ee10e82443f24e5251db63c58
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/common/VectorConverter.h
@@ -0,0 +1,69 @@
+/*
+ * 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/>.
+ *
+ * @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
+
+// STD/STL
+#include <memory>
+#include <string>
+#include <numeric>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+
+namespace armarx::aron::converter
+{
+ class AronVectorConverter
+ {
+ public:
+ AronVectorConverter() = delete;
+
+ template<typename T>
+ static std::vector<T> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
+ {
+ ARMARX_CHECK_NOT_NULL(nav);
+
+ const auto& dims = nav->getDimensions();
+
+ if(dims.size() != 2)
+ {
+ throw error::AronException("AronVectorConverter", "ConvertToVector", "The NDArray must have two dimensions.", nav->getPath());
+ }
+
+ if(dims.at(1) != sizeof(T))
+ {
+ throw error::AronException("AronVectorConverter", "ConvertToVector", "Dimension 1 of the array has to match the element size.", nav->getPath());
+ }
+
+ const int size = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>());
+
+ std::vector<T> v(dims.at(0));
+ memcpy(v.data(), nav->getData(), size);
+
+ return v;
+ }
+ };
+
+} // namespace armarx::aron::converter
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
index 44b70e2e88ffe15465aa558cb153d491f8ce62c4..d4f9008ea45cfb34d3a6056e2155907060e672f0 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/eigen/CMakeLists.txt
@@ -8,6 +8,7 @@ armarx_build_if(Eigen3_FOUND "Eigen3 not available")
set(LIBS
aron
+ Eigen3::Eigen
)
set(LIB_FILES
@@ -20,6 +21,4 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
+add_library(RobotAPI::aron::converter::eigen ALIAS aroneigenconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
index a7651058d79736edb0797659365ddfc4b62782c6..376a1ae7bcbc17edf348c03d40b7c05602751e04 100644
--- a/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/ivt/CMakeLists.txt
@@ -7,7 +7,9 @@ find_package(IVT COMPONENTS ivt ivtopencv QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
set(LIBS
- aron ivt ivtopencv
+ aron
+ ivt
+ ivtopencv
)
set(LIB_FILES
@@ -21,5 +23,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(IVT_FOUND)
- include_directories(${IVT_INCLUDE_DIRS})
+ target_include_directories(aronivtconverter
+ SYSTEM PUBLIC
+ ${IVT_INCLUDE_DIRS}
+ )
endif()
+
+add_library(RobotAPI::aron::converter::ivt ALIAS aronivtconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
index 1b54988b28c2e024b6035a8f722e2fe184ec78bf..5cbe0acad210ec314726aa204e59cefcb9fe23af 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/opencv/CMakeLists.txt
@@ -7,7 +7,8 @@ find_package(OpenCV QUIET)
armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(LIBS
- aron ${OpenCV_LIBRARIES}
+ aron
+ ${OpenCV_LIBRARIES}
)
set(LIB_FILES
@@ -21,5 +22,11 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(OpenCV_FOUND)
- include_directories(${OpenCV_INCLUDE_DIRS})
+ target_include_directories(aronopencvconverter
+ SYSTEM PUBLIC
+ ${OpenCV_INCLUDE_DIRS}
+ )
endif()
+
+
+add_library(RobotAPI::aron::converter::opencv ALIAS aronopencvconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
index 0a1c4532df8ad750b94056911c3cfd157ff937f4..c14d50a5f6f91db9b36088845b63d14ed217a6a4 100644
--- a/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/converter/pcl/CMakeLists.txt
@@ -7,7 +7,8 @@ find_package(PCL QUIET)
armarx_build_if(PCL_FOUND "PCL not available")
set(LIBS
- aron ${PCL_COMMON_LIBRARIES}
+ aron
+ ${PCL_COMMON_LIBRARIES}
)
set(LIB_FILES
@@ -21,5 +22,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
if(PCL_FOUND)
- include_directories(${PCL_INCLUDE_DIRS})
+ target_include_directories(aronpclconverter
+ SYSTEM PUBLIC
+ ${PCL_INCLUDE_DIRS}
+ )
endif()
+
+add_library(RobotAPI::aron::converter::pcl ALIAS aronpclconverter)
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 0bdb8203c8083108012fb02e10eac9b7b7a2779e..673ea307e6b52105e4152e54c9c7b2f41f8ca48a 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -5,15 +5,12 @@ armarx_set_target("Library: ${LIB_NAME}")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
-if(Simox_FOUND)
- include_directories(${Simox_INCLUDE_DIR})
-endif()
+
+find_package(PCL QUIET COMPONENTS io common)
+armarx_build_if(PCL_FOUND "PCL not available")
set(LIBS
@@ -21,7 +18,10 @@ set(LIBS
ArmarXCore
RobotAPIInterfaces
cppgen
- ${Simox_LIBS}
+ Simox::SimoxUtility
+ # System libraries
+ Eigen3::Eigen
+ # PCLInterface
)
set(LIB_FILES
@@ -217,7 +217,17 @@ set(LIB_HEADERS
codegenerator/typeReader/xml/Reader.h
)
+
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+if(PCL_FOUND)
+target_include_directories("${LIB_NAME}"
+ SYSTEM PUBLIC
+ "${PCL_INCLUDE_DIRS}"
+)
+endif()
+
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index 1ba373de5aaaf56fb7062da4134792991b80ac47..cd14f9221dce3541d1f2f2e59720077a8caed805 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -36,7 +36,7 @@ namespace armarx::aron::cppcodegenerator::serializer
{"PointXYZRGB", {"pcl::PointXYZRGB", 32}},
{"PointXYZRGBL", {"pcl::PointXYZRGBL", 32}},
{"PointXYZRGBA", {"pcl::PointXYZRGBA", 32}},
- {"PointXYZHSV", {"pcl::PointXYZHSV", 32}},
+ {"PointXYZHSV", {"pcl::PointXYZHSV", 32}}
};
// constructors
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
index 77485c5fdb3b8ce814366b0b73492d249235ab5c..cf56e483356695bf2b54a747e36c4d758e7f1a56 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
@@ -29,20 +29,38 @@
// ArmarX
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+
+namespace fs = std::filesystem;
+
namespace armarx::aron::xmltypereader
{
- void Reader::parseFile(const std::string& filename)
+ void Reader::parseFile(const std::string& _filename)
{
- RapidXmlReaderPtr reader = RapidXmlReader::FromFile(filename);
- parse(reader);
+ std::string filename = _filename;
+ // Handle C++ style includes like "<path/to/file>".
+ if (!filename.empty() && filename.front() == '<' && filename.back() == '>')
+ {
+ filename = filename.substr(1, filename.size() - 2);
+ }
+ parseFile(std::filesystem::path(filename));
}
- void Reader::parseFile(const std::filesystem::path& file)
+ void Reader::parseFile(const std::filesystem::path& _file)
{
+ fs::path file = _file;
+ if (!file.empty() && file.is_relative())
+ {
+ if (std::optional<fs::path> resolved = detail::resolveRelativePackagePath(file))
+ {
+ file = resolved.value();
+ }
+ }
+
RapidXmlReaderPtr reader = RapidXmlReader::FromFile(file.string());
parse(reader);
}
@@ -185,4 +203,30 @@ namespace armarx::aron::xmltypereader
return typenavigator::IntEnumNavigator::DynamicCastAndCheck(factory.create(node, Path()));
}
+
+
+ std::optional<fs::path> detail::resolveRelativePackagePath(const fs::path& path)
+ {
+ const std::string package = *path.begin();
+ armarx::CMakePackageFinder finder(package);
+ if (finder.packageFound())
+ {
+ for (const std::string& includePath : finder.getIncludePathList())
+ {
+ fs::path absPath = includePath / path;
+ if (fs::is_regular_file(absPath))
+ {
+ return absPath;
+ }
+ }
+ return std::nullopt;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
}
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
index 0801465160e06ddbd0ebfc78b57d8b8bb97eafcd..e19083a9aa2718792c705ec41267a4e7f50d2cc8 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
@@ -24,8 +24,10 @@
#pragma once
// STD/STL
-#include <memory>
#include <filesystem>
+#include <memory>
+#include <optional>
+
// Base Class
#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h>
@@ -63,4 +65,10 @@ namespace armarx::aron::xmltypereader
private:
ReaderFactory factory;
};
+
+
+ namespace detail
+ {
+ std::optional<std::filesystem::path> resolveRelativePackagePath(const std::filesystem::path& path);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
index 0ed94c664655cbb4668ef4611a843e94fb1e1420..97942372a9c6be2667ff6639c13779fc734e933a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.cpp
@@ -239,4 +239,5 @@ case type::e##upperType: \
{
return 0;
}
-}
+
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
index 60d8361038c5d0aeba9f9d902f038dfd0386a96f..5e447b102d889f2ba91d95e23a60a89028cf1ff4 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h
@@ -24,16 +24,22 @@
#pragma once
// STD/STL
+#include <cstddef>
+#include <functional>
#include <memory>
#include <map>
+#include <numeric>
+#include <vector>
// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
namespace armarx::aron::datanavigator
{
class NDArrayNavigator;
- typedef std::shared_ptr<NDArrayNavigator> NDArrayNavigatorPtr;
+ using NDArrayNavigatorPtr = std::shared_ptr<NDArrayNavigator>;
class NDArrayNavigator :
virtual public Navigator
@@ -41,7 +47,6 @@ namespace armarx::aron::datanavigator
public:
using PointerType = NDArrayNavigatorPtr;
- public:
// constructors
NDArrayNavigator(const Path& path = Path());
NDArrayNavigator(const data::AronNDArrayPtr&, const Path& path = Path());
@@ -63,11 +68,31 @@ namespace armarx::aron::datanavigator
/// Return dimensions in a readable string such as "(2, 3, 4)".
static std::string DimensionsToString(const std::vector<int>& dimensions);
+ // TODO(fabian.reister): move this to VectorConverter?
+ template<typename T>
+ static NDArrayNavigatorPtr FromVector(const std::vector<T>& data)
+ {
+ NDArrayNavigatorPtr ndArr(new NDArrayNavigator);
+
+ ndArr->setDimensions({static_cast<int>(data.size()), sizeof(T)});
+ ndArr->setData(data);
+
+ return ndArr;
+ }
+
// public member functions
unsigned char* getData() const;
void setData(unsigned int, const unsigned char*);
+ // TODO(fabian.reister): move this to VectorConverter?
+ template<typename T>
+ void setData(const std::vector<T>& data)
+ {
+ using E = typename decltype(aron->data)::value_type;
+ setData(sizeof(T) * data.size(), reinterpret_cast < const E* >(data.data()));
+ }
+
std::vector<int> getDimensions() const;
void setDimensions(const std::vector<int>&);
void addDimension(int);
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 7eeffdbb49db4b321b77ed47fa1e58361d538094..de44d5a1d28a8148139265d1a18a6667c1c4a697 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -40,20 +40,22 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${TEST_NAME}
ARON_FILES
- xmls/HumanPoseTest.xml
+ # xmls/BaseClass.xml
+ # xmls/DerivedClassTest.xml
xmls/DictTest.xml
+ xmls/EigenMatrixTest.xml
+ xmls/EigenQuaternionTest.xml
+ xmls/EnumTest.xml
+ xmls/HumanPoseTest.xml
+ xmls/IVTCByteImageTest.xml
xmls/ListTest.xml
xmls/NaturalIKTest.xml
xmls/ObjectTest.xml
- xmls/PrimitiveTest.xml
- xmls/IVTCByteImageTest.xml
- xmls/EigenMatrixTest.xml
- xmls/EigenQuaternionTest.xml
xmls/OpenCVMatTest.xml
- xmls/PCLPointCloudTest.xml
- xmls/PositionTest.xml
xmls/OrientationTest.xml
+ xmls/PCLPointCloudTest.xml
xmls/PoseTest.xml
- xmls/EnumTest.xml
+ xmls/PositionTest.xml
+ xmls/PrimitiveTest.xml
#ENABLE_DEBUG_INFO
)