ObjectInfo.cpp

Go to the documentation of this file.

#include "ObjectInfo.h"
 
#include <filesystem>
#include <optional>
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
 
#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx
{
    namespace fs = std::filesystem;
 
    ObjectInfo::ObjectInfo(const std::string& packageName,
                           const ObjectInfo::path& packageDataDir,
                           const path& relObjectsPath,
                           const ObjectID& id) :
        _packageName(packageName),
        _absPackageDataDir(packageDataDir),
        _relObjectsPath(relObjectsPath),
        _id(id)
    {
    }
 
    ObjectInfo::ObjectInfo(const std::string& packageName,
                           const ObjectInfo::path& packageDataDir,
                           const path& relObjectsPath,
                           const std::string& dataset,
                           const std::string& className) :
        _packageName(packageName),
        _absPackageDataDir(packageDataDir),
        _relObjectsPath(relObjectsPath),
        _id(dataset, className)
    {
    }
 
    void
    ObjectInfo::setLogError(bool enabled)
    {
        this->_logError = enabled;
    }
 
    std::string
    ObjectInfo::package() const
    {
        return _packageName;
    }
 
    std::string
    ObjectInfo::dataset() const
    {
        return _id.dataset();
    }
 
    std::string
    ObjectInfo::className() const
    {
        return _id.className();
    }
 
    ObjectID
    ObjectInfo::id() const
    {
        return _id;
    }
 
    std::string
    ObjectInfo::idStr() const
    {
        return _id.str();
    }
 
    ObjectInfo::path
    ObjectInfo::objectDirectory(const bool fixDataPath) const
    {
        if (fixDataPath)
        {
            return _relObjectsPath / _id.dataset() / _id.className();
        }
 
        return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
    }
 
    PackageFileLocation
    ObjectInfo::file(const std::string& _extension,
                     const std::string& suffix,
                     const bool fixDataPath) const
    {
        std::string extension = _extension;
        if (extension.at(0) != '.')
        {
            extension = "." + extension;
        }
        std::string filename = _id.className() + suffix + extension;
 
        PackageFileLocation loc;
        loc.package = _packageName;
        loc.relativePath = objectDirectory(fixDataPath) / filename;
        loc.absolutePath = _absPackageDataDir / objectDirectory(false) / filename;
        return loc;
    }
 
    PackageFileLocation
    ObjectInfo::simoxXML() const
    {
        return file(".xml");
    }
 
    PackageFileLocation
    ObjectInfo::urdf() const
    {
        return file(".urdf");
    }
 
    PackageFileLocation
    ObjectInfo::sdf() const
    {
        return file(".sdf");
    }
 
    std::optional<PackageFileLocation>
    ObjectInfo::getModel() const
    {
        if (fs::is_regular_file(simoxXML().absolutePath))
        {
            return simoxXML();
        }
        else if (fs::is_regular_file(urdf().absolutePath))
        {
            return urdf();
        }
        else if (fs::is_regular_file(sdf().absolutePath))
        {
            return sdf();
        }
        else
        {
            return std::nullopt;
        }
    }
 
    PackageFileLocation
    ObjectInfo::articulatedSimoxXML() const
    {
        return file(".xml", "_articulated", true);
    }
 
    PackageFileLocation
    ObjectInfo::articulatedUrdf() const
    {
        return file(".urdf", "_articulated", true);
    }
 
    PackageFileLocation
    ObjectInfo::articulatedSdf() const
    {
        return file(".sdf", "_articulated");
    }
 
    std::optional<PackageFileLocation>
    ObjectInfo::getArticulatedModel() const
    {
        if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
        {
            return articulatedSimoxXML();
        }
        else if (fs::is_regular_file(articulatedUrdf().absolutePath))
        {
            return articulatedUrdf();
        }
        else if (fs::is_regular_file(articulatedSdf().absolutePath))
        {
            return articulatedSdf();
        }
        else
        {
            return std::nullopt;
        }
    }
 
    PackageFileLocation
    ObjectInfo::meshWrl() const
    {
        return file(".wrl");
    }
 
    PackageFileLocation
    ObjectInfo::wavefrontObj() const
    {
        return file(".obj");
    }
 
    PackageFileLocation
    ObjectInfo::boundingBoxJson() const
    {
        return file(".json", "_bb");
    }
 
    PackageFileLocation
    ObjectInfo::namesJson() const
    {
        return file(".json", "_names");
    }
 
    std::optional<simox::AxisAlignedBoundingBox>
    ObjectInfo::loadAABB() const
    {
        nlohmann::json j;
        try
        {
            ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
            j = nlohmann::read_json(boundingBoxJson().absolutePath);
        }
        catch (const std::exception& e)
        {
            if (_logError)
            {
                ARMARX_ERROR << e.what();
            }
            return std::nullopt;
        }
 
        try
        {
 
            nlohmann::json jaabb = j.at("aabb");
            auto center = jaabb.at("center").get<Eigen::Vector3f>();
            auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
            auto min = jaabb.at("min").get<Eigen::Vector3f>();
            auto max = jaabb.at("max").get<Eigen::Vector3f>();
 
            simox::AxisAlignedBoundingBox aabb(min, max);
 
            static const float prec = 1e-3f;
            ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec)
                << aabb.center().transpose() << "\n"
                << center.transpose() << "\n"
                << id();
            ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec)
                << aabb.extents().transpose() << "\n"
                << extents.transpose() << "\n"
                << id();
            ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec)
                << aabb.min().transpose() << "\n"
                << min.transpose() << "\n"
                << id();
            ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec)
                << aabb.max().transpose() << "\n"
                << max.transpose() << "\n"
                << id();
 
            return aabb;
        }
        catch (...)
        {
            ARMARX_WARNING << GetHandledExceptionString();
        }
 
        return std::nullopt;
    }
 
    std::optional<simox::OrientedBox<float>>
    ObjectInfo::loadOOBB() const
    {
        nlohmann::json j;
        try
        {
            ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
            j = nlohmann::read_json(boundingBoxJson().absolutePath);
        }
        catch (const std::exception& e)
        {
            if (_logError)
            {
                ARMARX_ERROR << e.what();
            }
            return std::nullopt;
        }
 
        try
        {
            nlohmann::json joobb = j.at("oobb");
            auto pos = joobb.at("pos").get<Eigen::Vector3f>();
            auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
            auto extents = joobb.at("extents").get<Eigen::Vector3f>();
 
            Eigen::Vector3f corner = pos - ori * extents / 2;
 
            simox::OrientedBox<float> oobb(
                corner, ori.col(0) * extents(0), ori.col(1) * extents(1), ori.col(2) * extents(2));
 
            static const float prec = 1e-3f;
            ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n"
                                                              << ori << "\n"
                                                              << id();
            // If the object is too large, the above precision will trigger a false positive.
            if (extents.squaredNorm() < 1e5f * 1e5f)
            {
                ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec)
                    << VAROUT(oobb.center().transpose()) << "\n"
                    << VAROUT(pos.transpose()) << "\n"
                    << VAROUT(extents.norm()) << "\n"
                    << VAROUT(id());
                ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec)
                    << VAROUT(oobb.dimensions().transpose()) << "\n"
                    << VAROUT(extents.transpose()) << "\n"
                    << VAROUT(extents.norm()) << "\n"
                    << VAROUT(id());
            }
            return oobb;
        }
        catch (...)
        {
            ARMARX_WARNING << GetHandledExceptionString();
        }
 
        return std::nullopt;
    }
 
    std::optional<std::vector<std::string>>
    ObjectInfo::loadRecognizedNames() const
    {
        return loadNames("recognized_name");
    }
 
    std::optional<std::vector<std::string>>
    ObjectInfo::loadSpokenNames() const
    {
        return loadNames("spoken_name");
    }
 
    std::optional<std::vector<std::string>>
    ObjectInfo::loadNames(const std::string& jsonKey) const
    {
        const PackageFileLocation file = namesJson();
        if (fs::is_regular_file(file.absolutePath))
        {
            nlohmann::json json;
 
            try
            {
                json = nlohmann::read_json(file.absolutePath);
            }
            catch (const nlohmann::json::exception& e)
            {
                if (_logError)
                {
                    ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n"
                                   << e.what();
                }
                return std::nullopt;
            }
            catch (const std::exception& e)
            {
                if (_logError)
                {
                    ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n"
                                   << e.what();
                }
                return std::nullopt;
            }
 
            return json.at(jsonKey).get<std::vector<std::string>>();
        }
        else
        {
            return std::nullopt;
        }
    }
 
    bool
    ObjectInfo::checkPaths() const
    {
        namespace fs = std::filesystem;
        bool result = true;
 
        if (!fs::is_regular_file(simoxXML().absolutePath))
        {
            if (_logError)
            {
                ARMARX_WARNING << "Expected simox object file for object " << *this << ": "
                               << simoxXML().absolutePath;
            }
            result = false;
        }
        if (false and not fs::is_regular_file(wavefrontObj().absolutePath))
        {
            if (_logError)
            {
                ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this
                               << ": " << wavefrontObj().absolutePath;
            }
            result = false;
        }
 
        return result;
    }
 
} // namespace armarx
 
std::ostream&
armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
    return os << rhs.id();
}