armem_robot_mapping: new library to store mapping data (point clouds etc) in memory

source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt

+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
+)
+
+
+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)

source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.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    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/libraries/armem/client/Reader.h>
+
+#include "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 MappingDataReader :
+        virtual public armarx::MemoryConnector
+    {
+    public:
+        MappingDataReader(ManagedIceObject& component);
+
+        ~MappingDataReader() override;
+
+        void connect() override;
+
+
+        struct Query
+        {
+            std::string agentName;
+
+            TimeRange timeRange;
+
+            // if empty, all sensors will be queried
+            std::vector<std::string> sensorList;
+
+        };
+
+        struct Result
+        {
+            
+        };
+
+        Result queryData(const Query& query) const override;
+        
+        
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
+
+        const std::string& getPropertyPrefix() const override
+        {
+            return propertyPrefix;
+        }
+
+    private:
+        std::vector<std::string> buildTransformChain(const armem::Memory& memory,
+                const TransformQuery& 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;
+
+
+        armem::client::Reader memoryReader;
+
+        // Properties
+        struct Properties
+        {
+            std::string memoryName             = "RobotState";
+            std::string localizationMemoryName = "Localization";
+        } properties;
+
+
+        const std::string propertyPrefix = "mem.localization.read.";
+    };
+} // namespace armarx

source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp

+#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 = IceUtil::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();
+
+  aron::LaserScanStamped aronSensorData;
+  toAron(laserScan, timestamp, frame, agentName, aronSensorData);
+
+  update.instancesData = {aronSensorData.toAron()};
+  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

source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.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    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::MemoryConnector
+    {
+    public:
+        MappingDataWriter(ManagedIceObject& component);
+        ~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

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

+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+    </CodeIncludes>
+    <AronIncludes>
+    </AronIncludes>
+
+    <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 />
+            </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::aron::SensorHeader">
+            <ObjectChild key="agent">
+                <string/>
+            </ObjectChild>
+            <ObjectChild key="frame">
+                <string/>
+            </ObjectChild>
+            <ObjectChild key='timestamp'>
+                <Time />
+            </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>
+        </Object>
+
+    </GenerateTypes>
+</AronTypeDefinition> 
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp

+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <iterator>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.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,
+          .frame = aronSensorHeader.frame,
+          .timestamp = aronSensorHeader.timestamp};
+}
+
+void fromAron(const aron::LaserScanStamped &aronLaserScan,
+              LaserScanStamped &laserScan) {
+  laserScan.header = fromAron(aronLaserScan.header);
+  laserScan.data = fromAron(aronLaserScan.data);
+}
+
+void fromAron(const aron::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;
+  frame = header.frame;
+  agentName = header.agent;
+}
+
+
+/************ 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);
+}
+
+aron::SensorHeader toAron(const SensorHeader &sensorHeader) {
+  aron::SensorHeader aronSensorHeader;
+
+  aronSensorHeader.agent = sensorHeader.agent;
+  aronSensorHeader.frame = sensorHeader.frame;
+  aronSensorHeader.timestamp = sensorHeader.timestamp;
+
+  return aronSensorHeader;
+}
+
+void toAron(const LaserScanStamped &laserScanStamped,
+            aron::LaserScanStamped &aronLaserScanStamped) {
+  aronLaserScanStamped.header = toAron(laserScanStamped.header);
+  toAron(laserScanStamped.data, aronLaserScanStamped.data);
+}
+
+void toAron(const LaserScan &laserScan, 
+            const std::int64_t &timestamp,
+            const std::string &frame, 
+            const std::string &agentName,
+            aron::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

source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h

+#pragma once
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx {
+
+namespace aron {
+struct LaserScanStamped;
+} // namespace aron
+
+
+struct SensorHeader
+{
+    std::string agent;
+    std::string frame;
+    std::int64_t timestamp;
+};
+
+struct LaserScanStamped
+{
+    SensorHeader header;
+    LaserScan data;
+};
+
+
+void fromAron(
+    const aron::LaserScanStamped &aronLaserScan, 
+    LaserScan &laserScan,
+    std::int64_t &timestamp, 
+    std::string &frame,
+    std::string &agentName);
+
+void fromAron(const aron::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+void toAron(
+    const LaserScan &laserScan, 
+    const std::int64_t &timestamp,
+    const std::string &frame, 
+    const std::string& agentName,
+    aron::LaserScanStamped &aronLaserScan);
+
+} // namespace armarx
\ No newline at end of file