armem_robot_mapping: casting aron types to plain types

source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp

 #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

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

source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h

+/*
+ * 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(

source/RobotAPI/libraries/armem_robot_mapping/types.h

+/*
+ * 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