add synchronization of articulated objects

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp

@@ -25,10 +25,11 @@ namespace armarx::armem::articulated_object
 {
 
     VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(
-            const std::string& name,
-            const armem::Time& timestamp)
+            const std::string& typeName,
+            const armem::Time& timestamp,
+            const std::string& instanceName)
     {
-        const auto descriptions = queryDescriptions(armem::Time::Now());
+        const auto descriptions = queryDescriptions(timestamp);
 
         ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
@@ -36,15 +37,15 @@ namespace armarx::armem::articulated_object
                             descriptions.begin(),
                             descriptions.end(),
                             [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
-        { return desc.name == name; });
+        { return desc.name == typeName; });
 
         if (it == descriptions.end())
         {
-            ARMARX_WARNING << "Articulated object " << name << " not (yet) available";
+            ARMARX_WARNING << "Description for articulate object with type <" << typeName << "> not (yet) available!";
             return nullptr;
         }
 
-        ARMARX_DEBUG << "Object " << name << " available";
+        ARMARX_DEBUG << "Description for articulate object with type <" << typeName << "> available!";
 
         auto obj = VirtualRobot::RobotIO::loadRobot(
                     it->xml.toSystemPath(),
@@ -52,14 +53,29 @@ namespace armarx::armem::articulated_object
 
         if (not obj)
         {
-            ARMARX_WARNING << "Failed to load object: " << name;
+            ARMARX_WARNING << "Failed to load description for articulated object <" << typeName << ">!";
             return nullptr;
         }
 
-        obj->setName("");
+        obj->setName(instanceName);
         obj->setType(it->name);
 
         return obj;
     }
 
+    bool ArticulatedObjectReader::synchronizeArticulatedObject(VirtualRobot::Robot& object, const armem::Time& timestamp) {
+        const auto objectState = queryState(object.getType() + "/" + object.getName(), timestamp);
+        if (not objectState)
+        {
+            ARMARX_VERBOSE << deactivateSpam(5) << "Querying object state failed for object `" << object.getName() << "` "
+                           << "(type `" << object.getType() << "`)!";
+            return false;
+        }
+        
+        object.setJointValues(objectState->jointMap);
+        object.setGlobalPose(objectState->globalPose.matrix());
+
+        return true;
+    }
+
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h

@@ -11,7 +11,11 @@ namespace armarx::armem::articulated_object
         using Reader::Reader;
 
         VirtualRobot::RobotPtr getArticulatedObject(
-                const std::string& name,
-                const armem::Time& timestamp);
+                const std::string& typeName,
+                const armem::Time& timestamp,
+                const std::string& instanceName = "");
+        bool synchronizeArticulatedObject(
+            VirtualRobot::Robot& object,
+            const armem::Time& timestamp);
     };
 } // namespace armarx::armem::articulated_object