Move functions to ice_conversions

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp

@@ -31,18 +31,12 @@
 #include <RobotAPI/libraries/core/Pose.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 
+#include "ice_conversions.h"
+
 
 namespace armarx
 {
 
-    const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames =
-    {
-        { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
-        { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
-        { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
-    };
-
-
     ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
         armarx::ObserverPropertyDefinitions(prefix)
     {
@@ -121,7 +115,7 @@ namespace armarx
         {
             offerChannel(providerName, "Channel of provider '" + providerName + "'.");
         }
-        offerOrUpdateDataField(providerName, "objectType", ObjectTypeEnumNames.to_name(info.objectType), "");
+        offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
     }
 
     void ObjectPoseObserver::reportObjectPoses(
@@ -137,28 +131,7 @@ namespace armarx
             for (const auto& provided : providedPoses)
             {
                 objpose::ObjectPose& pose = objectPoses.emplace_back();
-                pose.objectType = provided.objectType;
-                pose.objectID = provided.objectID;
-
-                pose.objectPoseOriginal = provided.objectPose;
-                pose.objectPoseOriginalFrame = provided.objectPoseFrame;
-
-                armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose);
-                ARMARX_CHECK_NOT_NULL(p);
-                {
-                    armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName());
-                    framed.changeFrame(robot, robot->getRootNode()->getName());
-                    pose.objectPoseRobot = new Pose(framed.toEigen());
-                    framed.changeToGlobal(robot);
-                    pose.objectPoseGlobal = new Pose(framed.toEigen());
-
-                    pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
-                    pose.robotPose = new Pose(robot->getGlobalPose());
-                }
-
-                pose.confidence = provided.confidence;
-                pose.timestampMicroSeconds = provided.timestampMicroSeconds;
-                pose.providerName = provided.providerName;
+                objpose::objectPoseFromProvidedPose(pose, provided, robot);
             }
         }
 

source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h

@@ -33,7 +33,6 @@
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
 
 #include "ObjectFinder.h"
-#include "ice_conversions.h"
 
 #define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
 

source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp

 #include "ice_conversions.h"
 
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
 
 namespace armarx
 {
@@ -9,5 +16,50 @@ namespace armarx
         return os << "'" << id.dataset << "/" << id.name << "'";
     }
 
+    const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
+    {
+        { objpose::ObjectTypeEnum::AnyObject, "AnyObject" },
+        { objpose::ObjectTypeEnum::KnownObject, "KnownObject" },
+        { objpose::ObjectTypeEnum::UnknownObject, "UnknownObject" }
+    };
+
+    void objpose::objectPoseFromProvidedPose(
+        ObjectPose& pose,
+        const ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
+    {
+        pose.providerName = provided.providerName;
+        pose.objectType = provided.objectType;
+
+        pose.objectID = provided.objectID;
+
+        pose.objectPoseOriginal = provided.objectPose;
+        pose.objectPoseOriginalFrame = provided.objectPoseFrame;
+
+        armarx::PosePtr p = armarx::PosePtr::dynamicCast(provided.objectPose);
+        ARMARX_CHECK_NOT_NULL(p);
+        armarx::FramedPose framed(p->toEigen(), provided.objectPoseFrame, robot->getName());
+        framed.changeFrame(robot, robot->getRootNode()->getName());
+        pose.objectPoseRobot = new Pose(framed.toEigen());
+        framed.changeToGlobal(robot);
+        pose.objectPoseGlobal = new Pose(framed.toEigen());
+
+        pose.robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
+        pose.robotPose = new Pose(robot->getGlobalPose());
+
+        pose.confidence = provided.confidence;
+        pose.timestampMicroSeconds = provided.timestampMicroSeconds;
+
+        pose.localOOBB = provided.localOOBB;
+    }
+
+
+    objpose::ObjectPose objpose::objectPoseFromProvidedPose(
+        const ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
+    {
+        ObjectPose pose;
+        objectPoseFromProvidedPose(pose, provided, robot);
+        return pose;
+    }
 }
 
+

source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h

 #pragma once
 
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+#include <VirtualRobot/VirtualRobot.h>
+
 #include <RobotAPI/interface/objectpose/types.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
 
 
 namespace armarx::objpose
@@ -9,5 +12,10 @@ namespace armarx::objpose
 
     std::ostream& operator<<(std::ostream& os, const ObjectID& id);
 
+    extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
+
+
+    void objectPoseFromProvidedPose(ObjectPose& pose, const ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
+    ObjectPose objectPoseFromProvidedPose(const ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot);
 
 }