frame_conversions.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::armem::locations
{
    void
    toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
             const Eigen::Matrix4f& framedToGlobal)
    {
        if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
        {
            // No need to change to global
            framedLoc.framedPose.header.agent = ""; // sanity set
            return;
        }
 
        framedLoc.framedPose.pose = framedToGlobal * framedLoc.framedPose.pose;
        framedLoc.framedPose.header.frame = armarx::GlobalFrame;
        framedLoc.framedPose.header.agent = "";
    }
 
    void
    toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
             const armarx::objpose::ObjectPose& objPose)
    {
        if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
        {
            // No need to change to global
            framedLoc.framedPose.header.agent = ""; // sanity set
            return;
        }
 
        if (simox::alg::starts_with(objPose.objectID.str(), framedLoc.framedPose.header.agent))
        {
            ARMARX_CHECK(framedLoc.framedPose.header.frame ==
                         "root"); // TODO: Right now we only allow root
            toGlobal(framedLoc, objPose.objectPoseGlobal);
            return;
        }
 
        throw armarx::LocalException(
            "Could not convert a framedLocation because the used object is wrong!");
    }
 
} // namespace armarx::armem::locations