diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 040d156c83ba4c321a7f451e3f5719fb1a7cfe60..7797a72db4a5764be267c9e561762614d3b6f4d2 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -1,6 +1,159 @@
#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 <IceUtil/Time.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
+ {
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ auto qbProviderSegments = qb
+ .coreSegments().withName(properties.mappingMemoryName)
+ .providerSegments().withName(query.agent);
+ // clang-format on
+
+ auto& qbEntities = [&]() -> armarx::armem::client::query::EntitySelector &
+ {
+ if (query.sensorList.empty())
+ {
+ return qbProviderSegments.entities().all();
+ }
+
+ return qbProviderSegments.entities().withNames(query.sensorList);
+ }();
+
+ qbEntities.snapshots().timeRange(
+ IceUtil::Time::microSeconds(query.timeRange.min),
+ 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
+ {
+ 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)
+ {
+ 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
+ {
+ LaserScanStamped laserScan;
+ fromAron(aronLaserScan, laserScan);
+ return laserScan;
+ });
+
+ 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 0bfb0e9fb7c7b788c399d54baafa7a0d5e03c096..b5307d0de1b8da173c90c5667d2fe91bdc5491a3 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -22,13 +22,27 @@
#pragma once
+#include "RobotAPI/libraries/armem_robot_mapping/types.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>
-namespace armarx::armem
+
+namespace armarx
{
+ class ManagedIceObject;
+}
+namespace armarx::armem
+{
+ struct TimeRange
+ {
+ std::int64_t min;
+ std::int64_t max;
+ };
/**
* @defgroup Component-ExampleClient ExampleClient
@@ -42,19 +56,19 @@ namespace armarx::armem
* Detailed description of class ExampleClient.
*/
class MappingDataReader :
- virtual public armarx::MemoryConnector
+ virtual public armarx::armem::MemoryConnector
{
public:
MappingDataReader(ManagedIceObject& component);
~MappingDataReader() override;
- void connect() override;
+ void connect();
struct Query
{
- std::string agentName;
+ std::string agent;
TimeRange timeRange;
@@ -66,28 +80,31 @@ namespace armarx::armem
struct Result
{
- };
+ std::vector<LaserScanStamped> laserScans;
- Result queryData(const Query& query) const override;
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+ std::string errorMessage;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
+ };
+
+ Result queryData(const Query& query) const;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
const std::string& getPropertyPrefix() const override
{
return propertyPrefix;
}
- private:
- std::vector<std::string> buildTransformChain(const armem::Memory& memory,
- const TransformQuery& query) const;
+ client::query::Builder buildQuery(const Query& query) const ;
- std::vector<Eigen::Affine3f> obtainTransforms(const armem::Memory& memory,
- const std::vector<std::string>& tfChain,
- const std::string& agent, const std::int64_t& timestamp) const;
-
- Eigen::Affine3f obtainTransform(const std::string& entityName, const armem::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
+ private:
armem::client::Reader memoryReader;
@@ -95,10 +112,11 @@ namespace armarx::armem
struct Properties
{
std::string memoryName = "RobotState";
- std::string localizationMemoryName = "Localization";
+ std::string mappingMemoryName = "Mapping";
} properties;
- const std::string propertyPrefix = "mem.localization.read.";
+ const std::string propertyPrefix = "mem.mapping.read.";
};
-} // namespace armarx
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
index a08b47d63f2e2de3556df2f7ff8c0d9513a80bc1..865e054b1fafca21516548e995133592a9991963 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
@@ -1,5 +1,30 @@
+/*
+ * 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 <RobotAPI/interface/units/LaserScannerUnit.h>
+
namespace armarx
{
@@ -9,7 +34,7 @@ namespace armarx
struct LaserScanStamped;
} // namespace aron
- struct LaserScan;
+ // struct LaserScan;
struct LaserScanStamped;
void fromAron(
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..f3bfa33b76a5201465ce0bcccc3d53af9d284c91
--- /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/>.
+ *
+ * @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 <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx
+{
+
+ struct SensorHeader
+ {
+ std::string agent;
+ std::string frame;
+ std::int64_t timestamp;
+ };
+
+ struct LaserScanStamped
+ {
+ SensorHeader header;
+ LaserScan data;
+ };
+
+} // namespace armarx