aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
 
namespace armarx::armem
{
 
    /* Transform */
 
    void
    fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
    {
        fromAron(dto.header, bo.header);
        aron::fromAron(dto.transform, bo.transform);
    }
 
    void
    toAron(arondto::Transform& dto, const robot_state::Transform& bo)
    {
        toAron(dto.header, bo.header);
        aron::toAron(dto.transform, bo.transform);
    }
 
    /* TransformHeader */
 
    void
    toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
    {
        aron::toAron(dto.parentFrame, bo.parentFrame);
        aron::toAron(dto.frame, bo.frame);
        aron::toAron(dto.agent, bo.agent);
        aron::toAron(dto.timestamp, bo.timestamp);
    }
 
    void
    fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
    {
        aron::fromAron(dto.parentFrame, bo.parentFrame);
        aron::fromAron(dto.frame, bo.frame);
        aron::fromAron(dto.agent, bo.agent);
        aron::fromAron(dto.timestamp, bo.timestamp);
    }
 
    void
    fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
    {
        bo.twist.linear.setZero();
        bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
 
        bo.twist.angular.setZero();
        bo.twist.angular.z() = dto.velocity.z(); // yaw
    }
 
    void
    toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
    {
        ARMARX_ERROR << "Not implemented yet.";
    }
 
    void
    fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
    {
        aron::fromAron(dto.force, bo.force);
        aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
        aron::fromAron(dto.torque, bo.torque);
        aron::fromAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
    }
 
    void
    toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
    {
        aron::toAron(dto.force, bo.force);
        aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
        aron::toAron(dto.torque, bo.torque);
        aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
    }
 
 
    void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
        bo = dto.array;
    }
 
    void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
        dto.array = bo;
    }
 
 
} // namespace armarx::armem