ArticulatedObjectWriter: fixing data path

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

@@ -3,44 +3,53 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
+#include <VirtualRobot/Robot.h>
+
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 
 namespace armarx::armem::articulated_object
 {
-    armem::articulated_object::ArticulatedObject convert(
-            const VirtualRobot::Robot& obj,
-            const armem::Time& timestamp)
+    armem::articulated_object::ArticulatedObject
+    convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
     {
         ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
-        // TODO(fabian.reister): remove "PriorKnowledgeData" below
+        const std::vector<std::string> packages =
+            armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+        const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
 
-        return armem::articulated_object::ArticulatedObject
+        // make sure that the relative path is without the 'package/' prefix
+        const std::string fileRelPath = [&obj, &package]() -> std::string
         {
-            .description = {
-                .name = obj.getType(),
-                .xml  = PackagePath(armarx::ArmarXDataPath::getProject(
-                {"PriorKnowledgeData"}, obj.getFilename()),
-                obj.getFilename())
-            },
-            .instance    = obj.getName(),
-            .config      = {
-                .timestamp  = timestamp,
-                .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
-                .jointMap   = obj.getJointValues()
-            },
-            .timestamp   = timestamp};
+            if (simox::alg::starts_with(obj.getFilename(), package))
+            {
+                // remove "package" + "/"
+                const std::string fixedFilename = obj.getFilename().substr(package.size() + 1, -1);
+                return fixedFilename;
+            }
+
+            return obj.getFilename();
+        }();
+
+        return armem::articulated_object::ArticulatedObject{
+            .description = {.name = obj.getType(),
+                            .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+                                                   {package}, fileRelPath),
+                                               obj.getFilename())},
+            .instance = obj.getName(),
+            .config = {.timestamp = timestamp,
+                       .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+                       .jointMap = obj.getJointValues()},
+            .timestamp = timestamp};
     }
 
     bool
-    ArticulatedObjectWriter::storeArticulatedObject(
-            const VirtualRobot::RobotPtr& articulatedObject,
-            const armem::Time& timestamp)
+    ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+                                                    const armem::Time& timestamp)
     {
 
         ARMARX_CHECK_NOT_NULL(articulatedObject);