ObjectInfo.cpp

Go to the documentation of this file.

#include "ObjectInfo.h"
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.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
        {
            j = nlohmann::read_json(boundingBoxJson().absolutePath);
        }
        catch (const std::exception& e)
        {
            if (_logError)
            {
                ARMARX_ERROR << e.what();
            }
            return std::nullopt;
        }
 
        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;
    }
 
    std::optional<simox::OrientedBox<float>> ObjectInfo::loadOOBB() const
    {
        nlohmann::json j;
        try
        {
            j = nlohmann::read_json(boundingBoxJson().absolutePath);
        }
        catch (const std::exception& e)
        {
            if (_logError)
            {
                ARMARX_ERROR << e.what();
            }
            return std::nullopt;
        }
 
        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;
    }
 
    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;
    }
 
}
 
 
std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
    return os << rhs.id();
}