objpose.cpp

Go to the documentation of this file.

#include "objpose.h"
 
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
 
 
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
 
 
void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
{
    aron::fromAron(dto.frameName, bo.frameName);
    aron::fromAron(dto.agentName, bo.agentName);
    aron::fromAron(dto.poseInFrame, bo.poseInFrame);
 
}
void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
{
    aron::toAron(dto.frameName, bo.frameName);
    aron::toAron(dto.agentName, bo.agentName);
    aron::toAron(dto.poseInFrame, bo.poseInFrame);
 
}
 
 
void armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
{
    aron::fromAron(dto.mean, bo.mean);
    aron::fromAron(dto.covariance, bo.covariance);
 
}
void armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
{
    aron::toAron(dto.mean, bo.mean);
    aron::toAron(dto.covariance, bo.covariance);
 
}
 
 
void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
{
    switch (dto.value)
    {
        case arondto::ObjectType::AnyObject:
            bo = ObjectType::AnyObject;
            return;
        case arondto::ObjectType::KnownObject:
            bo = ObjectType::KnownObject;
            return;
        case arondto::ObjectType::UnknownObject:
            bo = ObjectType::UnknownObject;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
{
    switch (bo)
    {
        case ObjectType::AnyObject:
            dto.value = arondto::ObjectType::AnyObject;
            return;
        case ObjectType::KnownObject:
            dto.value = arondto::ObjectType::KnownObject;
            return;
        case ObjectType::UnknownObject:
            dto.value = arondto::ObjectType::UnknownObject;
            return;
    }
    ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
}
 
 
void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
{
    aron::fromAron(dto.providerName, bo.providerName);
    fromAron(dto.objectType, bo.objectType);
    aron::fromAron(dto.isStatic, bo.isStatic);
    fromAron(dto.objectID, bo.objectID);
 
    aron::fromAron(dto.objectPoseRobot, bo.objectPoseRobot);
    aron::fromAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
    aron::fromAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
    aron::fromAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
    aron::fromAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
    aron::fromAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
    aron::fromAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
    aron::fromAron(dto.objectJointValues, bo.objectJointValues);
 
    aron::fromAron(dto.robotConfig, bo.robotConfig);
    aron::fromAron(dto.robotPose, bo.robotPose);
    aron::fromAron(dto.robotName, bo.robotName);
 
    if (dto.attachmentValid)
    {
        bo.attachment = ObjectAttachmentInfo();
        fromAron(dto.attachment, *bo.attachment);
    }
    else
    {
        bo.attachment = std::nullopt;
    }
    aron::fromAron(dto.confidence, bo.confidence);
    aron::fromAron(dto.timestamp, bo.timestamp);
 
 
    if (dto.localOOBBValid)
    {
        bo.localOOBB = simox::OrientedBoxf();
        fromAron(dto.localOOBB, *bo.localOOBB);
    }
    else
    {
        bo.localOOBB = std::nullopt;
    }
}
 
 
void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
{
    aron::toAron(dto.providerName, bo.providerName);
    toAron(dto.objectType, bo.objectType);
    aron::toAron(dto.isStatic, bo.isStatic);
    toAron(dto.objectID, bo.objectID);
 
    aron::toAron(dto.objectPoseRobot, bo.objectPoseRobot);
    aron::toAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
    aron::toAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
    aron::toAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
    aron::toAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
    aron::toAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
    aron::toAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
 
    aron::toAron(dto.objectJointValues, bo.objectJointValues);
 
    aron::toAron(dto.robotConfig, bo.robotConfig);
    aron::toAron(dto.robotPose, bo.robotPose);
    aron::toAron(dto.robotName, bo.robotName);
 
    if (bo.attachment)
    {
        dto.attachmentValid = true;
        toAron(dto.attachment, *bo.attachment);
    }
    else
    {
        dto.attachmentValid = false;
        toAron(dto.attachment, ObjectAttachmentInfo());
    }
 
    aron::toAron(dto.confidence, bo.confidence);
    aron::toAron(dto.timestamp, bo.timestamp);
 
    if (bo.localOOBB)
    {
        dto.localOOBBValid = true;
        toAron(dto.localOOBB, *bo.localOOBB);
    }
    else
    {
        dto.localOOBBValid = false;
        toAron(dto.localOOBB, simox::OrientedBoxf());
    }
}