armem robot mapping: performance improvements: laser scans are now ndarrays

source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp

 #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 <SimoxUtility/algorithm/get_map_keys_values.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/core/EntitySnapshot.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>
-#include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>
-#include "RobotAPI/libraries/armem_robot_localization/MemoryConnector.h"
+
+namespace armarx::armem
+{
+    class Entity;
+}  // namespace armarx
 
 namespace armarx::armem
 {
@@ -94,6 +119,59 @@ namespace armarx::armem
 
     }
 
+    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 aron::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<aron::LaserScanStamped>(entityInstance);
+
+                    if (o)
+                    {
+                        outV.push_back(convert(*o, entityInstance));
+                    }
+                }
+            }
+        }
+
+        return outV;
+    }
+
     MappingDataReader::Result
     MappingDataReader::queryData(const Query& query) const
     {
@@ -122,18 +200,10 @@ namespace armarx::armem
             .getProviderSegment(query.agent)
             .entities;
 
-        auto laserScans = transformAllOfType<aron::LaserScanStamped>(
-                              entities,
-                              [](const aron::LaserScanStamped & aronLaserScan) -> LaserScanStamped
-        {
-            LaserScanStamped laserScan;
-            fromAron(aronLaserScan, laserScan);
-            return laserScan;
-        });
-
+        const auto laserScans = asLaserScans(entities);
         const auto sensors = simox::alg::get_keys(entities);
 
-        return {.laserScans = std::move(laserScans),
+        return {.laserScans = laserScans,
                 .sensors = sensors,
                 .status = Result::Status::Success,
                 .errorMessage = ""};

source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h

@@ -13,7 +13,6 @@
  * 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
@@ -22,12 +21,18 @@
 
 #pragma once
 
-#include "RobotAPI/libraries/armem_robot_mapping/types.h"
-#include <RobotAPI/libraries/armem/client/Reader.h>
+#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

source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp

@@ -4,28 +4,30 @@
 #include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
 
 
-namespace armarx::armem {
+namespace armarx::armem
+{
 
-MappingDataWriter::MappingDataWriter(ManagedIceObject &component)
-    : MemoryConnector(component) {}
+    MappingDataWriter::MappingDataWriter(ManagedIceObject& component)
+        : MemoryConnector(component) {}
 
-MappingDataWriter::~MappingDataWriter() = default;
+    MappingDataWriter::~MappingDataWriter() = default;
 
-void MappingDataWriter::registerPropertyDefinitions(
-    armarx::PropertyDefinitionsPtr &def) {
-  ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
+    void MappingDataWriter::registerPropertyDefinitions(
+        armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
 
-  MemoryConnector::registerPropertyDefinitions(def);
+        MemoryConnector::registerPropertyDefinitions(def);
 
-  const std::string prefix = getPropertyPrefix();
+        const std::string prefix = getPropertyPrefix();
 
-  def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
-                "Name of the mapping memory core segment to use.");
+        def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+                      "Name of the mapping memory core segment to use.");
 
-  def->optional(properties.memoryName, prefix + "MemoryName");
-}
+        def->optional(properties.memoryName, prefix + "MemoryName");
+    }
 
- void MappingDataWriter::connect()
+    void MappingDataWriter::connect()
     {
         // Wait for the memory to become available and add it as dependency.
         ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
@@ -43,48 +45,55 @@ void MappingDataWriter::registerPropertyDefinitions(
     }
 
 
-bool MappingDataWriter::storeSensorData(const LaserScan &laserScan,
-                                        const std::string &frame,
-                                        const std::string &agentName,
-                                        const std::int64_t &timestamp) {
-  std::lock_guard g{memoryWriterMutex};
+    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);
+        const auto result =
+            memoryWriter.addSegment(properties.mappingMemoryName, agentName);
 
-  if (not result.success) {
-    ARMARX_ERROR << result.errorMessage;
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
 
-    // TODO(fabian.reister): message
-    return false;
-  }
+            // TODO(fabian.reister): message
+            return false;
+        }
 
-  const auto iceTimestamp = IceUtil::Time::microSeconds(timestamp);
+        const auto iceTimestamp = IceUtil::Time::microSeconds(timestamp);
 
-  const auto providerId = armem::MemoryID(result.segmentID);
-  const auto entityID =
-      providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+        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();
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+        update.timeCreated = armem::Time::now();
 
-  aron::LaserScanStamped aronSensorData;
-  toAron(laserScan, timestamp, frame, agentName, aronSensorData);
+        aron::LaserScanStamped aronSensorData;
+        // currently only sets the header
+        toAron(laserScan, timestamp, frame, agentName, aronSensorData);
 
-  update.instancesData = {aronSensorData.toAron()};
-  update.timeCreated = iceTimestamp;
+        auto dict = aronSensorData.toAron();
+        dict->addElement("scan", toAron(laserScan));
 
-  ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
-  armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+        update.instancesData = {dict};
+        update.timeCreated = iceTimestamp;
 
-  ARMARX_DEBUG << updateResult;
+        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
-  if (not updateResult.success) {
-    ARMARX_ERROR << updateResult.errorMessage;
-  }
+        ARMARX_DEBUG << updateResult;
 
-  return updateResult.success;
-}
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
+
+        return updateResult.success;
+    }
 
 } // namespace armarx::armem
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml

@@ -8,21 +8,6 @@
 
     <GenerateTypes>
 
-        <Object name='armarx::aron::LaserScanStep'>
-            <ObjectChild key='distance'>
-                <float />
-            </ObjectChild>
-            <ObjectChild key='angle'>
-                <float />
-            </ObjectChild>
-            <!--ObjectChild key='relativeTime'>
-                <float />
-            </ObjectChild-->
-            <!--ObjectChild key='intensity'>
-                <float />
-            </ObjectChild-->
-        </Object>
-
         <Object name='armarx::aron::LaserScannerInfo'>
             <ObjectChild key='device'>
                 <string />
@@ -53,21 +38,16 @@
             </ObjectChild>
         </Object>
 
-        <Object name="armarx::aron::LaserScan">
-            <ObjectChild key='scan'>
-                <List>
-                    <armarx::aron::LaserScanStep />
-                </List>
-            </ObjectChild>
-        </Object>
 
         <Object name='armarx::aron::LaserScanStamped'>
             <ObjectChild key="header">
                 <armarx::aron::SensorHeader />
             </ObjectChild>
+
+            <!-- 
             <ObjectChild key='data'>
-                <armarx::aron::LaserScan />
-            </ObjectChild>
+                <NdArray />
+            </ObjectChild> -->
         </Object>
 
     </GenerateTypes>

source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp

@@ -5,44 +5,17 @@
 
 #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 ************/
 
-
-    LaserScanStep fromAron(const aron::LaserScanStep& aronLaserScanStep)
-    {
-        LaserScanStep laserScanStep;
-
-        laserScanStep.angle = aronLaserScanStep.angle;
-        laserScanStep.distance = aronLaserScanStep.distance;
-
-        return laserScanStep;
-    }
-
-    template <typename T> auto fromAron(const std::vector<T>& v)
-    {
-        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;
-    }
-
-    auto fromAron(const aron::LaserScan& aronLaserScan)
-    {
-        return fromAron(aronLaserScan.scan);
-    }
-
     SensorHeader fromAron(const aron::SensorHeader& aronSensorHeader)
     {
         return {.agent = aronSensorHeader.agent,
@@ -54,7 +27,7 @@ namespace armarx
                   LaserScanStamped& laserScan)
     {
         laserScan.header = fromAron(aronLaserScan.header);
-        laserScan.data = fromAron(aronLaserScan.data);
+        // laserScan.data = fromAron(aronLaserScan.data);
     }
 
     void fromAron(const aron::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
@@ -63,7 +36,7 @@ namespace armarx
     {
         const auto header = fromAron(aronLaserScan.header);
 
-        laserScan = fromAron(aronLaserScan.data);
+        // laserScan = fromAron(aronLaserScan.data);
 
         timestamp = header.timestamp;
         frame = header.frame;
@@ -74,34 +47,10 @@ namespace armarx
     /************ toAron ************/
 
 
-    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;
-    }
-
-    aron::LaserScanStep toAron(const LaserScanStep& laserScanStep)
-    {
-        aron::LaserScanStep aronLaserScan;
-
-        aronLaserScan.angle = laserScanStep.angle;
-        aronLaserScan.distance = laserScanStep.distance;
-
-        return aronLaserScan;
-    }
-
-    auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
-    {
-        aronLaserScan.scan = toAron(laserScan);
-    }
+    // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+    // {
+    //     aronLaserScan.scan = toAron(laserScan);
+    // }
 
     aron::SensorHeader toAron(const SensorHeader& sensorHeader)
     {
@@ -118,7 +67,7 @@ namespace armarx
                 aron::LaserScanStamped& aronLaserScanStamped)
     {
         aronLaserScanStamped.header = toAron(laserScanStamped.header);
-        toAron(laserScanStamped.data, aronLaserScanStamped.data);
+        // toAron(laserScanStamped.data, aronLaserScanStamped.data);
     }
 
     void toAron(const LaserScan& laserScan,

source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h

@@ -13,7 +13,6 @@
  * 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
@@ -24,7 +23,8 @@
 #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>
 
 namespace armarx
 {
@@ -44,6 +44,13 @@ namespace armarx
         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 aron::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
 
     void toAron(
@@ -53,4 +60,10 @@ namespace armarx
         const std::string& agentName,
         aron::LaserScanStamped& aronLaserScan);
 
+    inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
+    {
+        return aron::datanavigator::NDArrayNavigator::FromVector(laserScan);
+    }
+
+
 } // namespace armarx
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_mapping/types.h

@@ -13,7 +13,6 @@
  * 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