armem_robot_localization: cleanup

source/RobotAPI/libraries/CMakeLists.txt

@@ -20,6 +20,7 @@ add_subdirectory(natik)
 add_subdirectory(armem)
 add_subdirectory(armem_gui)
 add_subdirectory(armem_robot_localization)
+add_subdirectory(armem_robot_mapping)
 add_subdirectory(aron)
 
 add_subdirectory(NJointControllerGuiPluginUtility)

source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.cpp

@@ -11,10 +11,11 @@ namespace armarx
 
     void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
     {
-        ARMARX_INFO << "Memory connector: registerPropertyDefinitions";
-
         const std::string prefix = getPropertyPrefix();
 
+        ARMARX_INFO << "Memory connector: registerPropertyDefinitions with prefix " << prefix;
+
+
         def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
     }
 
@@ -40,4 +41,10 @@ namespace armarx
         return result;
     }
 
+    const std::string& MemoryConnector::getPropertyPrefix() const
+    {
+        return propertyPrefix;
+    }
+
+
 } // namespace armarx

source/RobotAPI/libraries/armem_robot_localization/MemoryConnector.h

@@ -48,15 +48,14 @@ namespace armarx
 
         void waitForMemory();
 
-        virtual std::string getPropertyPrefix()
-        {
-            return "mem.";
-        }
+        virtual const std::string& getPropertyPrefix() const;
 
     private:
         ManagedIceObject& component;
 
         armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
+
+        const std::string propertyPrefix;
     };
 
 } // namespace armarx
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp

@@ -65,7 +65,7 @@ namespace armarx
 
     void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
     {
-        ARMARX_INFO << "TransformReader: registerPropertyDefinitions";
+        ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
         MemoryConnector::registerPropertyDefinitions(def);
 
         const std::string prefix = getPropertyPrefix();
@@ -131,7 +131,7 @@ namespace armarx
     std::vector<std::string> TransformReader::buildTransformChain(const armem::Memory& memory,
             const TransformQuery& query) const
     {
-        ARMARX_INFO << "Building transform chain";
+        ARMARX_DEBUG << "Building transform chain";
 
         auto join = [](const std::string & parentFrame, const std::string & frame)
         {
@@ -229,17 +229,17 @@ namespace armarx
     {
         ARMARX_TRACE;
 
-        ARMARX_INFO << "Entering";
+        ARMARX_DEBUG << "Entering";
 
 
         ARMARX_CHECK(not queue.empty())
                 << "The queue has to contain at least two items to perform a lookup";
 
 
-        ARMARX_INFO << "Entering ... "
-                    << "Q front " << queue.front().header.timestamp << "  "
-                    << "Q back " << queue.back().header.timestamp << "  "
-                    << "query timestamp " << timestamp;
+        ARMARX_DEBUG << "Entering ... "
+                     << "Q front " << queue.front().header.timestamp << "  "
+                     << "Q back " << queue.back().header.timestamp << "  "
+                     << "query timestamp " << timestamp;
 
         // TODO(fabian.reister): sort queue.
 
@@ -247,26 +247,26 @@ namespace armarx
         ARMARX_CHECK(queue.back().header.timestamp > timestamp)
                 << "Cannot perform lookup into the future!";
 
-        // ARMARX_INFO << "Entering 1.5 " << queue.front().timestamp << "  " << timestamp;
+        // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << "  " << timestamp;
         ARMARX_CHECK(queue.front().header.timestamp < timestamp)
                 << "Cannot perform lookup. Timestamp too old";
         // => now we know that there is an element right after and before the timestamp within our queue
 
-        ARMARX_INFO << "Entering 2";
+        ARMARX_DEBUG << "Entering 2";
 
         const auto poseNextIt = findFirstElementAfter(queue, timestamp);
 
-        ARMARX_INFO << "it ari";
+        ARMARX_DEBUG << "it ari";
 
         const auto posePreIt = poseNextIt - 1;
 
-        ARMARX_INFO << "deref";
+        ARMARX_DEBUG << "deref";
 
         // the time fraction [0..1] of the lookup wrt to posePre and poseNext
         const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
                         (poseNextIt->header.timestamp - posePreIt->header.timestamp);
 
-        ARMARX_INFO << "interpolate";
+        ARMARX_DEBUG << "interpolate";
 
         return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
     }
@@ -300,13 +300,16 @@ namespace armarx
             return convertEntityToTransform(entity.getInstance(0));
         });
 
-        ARMARX_INFO << "obtaining transform";
+        ARMARX_DEBUG << "obtaining transform";
 
         if (transforms.size() > 1)
         {
-            ARMARX_INFO << "More than one snapshots received: " << transforms.size();
+            // TODO(fabian.reister): remove
+            return transforms.front().transform;
+
+            ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
             const auto p = interpolateTransform(transforms, timestamp);
-            ARMARX_INFO << "Done interpolating transform";
+            ARMARX_DEBUG << "Done interpolating transform";
             return p;
         }
 
@@ -314,12 +317,12 @@ namespace armarx
         // accept this to fail (will raise armem::error::MissingEntry)
         if (transforms.empty())
         {
-            ARMARX_INFO << "empty transform";
+            ARMARX_DEBUG << "empty transform";
 
             throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
         }
 
-        ARMARX_INFO << "single transform";
+        ARMARX_DEBUG << "single transform";
 
 
         return transforms.front().transform;
@@ -377,7 +380,7 @@ namespace armarx
 
         const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
 
-        ARMARX_INFO << "Looking up transform at timestamp " << timestamp;
+        ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
 
         // Query all entities from provider.
         armem::client::query::Builder qb;
@@ -395,7 +398,7 @@ namespace armarx
 
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
-        ARMARX_INFO << "Lookup result in reader: " << qResult;
+        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
         if (not qResult.success)
         {
@@ -437,7 +440,7 @@ namespace armarx
                                           Eigen::Affine3f::Identity(),
                                           std::multiplies<>());
 
-        ARMARX_INFO << "Found valid transform";
+        ARMARX_DEBUG << "Found valid transform";
 
         return {.transform = {.header = query.header, .transform = transform},
                 .status    = TransformResult::Status::success};

source/RobotAPI/libraries/armem_robot_localization/TransformReader.h

@@ -58,9 +58,9 @@ namespace armarx
 
         void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
 
-        std::string getPropertyPrefix() override
+        const std::string& getPropertyPrefix() const override
         {
-            return "mem.read.";
+            return propertyPrefix;
         }
 
     private:
@@ -82,5 +82,8 @@ namespace armarx
             std::string memoryName             = "RobotState";
             std::string localizationMemoryName = "Localization";
         } properties;
+
+
+        const std::string propertyPrefix = "mem.localization.read.";
     };
 } // namespace armarx

source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp

@@ -58,7 +58,7 @@ namespace armarx
 
     void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
     {
-        ARMARX_INFO << "TransformWriter: registerPropertyDefinitions";
+        ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
 
         MemoryConnector::registerPropertyDefinitions(def);
 
@@ -121,10 +121,10 @@ namespace armarx
         update.instancesData = {aronTransform.toAron()};
         update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
 
-        ARMARX_INFO << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+        ARMARX_DEBUG << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
-        ARMARX_INFO << updateResult;
+        ARMARX_DEBUG << updateResult;
 
         if (not updateResult.success)
         {
@@ -134,4 +134,10 @@ namespace armarx
         return updateResult.success;
     }
 
+    const std::string& TransformWriter::getPropertyPrefix() const
+    {
+        return propertyPrefix;
+    }
+
+
 } // namespace armarx

source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h

@@ -22,9 +22,10 @@
 
 #pragma once
 
-#include <RobotAPI/libraries/armem/client/Writer.h>
 #include <mutex>
 
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
 #include "MemoryConnector.h"
 #include "TransformInterfaces.h"
 
@@ -60,10 +61,7 @@ namespace armarx
 
         bool commitTransform(const Transform& transform) override;
 
-        std::string getPropertyPrefix() override
-        {
-            return "mem.write.";
-        }
+        const std::string& getPropertyPrefix() const override;
 
     private:
         armem::client::Writer memoryWriter;
@@ -76,5 +74,7 @@ namespace armarx
         } properties;
 
         std::mutex memoryWriterMutex;
+
+        const std::string propertyPrefix = "mem.localization.write.";
     };
 } // namespace armarx

source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h

@@ -8,7 +8,7 @@ namespace armarx
     namespace aron
     {
         struct Transform;
-    }
+    } // namespace aron
 
     void fromAron(const aron::Transform& aronTransform, Transform& transform);
     void toAron(const Transform& transform, aron::Transform& aronTransform);