memory client + transform reader + writer now fully implemented

source/RobotAPI/components/armem/client/TransformClient/MemoryConnector.cpp

@@ -4,37 +4,40 @@
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
-namespace armarx {
-
-MemoryConnector::MemoryConnector(ManagedIceObject &component) : component(component) {}
-
-void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr &def)
+namespace armarx
 {
-    ARMARX_INFO << "Memory connector: registerPropertyDefinitions";
-
-    const std::string prefix = getPropertyPrefix();
 
-    def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
-}
+    MemoryConnector::MemoryConnector(ManagedIceObject& component) : component(component) {}
 
-armem::data::WaitForMemoryResult MemoryConnector::useMemory(const std::string &memoryName)
-{
-    armem::data::WaitForMemoryInput input;
-    input.name = memoryName;
-
-    ARMARX_CHECK_NOT_NULL(memoryNameSystem);
+    void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
+    {
+        ARMARX_INFO << "Memory connector: registerPropertyDefinitions";
 
-    ARMARX_INFO << "Waiting for memory ...";
-    armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
+        const std::string prefix = getPropertyPrefix();
 
-    if (result.success) {
-        // Add dependency.
-        component.usingProxy(result.proxy->ice_getIdentity().name);
+        def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
     }
-    else {
-        ARMARX_WARNING << "Use memory: Failure";
+
+    armem::data::WaitForMemoryResult MemoryConnector::useMemory(const std::string& memoryName)
+    {
+        armem::data::WaitForMemoryInput input;
+        input.name = memoryName;
+
+        ARMARX_CHECK_NOT_NULL(memoryNameSystem);
+
+        ARMARX_INFO << "Waiting for memory ...";
+        armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
+
+        if (result.success)
+        {
+            // Add dependency.
+            component.usingProxy(result.proxy->ice_getIdentity().name);
+        }
+        else
+        {
+            ARMARX_WARNING << "Use memory: Failure";
+        }
+        return result;
     }
-    return result;
-}
 
 } // namespace armarx

source/RobotAPI/components/armem/client/TransformClient/MemoryConnector.h

@@ -27,32 +27,35 @@
 // TODO(fabian.reister): remove
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 
-namespace armarx {
+namespace armarx
+{
 
-class ManagedIceObject;
+    class ManagedIceObject;
 
-// TODO(fabian.reister): add
-// class PropertyDefinitionsPtr;
+    // TODO(fabian.reister): add
+    // class PropertyDefinitionsPtr;
 
-class MemoryConnector {
+    class MemoryConnector
+    {
 
-  public:
-    MemoryConnector(ManagedIceObject &component);
+    public:
+        MemoryConnector(ManagedIceObject& component);
 
-  protected:
-    armem::data::WaitForMemoryResult useMemory(const std::string &memoryName);
-    void registerPropertyDefinitions(PropertyDefinitionsPtr &def);
+    protected:
+        armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
+        void registerPropertyDefinitions(PropertyDefinitionsPtr& def);
 
-    void waitForMemory();
+        void waitForMemory();
 
-    virtual std::string getPropertyPrefix(){
-        return "mem.";
-    }
+        virtual std::string getPropertyPrefix()
+        {
+            return "mem.";
+        }
 
-  private:
-    ManagedIceObject &component;
+    private:
+        ManagedIceObject& component;
 
-    armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
-};
+        armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
+    };
 
 } // namespace armarx
\ No newline at end of file

source/RobotAPI/components/armem/client/TransformClient/TransformInterfaces.h

@@ -62,7 +62,7 @@ namespace armarx
             return status == Status::success;
         }
 
-        std::string errorMessage;
+        std::string errorMessage = "";
     };
 
     struct TransformQuery
@@ -78,8 +78,7 @@ namespace armarx
         virtual ~TransformInterface() = default;
 
         virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
-        virtual void connect() = 0;
-
+        virtual void connect()                                                = 0;
     };
 
     class TransformReaderInterface : virtual public TransformInterface
@@ -87,8 +86,9 @@ namespace armarx
     public:
         virtual ~TransformReaderInterface() = default;
 
-        virtual TransformResult
-        getGlobalPose(const std::string& agentName, const std::string& robotRootFrame, const std::int64_t& timestamp) const = 0;
+        virtual TransformResult getGlobalPose(const std::string& agentName,
+                                              const std::string& robotRootFrame,
+                                              const std::int64_t& timestamp) const = 0;
 
         virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
         // waitForTransform()

source/RobotAPI/components/armem/client/TransformClient/TransformReader.h

@@ -27,55 +27,57 @@
 #include "MemoryConnector.h"
 #include "TransformInterfaces.h"
 
-namespace armarx {
-/**
- * @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 TransformReader :
-    virtual public armarx::TransformReaderInterface,
-    virtual public armarx::MemoryConnector {
-  public:
+namespace armarx
+{
+    /**
+    * @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 TransformReader :
+        virtual public armarx::TransformReaderInterface,
+        virtual public armarx::MemoryConnector
+    {
+    public:
+        TransformReader(ManagedIceObject& component);
 
-    TransformReader(ManagedIceObject &component);
+        ~TransformReader() override;
 
-    ~TransformReader() override;
+        void connect() override;
 
-    void connect() override;
+        TransformResult getGlobalPose(const std::string& agentName,
+                                      const std::string& robotRootFrame,
+                                      const std::int64_t& timestamp) const override;
+        TransformResult lookupTransform(const TransformQuery& query) const override;
 
-    TransformResult getGlobalPose(
-        const std::string &agentName,
-        const std::string &robotRootFrame,
-        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
+        {
+            return "mem.read.";
+        }
 
-    std::string getPropertyPrefix() override{
-        return "mem.read.";
-    }
+    private:
+        std::vector<std::string> buildTransformChain(const armem::Memory& memory,
+                const TransformQuery& query) const;
 
-  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::vector<Eigen::Affine3f> obtainTransforms(const armem::Memory& memory,
+                const std::vector<std::string>& tfChain,
+                const std::string& agent, const std::int64_t& timestamp) const;
 
-    armem::client::Reader memoryReader;
+        armem::client::Reader memoryReader;
 
-    // Properties
-    struct Properties {
-        std::string memoryName = "RobotState";
-        std::string localizationMemoryName = "Localization";
-    } properties;
-};
+        // Properties
+        struct Properties
+        {
+            std::string memoryName             = "RobotState";
+            std::string localizationMemoryName = "Localization";
+        } properties;
+    };
 } // namespace armarx

source/RobotAPI/components/armem/client/TransformClient/TransformWriter.cpp

@@ -47,82 +47,89 @@
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
-namespace armarx {
+namespace armarx
+{
 
-TransformWriter::TransformWriter(ManagedIceObject &component) : MemoryConnector(component) {}
+    TransformWriter::TransformWriter(ManagedIceObject& component) : MemoryConnector(component) {}
 
-TransformWriter::~TransformWriter() = default;
+    TransformWriter::~TransformWriter() = default;
 
-void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
-{
-    ARMARX_INFO << "TransformWriter: registerPropertyDefinitions";
+    void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_INFO << "TransformWriter: registerPropertyDefinitions";
 
-    MemoryConnector::registerPropertyDefinitions(def);
+        MemoryConnector::registerPropertyDefinitions(def);
 
-    const std::string prefix = getPropertyPrefix();
+        const std::string prefix = getPropertyPrefix();
 
-    def->optional(
-        properties.localizationMemoryName,
-        prefix + "LocalizationMemoryName",
-        "Name of the localization memory core segment to use.");
+        def->optional(properties.localizationMemoryName,
+                      prefix + "LocalizationMemoryName",
+                      "Name of the localization memory core segment to use.");
 
-    def->optional(properties.memoryName, prefix + "MemoryName");
-}
+        def->optional(properties.memoryName, prefix + "MemoryName");
+    }
 
-void TransformWriter::connect()
-{
-    // Wait for the memory to become available and add it as dependency.
-    ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName << "' ...";
-    auto result = useMemory(properties.memoryName);
-    if (not result.success) {
-        ARMARX_ERROR << result.errorMessage;
-        return;
+    void TransformWriter::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "TransformWriter: 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);
     }
 
-    ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
+    bool TransformWriter::commitTransform(const Transform& transform)
+    {
+        std::lock_guard g{memoryWriterMutex};
 
-    memoryWriter.setWritingMemory(result.proxy);
-}
+        ARMARX_DEBUG << "Trying to create core segment + provider segment";
 
-bool TransformWriter::commitTransform(const Transform &transform)
-{
-    std::lock_guard g{memoryWriterMutex};
+        const auto result =
+            memoryWriter.addSegment(properties.localizationMemoryName, transform.header.agent);
 
-    ARMARX_INFO << "Trying to create core segment + provider segment";
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
 
-    const auto result =
-        memoryWriter.addSegment(properties.localizationMemoryName, transform.header.agent);
+            // TODO(fabian.reister): message
+            return false;
+        }
 
-    if (not result.success) {
-        ARMARX_ERROR << result.errorMessage;
+        const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
 
-        // TODO(fabian.reister): message
-        return false;
-    }
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID   = providerId.withEntityName(transform.header.parentFrame + "," +
+                                transform.header.frame).withTimestamp(timestamp);
 
-    const auto providerId = armem::MemoryID(result.segmentID);
-    const auto entityID =
-        providerId.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+        armem::EntityUpdate update;
+        update.entityID    = entityID;
+        update.timeCreated = armem::Time::now();
 
-    armem::EntityUpdate update;
-    update.entityID = entityID;
-    update.timeCreated = armem::Time::now();
+        aron::Transform aronTransform;
+        toAron(transform, aronTransform);
 
-    aron::Transform aronTransform;
-    toAron(transform, aronTransform);
+        update.instancesData = {aronTransform.toAron()};
+        update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
 
-    update.instancesData = {aronTransform.toAron()};
+        ARMARX_INFO << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
-    ARMARX_IMPORTANT << "Committing " << update;
-    armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+        ARMARX_INFO << updateResult;
 
-    ARMARX_INFO << updateResult;
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
 
-    if (not updateResult.success) {
-        ARMARX_ERROR << updateResult.errorMessage;
+        return updateResult.success;
     }
 
-    return updateResult.success;
-}
-
 } // namespace armarx

source/RobotAPI/components/armem/client/TransformClient/TransformWriter.h

@@ -28,52 +28,53 @@
 #include "MemoryConnector.h"
 #include "TransformInterfaces.h"
 
-namespace armarx {
-
-/**
- * @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 TransformWriter :
-    virtual public armarx::TransformWriterInterface,
-    virtual public armarx::MemoryConnector {
-  public:
-
-    TransformWriter(ManagedIceObject &component);
-
-    ~TransformWriter() override;
-
-    // TransformWriterInterface
-    /// to be called in Component::onConnectComponent
-    void connect() override;
-
-    /// to be called in Component::addPropertyDefinitions
-    void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def);
-
-
-    bool commitTransform(const Transform &transform) override;
-
-     std::string getPropertyPrefix() override{
-        return "mem.write.";
-    }
-
-
-  private:
-    armem::client::Writer memoryWriter;
-
-    // Properties
-    struct Properties {
-        std::string memoryName = "RobotState";
-        std::string localizationMemoryName = "Localization";
-    } properties;
-
-    std::mutex memoryWriterMutex;
-};
+namespace armarx
+{
+
+    /**
+    * @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 TransformWriter :
+        virtual public armarx::TransformWriterInterface,
+        virtual public armarx::MemoryConnector
+    {
+    public:
+        TransformWriter(ManagedIceObject& component);
+
+        ~TransformWriter() override;
+
+        // TransformWriterInterface
+        /// to be called in Component::onConnectComponent
+        void connect() override;
+
+        /// to be called in Component::addPropertyDefinitions
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+        bool commitTransform(const Transform& transform) override;
+
+        std::string getPropertyPrefix() override
+        {
+            return "mem.write.";
+        }
+
+    private:
+        armem::client::Writer memoryWriter;
+
+        // Properties
+        struct Properties
+        {
+            std::string memoryName             = "RobotState";
+            std::string localizationMemoryName = "Localization";
+        } properties;
+
+        std::mutex memoryWriterMutex;
+    };
 } // namespace armarx