LocationUtils.cpp

Go to the documentation of this file.

#include "LocationUtils.h"
 
#include <optional>
#include <string>
#include <variant>
#include <vector>
 
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::core
{
 
    template <typename... Ts>
    struct overload : Ts...
    {
        using Ts::operator()...;
    };
 
    template <typename... Ts>
    overload(Ts...) -> overload<Ts...>; // deduction guide
 
    ResolvedLocation
    resolveArticulated(const FramedPose& framedPose,
                       const objpose::ObjectPose& pose,
                       const ObjectInfo info)
    {
        std::optional<PackageFileLocation> articulatedModel = info.articulatedSimoxXML();
        if (!articulatedModel.has_value())
        {
            return "object '" + info.idStr() +
                   "' does not have an articulated xml model but the specified frame isn't "
                   "root";
        }
 
        const auto modelPath = armarx::PackagePath(articulatedModel.value().package,
                                                   articulatedModel.value().relativePath);
        const auto robot = VirtualRobot::RobotIO::loadRobot(modelPath.toSystemPath(),
                                                            VirtualRobot::BaseIO::eStructure);
        if (robot == nullptr)
        {
            return "articulated model of '" + info.idStr() + "couldn't be loaded";
        }
        robot->setGlobalPose(pose.objectPoseGlobal);
        robot->setJointValues(pose.objectJointValues);
 
        const auto node = robot->getRobotNode(framedPose.frame);
        if (node == nullptr)
        {
            return "unknown node '" + framedPose.frame + "' of object '" + info.idStr() + "'";
        }
 
        const auto& global_T_node = node->getGlobalPose();
        const auto& node_T_location = framedPose.toEigen();
 
        return Pose(global_T_node * node_T_location);
    }
 
    /*!
     * \brief Resolve the location to it's global pose if it is defined globally or return the
     *        ObjectID it is defined relatively to
     */
    std::variant<Pose, ObjectID>
    resolveGlobal(const FramedPose& framedPose, const std::string& instanceID)
    {
        if (framedPose.agent.empty())
        {
            ARMARX_CHECK_EQUAL(framedPose.frame, armarx::GlobalFrame)
                << "frame is not set as global frame even though agent is empty";
            return core::Pose(framedPose.toEigen());
        }
 
        ObjectID id(framedPose.agent);
        if (id.instanceName().empty())
        {
            id.setInstanceName(instanceID);
        }
 
        return id;
    }
 
    ResolvedLocation
    resolveLocation(const objpose::ObjectPoseClient& client,
                    const FramedPose& framedPose,
                    const std::string& instanceID)
    {
        const auto res = resolveGlobal(framedPose, instanceID);
 
        return std::visit(
            overload{[&](const Pose& pose) -> ResolvedLocation { return pose; },
                     [&](const ObjectID& id) -> ResolvedLocation
                     {
                         const std::optional<objpose::ObjectPose> pose = client.fetchObjectPose(id);
                         if (!pose.has_value())
                         {
                             return "unknown object '" + id.str() + "'";
                         }
 
                         if (framedPose.frame == "root")
                         {
                             const auto& global_T_obj = pose.value().objectPoseGlobal;
                             const auto& obj_T_location = framedPose.toEigen();
 
                             return Pose(global_T_obj * obj_T_location);
                         }
                         else
                         {
                             const auto& finder = client.getObjectFinder();
                             std::optional<ObjectInfo> info = finder.findObject(id);
                             if (!info.has_value())
                             {
                                 return "can't find ObjectInfo for '" + id.str() + "'";
                             }
 
                             return resolveArticulated(framedPose, pose.value(), info.value());
                         }
                     }},
            res);
    }
 
    ResolvedLocation
    resolveLocation(const objpose::ObjectPoseMap& objects,
                    const std::vector<ObjectInfo>& info,
                    const FramedPose& framedPose,
                    const std::string& instanceID)
    {
        const auto res = resolveGlobal(framedPose, instanceID);
 
        return std::visit(
 
            overload{[&](const Pose& pose) -> ResolvedLocation { return pose; },
                     [&](const ObjectID& id) -> ResolvedLocation
                     {
                         const auto pose = objects.find(id);
                         if (pose == objects.end())
                         {
                             return "unknown object '" + id.str() + "'";
                         }
 
                         if (framedPose.frame == "root")
                         {
                             const auto& global_T_obj = pose->second.objectPoseGlobal;
                             const auto& obj_T_location = framedPose.toEigen();
 
                             return Pose(global_T_obj * obj_T_location);
                         }
                         else
                         {
                             for (const ObjectInfo& i : info)
                             {
                                 if (i.id().equalClass(id))
                                 {
                                     return resolveArticulated(framedPose, pose->second, i);
                                 }
                             }
 
                             return "Can't find ObjectInfo for '" + id.str() + "'";
                         }
                     }},
            res);
    }
 
 
} // namespace armarx::navigation::core