ProvidedObjectPose.cpp

Go to the documentation of this file.

#include "ProvidedObjectPose.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
#include "ice_conversions.h"
 
 
namespace armarx::objpose
{
    ProvidedObjectPose::ProvidedObjectPose()
    {
    }
 
    ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
    {
        fromIce(ice);
    }
 
    void ProvidedObjectPose::fromIce(const data::ProvidedObjectPose& ice)
    {
        providerName = ice.providerName;
        objectType = ice.objectType;
        isStatic = ice.isStatic;
        armarx::fromIce(ice.objectID, objectID);
 
        armarx::fromIce(ice.objectPose, objectPose);
        objectPoseFrame = ice.objectPoseFrame;
        objpose::fromIce(ice.objectPoseGaussian, objectPoseGaussian);
 
        objectJointValues = ice.objectJointValues;
 
        confidence = ice.confidence;
        armarx::core::time::fromIce(ice.timestamp, timestamp);
 
        objpose::fromIce(ice.localOOBB, localOOBB);
    }
 
    data::ProvidedObjectPose ProvidedObjectPose::toIce() const
    {
        data::ProvidedObjectPose ice;
        toIce(ice);
        return ice;
    }
 
    void ProvidedObjectPose::toIce(data::ProvidedObjectPose& ice) const
    {
        ice.providerName = providerName;
        ice.objectType = objectType;
        ice.isStatic = isStatic;
        armarx::toIce(ice.objectID, objectID);
 
        armarx::toIce(ice.objectPose, objectPose);
        ice.objectPoseFrame = objectPoseFrame;
        objpose::toIce(ice.objectPoseGaussian, objectPoseGaussian);
 
        ice.objectJointValues = objectJointValues;
 
        ice.confidence = confidence;
        armarx::core::time::toIce(ice.timestamp, timestamp);
 
        objpose::toIce(ice.localOOBB, localOOBB);
    }
}
 
namespace armarx
{
 
    void objpose::fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose)
    {
        pose.fromIce(ice);
    }
    objpose::ProvidedObjectPose objpose::fromIce(const data::ProvidedObjectPose& ice)
    {
        return ProvidedObjectPose(ice);
    }
 
    void objpose::fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses)
    {
        poses.clear();
        poses.reserve(ice.size());
        for (const auto& i : ice)
        {
            poses.emplace_back(i);
        }
    }
 
    objpose::ProvidedObjectPoseSeq objpose::fromIce(const data::ProvidedObjectPoseSeq& ice)
    {
        ProvidedObjectPoseSeq poses;
        fromIce(ice, poses);
        return poses;
    }
 
 
    void objpose::toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose)
    {
        pose.toIce(ice);
    }
 
    objpose::data::ProvidedObjectPose objpose::toIce(const ProvidedObjectPose& pose)
    {
        return pose.toIce();
    }
 
 
    void objpose::toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses)
    {
        ice.clear();
        ice.reserve(poses.size());
        for (const auto& p : poses)
        {
            ice.emplace_back(p.toIce());
        }
    }
 
    objpose::data::ProvidedObjectPoseSeq objpose::toIce(const ProvidedObjectPoseSeq& poses)
    {
        data::ProvidedObjectPoseSeq ice;
        toIce(ice, poses);
        return ice;
    }
 
}