aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.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::robot_state::localization
{
 
    /* Transform */
 
    void
    fromAron(const arondto::Transform& dto, Transform& bo)
    {
        fromAron(dto.header, bo.header);
        aron::fromAron(dto.transform, bo.transform);
    }
 
    void
    toAron(arondto::Transform& dto, const Transform& bo)
    {
        toAron(dto.header, bo.header);
        aron::toAron(dto.transform, bo.transform);
    }
 
    /* TransformHeader */
 
    void
    toAron(arondto::TransformHeader& dto, const 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, 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);
    }
} // namespace armarx::armem::robot_state::localization
 
namespace armarx::armem::robot_state
{
    void
    fromAron(const armarx::armem::prop::arondto::Platform& dto, robot_state::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_state::PlatformState& bo)
    {
        ARMARX_ERROR << "Not implemented yet.";
    }
} // namespace armarx::armem::robot_state
 
namespace armarx::armem::robot_state::proprioception
{
    void
    fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, 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 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);
    }
} // namespace armarx::armem::robot_state::proprioception
 
namespace armarx::armem::robot_state::exteroception
{
    void
    fromAron(const armarx::armem::exteroception::arondto::ToF& dto, ToF& bo)
    {
        bo.depth = dto.depth;
        bo.sigma = dto.sigma;
        bo.targetDetected = dto.targetDetected;
    }
 
    void
    toAron(armarx::armem::exteroception::arondto::ToF& dto, const ToF& bo)
    {
        dto.depth = bo.depth;
        dto.sigma = bo.sigma;
        dto.targetDetected = bo.targetDetected;
    }
 
} // namespace armarx::armem::robot_state::exteroception
 
namespace armarx::armem::robot_state::description
{
    void
    fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
    {
        aron::fromAron(dto.name, bo.name);
        fromAron(dto.xml, bo.xml);
        aron::fromAron(dto.visualization, bo.visualization);
        aron::fromAron(dto.info, bo.info);
        aron::fromAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
    }
 
    void
    toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
    {
        aron::toAron(dto.name, bo.name);
        toAron(dto.xml, bo.xml);
        aron::toAron(dto.visualization, bo.visualization);
        aron::toAron(dto.info, bo.info);
        aron::toAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
    }
} // namespace armarx::armem::robot_state::description
 
namespace armarx::armem::robot_state
{
 
    /* Robot */
 
    void
    fromAron(const arondto::Robot& dto, Robot& bo)
    {
        // fromAron(dto.description, bo.description);
        fromAron(dto.state, bo.config);
    }
 
    void
    toAron(arondto::Robot& dto, const Robot& bo)
    {
        // toAron(dto.description, bo.description);
        toAron(dto.state, bo.config);
    }
 
    /* RobotDescription */
 
    /* RobotState */
 
    void
    fromAron(const arondto::RobotState& dto, RobotState& bo)
    {
        aron::fromAron(dto.timestamp, bo.timestamp);
        aron::fromAron(dto.globalPose, bo.globalPose.matrix());
        aron::fromAron(dto.jointMap, bo.jointMap);
    }
 
    void
    toAron(arondto::RobotState& dto, const RobotState& bo)
    {
        aron::toAron(dto.timestamp, bo.timestamp);
        aron::toAron(dto.globalPose, bo.globalPose.matrix());
        aron::toAron(dto.jointMap, bo.jointMap);
    }
 
} // namespace armarx::armem::robot_state