framed.cpp

Go to the documentation of this file.

#include "framed.h"
 
#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
 
namespace armarx::aron::framed
{
    void
    fromAron(const arondto::FramedPosition& dto, armarx::FramedPosition& bo)
    {
        bo.frame = dto.header.frame;
        bo.agent = dto.header.agent;
        bo.x = dto.position.x();
        bo.y = dto.position.y();
        bo.z = dto.position.z();
    }
 
    void
    toAron(arondto::FramedPosition& dto, const armarx::FramedPosition& bo)
    {
        dto.header.frame = bo.frame;
        dto.header.agent = bo.agent;
        dto.position.x() = bo.x;
        dto.position.y() = bo.y;
        dto.position.z() = bo.z;
    }
 
    void
    fromAron(const arondto::FramedDirection& dto, armarx::FramedDirection& bo)
    {
        bo.frame = dto.header.frame;
        bo.agent = dto.header.agent;
        bo.x = dto.direction.x();
        bo.y = dto.direction.y();
        bo.z = dto.direction.z();
    }
 
    void
    toAron(arondto::FramedDirection& dto, const armarx::FramedDirection& bo)
    {
        dto.header.frame = bo.frame;
        dto.header.agent = bo.agent;
        dto.direction.x() = bo.x;
        dto.direction.y() = bo.y;
        dto.direction.z() = bo.z;
    }
 
    void
    fromAron(const arondto::FramedOrientation& dto, armarx::FramedOrientation& bo)
    {
        bo.frame = dto.header.frame;
        bo.agent = dto.header.agent;
        bo.qw = dto.orientation.w();
        bo.qx = dto.orientation.x();
        bo.qy = dto.orientation.y();
        bo.qz = dto.orientation.z();
    }
 
    void
    toAron(arondto::FramedOrientation& dto, const armarx::FramedOrientation& bo)
    {
        dto.header.frame = bo.frame;
        dto.header.agent = bo.agent;
        dto.orientation.w() = bo.qw;
        dto.orientation.x() = bo.qx;
        dto.orientation.y() = bo.qy;
        dto.orientation.z() = bo.qz;
    }
 
    void
    fromAron(const arondto::FramedPose& dto, armarx::FramedPose& bo)
    {
        bo.frame = dto.header.frame;
        bo.agent = dto.header.agent;
        auto vec = Eigen::Vector3f(dto.pose.block<3, 1>(0, 3));
        bo.position->x = vec.x();
        bo.position->y = vec.y();
        bo.position->z = vec.z();
        auto quat = Eigen::Quaternionf(dto.pose.block<3, 3>(0, 0));
        bo.orientation->qw = quat.w();
        bo.orientation->qx = quat.x();
        bo.orientation->qy = quat.y();
        bo.orientation->qz = quat.z();
    }
 
    void
    toAron(arondto::FramedPose& dto, const armarx::FramedPose& bo)
    {
        dto.header.frame = bo.frame;
        dto.header.agent = bo.agent;
        dto.pose = Eigen::Matrix4f::Identity();
        Eigen::Vector3f vec;
        vec.x() = bo.position->x;
        vec.y() = bo.position->y;
        vec.z() = bo.position->z;
        Eigen::Quaternionf quat;
        quat.w() = bo.orientation->qw;
        quat.x() = bo.orientation->qx;
        quat.y() = bo.orientation->qy;
        quat.z() = bo.orientation->qz;
        dto.pose.block<3, 1>(0, 3) = vec;
        dto.pose.block<3, 3>(0, 0) = quat.toRotationMatrix();
    }
} // namespace armarx::aron::framed