ArticulatedObjectWriter: adding alternative implementation for storeArticulatedObject which should be more efficient

ArticulatedObjectWriter: adding alternative implementation for storeArticulatedObject which should be more efficient

/cc @ujrwq_arlt

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

@@ -14,11 +14,14 @@
 
 namespace armarx::armem::articulated_object
 {
-    armem::articulated_object::ArticulatedObject
+
+    // Slow version with filesystem access. It runs CMake for each project / environment variable.
+    static armem::articulated_object::ArticulatedObject
     convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
     {
         ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
+        // this is very inefficient. It runs CMake for each project
         const std::vector<std::string> packages =
             armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
         const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
@@ -51,6 +54,37 @@ namespace armarx::armem::articulated_object
             .timestamp = timestamp};
     }
 
+    // fast version without filesystem (CMake) access
+    static armem::articulated_object::ArticulatedObject
+    convertWithoutPackagePath(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
+    {
+        ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+        return armem::articulated_object::ArticulatedObject{
+            .description = {.name = obj.getType(), .xml = {}, .visualization = {}, .info = {}},
+            .instance = obj.getName(),
+            .config = {.timestamp = timestamp,
+                       .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
+                       .jointMap = obj.getJointValues(),
+                       .proprioception = std::nullopt},
+            .timestamp = timestamp};
+    }
+
+    bool
+    ArticulatedObjectWriter::storeArticulatedObjectWithObjectClass(
+        const VirtualRobot::RobotPtr& articulatedObject,
+        const armem::Time& timestamp,
+        const bool isStatic)
+    {
+
+        ARMARX_CHECK_NOT_NULL(articulatedObject);
+
+        armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
+            convert(*articulatedObject, timestamp);
+
+        return store(armemArticulatedObject, isStatic);
+    }
+
     bool
     ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
                                                     const armem::Time& timestamp,
@@ -60,8 +94,9 @@ namespace armarx::armem::articulated_object
         ARMARX_CHECK_NOT_NULL(articulatedObject);
 
         armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
-            convert(*articulatedObject, Time::Now());
+            convertWithoutPackagePath(*articulatedObject, timestamp);
 
-        return store(armemArticulatedObject, isStatic);
+        return storeInstance(armemArticulatedObject, isStatic);
     }
+
 } // namespace armarx::armem::articulated_object