LocationLoader.cpp

Go to the documentation of this file.

#include "LocationLoader.h"
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/eigen/ice_conversions.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/json_conversions.h>
 
namespace armarx::priorknowledge::util
{
    FramedLocationPtr
    LocationLoader::LoadFramedLocation(const std::string& source,
                                       const std::string& locationName,
                                       const nlohmann::json& j)
    {
        ARMARX_CHECK(j.find("framedPose") != j.end());
 
        const auto framedPose = j.at("framedPose").get<std::map<std::string, nlohmann::json>>();
 
        std::string frame = framedPose.at("frame");
        std::string agent = framedPose.at("agent");
        Eigen::Matrix4f pose;
 
        // sanitize frame if not set
        if (frame.empty())
        {
            if (agent.empty())
            {
                ARMARX_WARNING << "Got empty frame for location '" + locationName +
                                      "'. Sanitizing it to '" + GlobalFrame + "'.";
                frame = GlobalFrame;
            }
            else
            {
                ARMARX_WARNING << "Got empty frame for location '" + locationName +
                                      "'. Sanitizing it to 'root' because "
                                      "an agent name '" +
                                      agent + "' was set.";
                frame = "root";
            }
        }
 
        if (frame != GlobalFrame && agent.empty())
        {
            ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
                                  "' for location '" + locationName +
                                  "'. This may lead to problems...";
            // TODO: What to do in that case?
        }
 
        // Utilize ice structure of eigen
        armarx::core::eigen::fromIce(
            pose,
            framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
 
        std::optional<Names> names;
        if (auto it = j.find("names"); it != j.end())
        {
            it->get_to(names.emplace());
        }
 
        FramedLocationPtr loc(new FramedLocation(LocationId(source, locationName),
                                                 LocationType::FRAMED_LOCATION,
                                                 frame,
                                                 agent,
                                                 pose,
                                                 names));
        return loc;
    }
 
    FramedBoxedLocationPtr
    LocationLoader::LoadBoxedLocation(const std::string& source,
                                      const std::string& locationName,
                                      const nlohmann::json& j)
    {
        ARMARX_CHECK(j.find("framedOrientedBox") != j.end());
 
        const auto framedOrientedBox =
            j.at("framedOrientedBox").get<std::map<std::string, nlohmann::json>>();
 
        std::string frame = framedOrientedBox.at("frame");
        std::string agent = framedOrientedBox.at("agent");
        Eigen::Matrix4f pose;
        Eigen::Vector3f extents;
 
        // sanitize frame if not set
        if (frame.empty())
        {
            if (agent.empty())
            {
                ARMARX_WARNING << "Got empty frame for location '" + locationName +
                                      "'. Sanitizing it to '" + GlobalFrame + "'.";
                frame = GlobalFrame;
            }
            else
            {
                ARMARX_WARNING << "Got empty frame for location '" + locationName +
                                      "'. Sanitizing it to 'root' because "
                                      "an agent name '" +
                                      agent + "' was set.";
                frame = "root";
            }
        }
 
        if (frame != GlobalFrame && agent.empty())
        {
            ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
                                  "' for location '" + locationName +
                                  "'. This may lead to problems...";
            // TODO: What to do in that case?
        }
 
        // Utilize ice structure of eigen
        armarx::core::eigen::fromIce(
            pose,
            framedOrientedBox.at("pose")
                .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
 
        // Utilize ice structure of eigen
        armarx::core::eigen::fromIce(
            extents,
            framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
 
        std::optional<Names> names;
        if (auto it = j.find("names"); it != j.end())
        {
            it->get_to(names.emplace());
        }
 
        FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
                                                           LocationType::FRAMED_BOXED_LOCATION,
                                                           frame,
                                                           agent,
                                                           pose,
                                                           extents,
                                                           names));
        return loc;
    }
 
    std::vector<LocationPtr>
    LocationLoader::LoadLocations(const std::string& source /* e.g. dataset/class or graph */,
                                  const nlohmann::json& js)
    {
        std::vector<LocationPtr> ret;
        if (not js.contains("locations"))
        {
            ARMARX_WARNING
                << "The locations file has the wrong structure. Missing key 'locations'.";
            return ret;
        }
 
        for (const auto& [locationName, j] :
             js.at("locations").get<std::map<std::string, nlohmann::json>>())
        {
            if (j.find("framedPose") != j.end())
            {
                ret.push_back(LocationLoader::LoadFramedLocation(source, locationName, j));
            }
            else if (j.find("framedOrientedBox") != j.end())
            {
                ret.push_back(LocationLoader::LoadBoxedLocation(source, locationName, j));
            }
            else
            {
                ARMARX_WARNING
                    << "The element '" << locationName
                    << "' has no 'framedPose' member or no 'framedOrientedBox' member. Skipping "
                       "this entity.";
                continue;
            }
        }
        return ret;
    }
} // namespace armarx::priorknowledge::util