Refactor SimulationObjectPoseProvider (wip)

source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp

@@ -203,26 +203,18 @@ namespace armarx
 
                 for (auto req_it = activeRequests.begin(); req_it != activeRequests.end(); /* no increment */)
                 {
+                    // Remove invalid and outdated requests
                     if (not(req_it->second.isInvalid()) and now > req_it->second)
                     {
                         req_it = activeRequests.erase(req_it);
                         continue;
                     }
 
+                    // Add object poses for requested objects
                     if (const auto& object_it = knownObjects.find(req_it->first); object_it != knownObjects.end())
                     {
                         objpose::ProvidedObjectPose& pose = providedObjects.emplace_back();
-                        pose.providerName = getName();
-                        pose.objectType = objpose::ObjectType::KnownObject;
-                        pose.isStatic = object_it->second.staticObject;
-
-                        pose.objectID = armarx::ObjectID(object_it->second.name);
-
-                        pose.objectPose = armarx::fromIce(object_it->second.objectPoses.at(object_it->second.name));
-                        pose.objectPoseFrame = armarx::GlobalFrame;
-
-                        pose.confidence = 1;
-                        pose.timestamp = now;
+                        objectPoseFromVisuData(pose, object_it->second, now);
                     }
                     req_it++;
                 }
@@ -241,4 +233,21 @@ namespace armarx
         }
     }
 
+    void
+    SimulationObjectPoseProvider::objectPoseFromVisuData(objpose::ProvidedObjectPose& pose,
+                                                         const armarx::ObjectVisuData& visuData,
+                                                         const DateTime& time)
+    {
+        pose.providerName = getName();
+        pose.objectType = objpose::ObjectType::KnownObject;
+        pose.isStatic = visuData.staticObject;
+
+        pose.objectID = armarx::ObjectID(visuData.name);
+
+        pose.objectPose = armarx::fromIce(visuData.objectPoses.at(visuData.name));
+        pose.objectPoseFrame = armarx::GlobalFrame;
+
+        pose.confidence = 1;
+        pose.timestamp = time;
+    }
 } // namespace armarx

source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.h

@@ -28,6 +28,7 @@
 #include <ArmarXCore/core/time/DateTime.h>
 #include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
 #include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
 
 #include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
@@ -76,6 +77,10 @@ namespace armarx
 
         void poseProviderTaskRun();
 
+        void objectPoseFromVisuData(objpose::ProvidedObjectPose& pose,
+                                    const armarx::ObjectVisuData& visuData,
+                                    const DateTime& time);
+
         // Forward declare `Properties` if you used it before its defined.
         // struct Properties;