localization segment: implemented lookup

source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp

@@ -6,18 +6,20 @@
 #include <iterator>
 #include <sstream>
 
-#include <ArmarXCore/core/time/TimeUtil.h>
 #include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
 
 #include "RobotAPI/libraries/aron/common/aron_conversions.h"
 
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
 #include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
 #include <RobotAPI/libraries/armem/client/Writer.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
 #include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
 
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -26,12 +28,16 @@
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
 namespace armarx::armem::server::robot_state::localization
 {
 
-    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
-        iceMemory(memoryToIceAdapter),
-        memoryMutex(memoryMutex)
+    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+                     std::mutex& memoryMutex) :
+        iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
     {
         Logging::setTag("ArticulatedObjectInstanceSegment");
     }
@@ -40,17 +46,22 @@ namespace armarx::armem::server::robot_state::localization
 
     void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
     {
-        defs->optional(p.coreSegment, prefix + "seg.localization.CoreSegment", "Name of the object instance core segment.");
-        defs->optional(p.maxHistorySize, prefix + "seg.localization.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+        defs->optional(p.coreSegment,
+                       prefix + "seg.localization.CoreSegment",
+                       "Name of the object instance core segment.");
+        defs->optional(p.maxHistorySize,
+                       prefix + "seg.localization.MaxHistorySize",
+                       "Maximal size of object poses history (-1 for infinite).");
     }
 
     void Segment::init()
     {
         ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
 
-        ARMARX_INFO << "Adding core segment '" <<  p.coreSegment << "'";
+        ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
 
-        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::Transform::toInitialAronType());
+        coreSegment = &iceMemory.workingMemory->addCoreSegment(
+                          p.coreSegment, arondto::Transform::toInitialAronType());
         coreSegment->setMaxHistorySize(p.maxHistorySize);
     }
 
@@ -59,40 +70,42 @@ namespace armarx::armem::server::robot_state::localization
         // this->visu = std::make_unique<Visu>(arviz, *this);
     }
 
-    std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses() const
+    std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
     {
-        std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
 
-        for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
-        {
-            for (const auto& [name, entity] :  provSeg.entities())
-            {
-                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
-                // arondto::Transform aronTransform;
-                // aronTransform.fromAron(entityInstance);
+        using common::robot_state::localization::TransformHelper;
+        using common::robot_state::localization::TransformQuery;
 
+        std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
 
+        const auto& localizationCoreSegment =
+            iceMemory.workingMemory->getCoreSegment(p.coreSegment);
+        const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
 
-                // // Assemble trafo
+        for (const auto& robotName : knownRobots)
+        {
 
-                // if (not robotState)
-                // {
-                //     ARMARX_WARNING << "Could not convert entity instance to 'RobotState'";
-                //     continue;
-                // }
+            TransformQuery query{.header{
+                    .parentFrame = GlobalFrame,
+                    .frame       = "root", // TODO, FIXME
+                    .agent       = robotName,
+                    .timestamp   = timestamp.toMicroSeconds()
+                }};
 
-                // ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+            const auto result =
+                TransformHelper::lookupTransform(localizationCoreSegment, query);
 
+            if (not result)
+            {
                 // TODO
-
+                continue;
             }
 
-            robotGlobalPoses.emplace(robotName, Eigen::Affine3f::Identity());
-
+            robotGlobalPoses.emplace(robotName, result.transform.transform);
         }
 
-        ARMARX_INFO << deactivateSpam(10) <<  "Number of known robot poses: " << robotGlobalPoses.size();
+        ARMARX_INFO << deactivateSpam(10)
+                    << "Number of known robot poses: " << robotGlobalPoses.size();
 
         return robotGlobalPoses;
     }
@@ -101,7 +114,8 @@ namespace armarx::armem::server::robot_state::localization
     {
         const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
 
-        const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent);
+        const MemoryID providerID =
+            coreSegment->id().withProviderSegmentName(transform.header.agent);
         if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
         {
             coreSegment->addProviderSegment(providerID.providerSegmentName);
@@ -110,7 +124,8 @@ namespace armarx::armem::server::robot_state::localization
         Commit commit;
 
         EntityUpdate& update = commit.updates.emplace_back();
-        update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+        update.entityID      = providerID.withEntityName(transform.header.parentFrame + "," +
+                               transform.header.frame);
         update.timeArrived = update.timeCreated = update.timeSent = timestamp;
 
         arondto::Transform aronTransform;
@@ -149,8 +164,6 @@ namespace armarx::armem::server::robot_state::localization
     //     return objects;
     // }
 
-
-
     // void Segment::RemoteGui::setup(const Segment& data)
     // {
     //     using namespace armarx::RemoteGui::Client;
@@ -193,4 +206,4 @@ namespace armarx::armem::server::robot_state::localization
     //     }
     // }
 
-}  // namespace armarx::armem::server::robot_state::localization
+} // namespace armarx::armem::server::robot_state::localization

source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h

@@ -70,7 +70,7 @@ namespace armarx::armem::server::robot_state::localization
 
         void init();
 
-        std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses() const;
+        std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
 
         bool storeTransform(const armarx::armem::robot_state::Transform& transform);