Update ObjectPoseObserver

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp

@@ -119,7 +119,7 @@ namespace armarx
     }
 
     void ObjectPoseObserver::reportObjectPoses(
-        const std::string& providerName, const objpose::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+        const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
     {
         ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
 
@@ -131,7 +131,7 @@ namespace armarx
             for (const auto& provided : providedPoses)
             {
                 objpose::ObjectPose& pose = objectPoses.emplace_back();
-                objpose::objectPoseFromProvidedPose(pose, provided, robot);
+                pose.fromProvidedPose(provided, robot);
             }
         }
 
@@ -143,24 +143,24 @@ namespace armarx
     }
 
 
-    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
+    objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
     {
         std::scoped_lock lock(dataMutex);
-        objpose::ObjectPoseSeq result;
+        objpose::data::ObjectPoseSeq result;
         for (const auto& [name, poses] : objectPoses)
         {
             for (const auto& pose : poses)
             {
-                result.push_back(pose);
+                result.push_back(pose.toIce());
             }
         }
         return result;
     }
 
-    objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+    objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
     {
         std::scoped_lock lock(dataMutex);
-        return objectPoses.at(providerName);
+        return objpose::toIce(objectPoses.at(providerName));
     }
 
 
@@ -298,11 +298,11 @@ namespace armarx
                 continue;
             }
 
-            PoseBasePtr pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+            Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
 
             viz::Object object = viz::Object(id.dataset + "/" + id.name)
                                  .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
-                                 .pose(armarx::PosePtr::dynamicCast(pose)->toEigen());
+                                 .pose(pose);
             if (visu.alpha < 1)
             {
                 object.overrideColor(simox::Color::white().with_alpha(visu.alpha));

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h

@@ -33,6 +33,7 @@
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
 
 #include "ObjectFinder.h"
+#include "ObjectPose.h"
 
 #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
 
@@ -82,13 +83,13 @@ namespace armarx
         // ObjectPoseTopic interface
     public:
         void reportProviderAvailable(const std::string& providerName, ICE_CURRENT_ARG) override;
-        void reportObjectPoses(const std::string& providerName, const objpose::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+        void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
 
         // ObjectPoseObserverInterface interface
     public:
 
-        objpose::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
-        objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+        objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+        objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
 
         void requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, ICE_CURRENT_ARG) override;