From e3713eb4d95680f87e9f812beb6fee159be7fff1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 May 2023 12:11:49 +0200
Subject: [PATCH] fix: articulated objects in memory

---
 .../ArticulatedObjectLocalizerExample.cpp             | 10 ++++++++--
 .../client/articulated_object/Writer.cpp              |  1 +
 .../libraries/armem_objects/server/instance/Visu.cpp  | 11 +++++++++++
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 77943d887..f0e307742 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,6 +32,7 @@
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/XML/RobotIO.h>
 
+#include "ArmarXCore/core/time/Clock.h"
 #include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
@@ -135,7 +136,7 @@ namespace armarx::articulated_object
         }
 
         auto obj =
-            VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+            VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
                                              VirtualRobot::RobotIO::eStructure);
 
         if (not obj)
@@ -163,6 +164,11 @@ namespace armarx::articulated_object
             }
         }
 
+        const auto state = articulatedObjectReaderPlugin->get().queryState(articulatedObject->getType() + "/" + articulatedObject->getName(), Clock::Now());
+        
+        ARMARX_CHECK(state.has_value());
+        articulatedObject->setGlobalPose(state->globalPose.matrix());
+
         ARMARX_DEBUG << "Reporting articulated objects";
 
         const armem::Time now = armem::Time::Now();
@@ -179,7 +185,7 @@ namespace armarx::articulated_object
             jointValue = node->unscaleJointValue(k, 0, 1);
         }
 
-        articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+        // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
         articulatedObject->setJointValues(jointValues);
 
         auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 80c3168c5..18d7aa50a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -231,6 +231,7 @@ namespace armarx::armem::articulated_object
         cs.dataset = objectId.dataset();
 
         objectInstance.pose.objectID = cs;
+        objectInstance.pose.providerName = properties.providerName;
 
         update.instancesData = {objectInstance.toAron()};
         update.referencedTime   = timestamp;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 514096731..b9abf459e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -70,6 +70,11 @@ namespace armarx::armem::server::obj::instance
         std::vector<viz::Layer> layers;
         for (const auto& [name, poses] : objectPoses)
         {
+            if(name.empty())
+            {
+                continue;
+            }
+
             auto poseHistoryMap = poseHistories.find(name);
             if (poseHistoryMap != poseHistories.end())
             {
@@ -92,6 +97,12 @@ namespace armarx::armem::server::obj::instance
         std::map<std::string, viz::Layer> stage;
         for (size_t i = 0; i < objectPoses.size(); ++i)
         {
+            if(objectPoses.at(i).providerName.empty())
+            {
+                ARMARX_INFO << "Object pose provider not set!";
+                continue;
+            }
+
             visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
                                 objectPoses.at(i),
                                 poseHistories.at(i),
-- 
GitLab