ObjectFinder.cpp

Go to the documentation of this file.

#include "ObjectFinder.h"
 
#include <filesystem>
#include <functional>
#include <map>
#include <optional>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/ObjectIO.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
#include "RobotAPI/libraries/armem_objects/types.h"
 
namespace armarx
{
    namespace fs = std::filesystem;
 
    ObjectFinder::ObjectFinder(const std::string& objectsPackageName,
                               const ObjectFinder::path& relObjectsDir) :
        packageName(objectsPackageName), relObjectsDir(relObjectsDir)
    {
        Logging::setTag("ObjectFinder");
    }
 
    void
    ObjectFinder::setPath(const std::string& path)
    {
        packageName = path;
        absPackageDataDir.clear();
    }
 
    std::string
    ObjectFinder::getPackageName() const
    {
        return packageName;
    }
 
    void
    ObjectFinder::init() const
    {
        if (absPackageDataDir.empty())
        {
            CMakePackageFinder packageFinder(packageName);
            absPackageDataDir = packageFinder.getDataDir();
            if (absPackageDataDir.empty())
            {
                ARMARX_WARNING << "Could not find package '" << packageName << "'.";
                // throw LocalException() << "Could not find package '" << packageName << "'.";
            }
            else
            {
                ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
 
                // make sure this data path is available => e.g. for findArticulatedObjects
                armarx::ArmarXDataPath::addDataPaths(std::vector<std::string>{absPackageDataDir});
            }
        }
    }
 
    bool
    ObjectFinder::isDatasetDirValid(const path& path) const
    {
        return std::filesystem::is_directory(path);
    }
 
    std::optional<ObjectInfo>
    ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
    {
        init();
        if (!_ready())
        {
            return std::nullopt;
        }
        if (!dataset.empty())
        {
            return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
        }
        // Search for object in datasets.
        const std::vector<std::string>& datasets = getDatasets();
        for (const std::string& ds : datasets)
        {
            if (fs::is_directory(_rootDirAbs() / ds / name))
            {
                return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, ds, name);
            }
        }
 
        std::stringstream ss;
        ss << "Did not find object '" << name << "' in any of these datasets:\n";
        for (const auto& ds : datasets)
        {
            ss << "- " << ds << "\n";
        }
        ss << "Objects root directory: " << _rootDirAbs();
        ARMARX_VERBOSE << ss.str();
 
        return std::nullopt;
    }
 
    std::optional<ObjectInfo>
    ObjectFinder::findObject(const std::string& nameOrID) const
    {
        return findObject(ObjectID(nameOrID));
    }
 
    std::optional<ObjectInfo>
    ObjectFinder::findObject(const ObjectID& id) const
    {
        return findObject(id.dataset(), id.className());
    }
 
    std::optional<ObjectInfo>
    ObjectFinder::findObject(const objpose::ObjectPose& obj) const
    {
        return findObject(obj.objectID);
    }
 
    std::vector<std::string>
    ObjectFinder::getDatasets() const
    {
        // init();  // Done by called methods.
        std::vector<std::string> datasets;
        for (const path& dir : getDatasetDirectories())
        {
            datasets.push_back(dir.filename());
        }
        return datasets;
    }
 
    std::vector<ObjectFinder::path>
    ObjectFinder::getDatasetDirectories() const
    {
        init();
        if (!_ready())
        {
            return {};
        }
        const bool local = false;
        std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
        std::vector<path> datasetDirs;
        for (const path& p : dirs)
        {
            if (isDatasetDirValid(p))
            {
                datasetDirs.push_back(p);
            }
        }
        return datasetDirs;
    }
 
    std::vector<ObjectInfo>
    ObjectFinder::findAllObjects(bool checkPaths) const
    {
        init();
        if (!_ready())
        {
            return {};
        }
        const bool local = true;
        std::vector<ObjectInfo> objects;
        for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
        {
            if (isDatasetDirValid(_rootDirAbs() / datasetDir))
            {
                std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
                for (const auto& o : dataset)
                {
                    objects.push_back(o);
                }
            }
        }
        return objects;
    }
 
    std::vector<armem::articulated_object::ArticulatedObjectDescription>
    ObjectFinder::findAllArticulatedObjects(bool checkPaths) const
    {
        init();
        if (!_ready())
        {
            return {};
        }
 
        const bool local = true;
 
        std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
        for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
        {
            if (isDatasetDirValid(_rootDirAbs() / datasetDir))
            {
                const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
                objects.insert(objects.end(), dataset.begin(), dataset.end());
            }
        }
        return objects;
    }
 
    std::map<std::string, std::vector<ObjectInfo>>
    ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
    {
        // init();  // Done by called methods.
        std::map<std::string, std::vector<ObjectInfo>> objects;
        for (const std::string& dataset : getDatasets())
        {
            objects[dataset] = findAllObjectsOfDataset(dataset, checkPaths);
        }
        return objects;
    }
 
    std::vector<ObjectInfo>
    ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
    {
        init();
        if (!_ready())
        {
            return {};
        }
        path datasetDir = _rootDirAbs() / dataset;
        if (!fs::is_directory(datasetDir))
        {
            ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
                           << datasetDir;
            return {};
        }
 
        std::vector<ObjectInfo> objects;
        const bool local = true;
        for (const path& dir : simox::fs::list_directory(datasetDir, local))
        {
            if (fs::is_directory(datasetDir / dir))
            {
                ObjectInfo object(packageName,
                                  absPackageDataDir,
                                  relObjectsDir,
                                  dataset,
                                  dir.filename());
                object.setLogError(logObjectDiscoveryError);
 
                if (!checkPaths || object.checkPaths())
                {
                    objects.push_back(object);
                }
            }
        }
        return objects;
    }
 
    std::unordered_map<std::string,
                       std::vector<armem::articulated_object::ArticulatedObjectDescription>>
    ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
    {
        init();
        if (!_ready())
        {
            return {};
        }
 
        const bool local = true;
 
        std::unordered_map<std::string,
                           std::vector<armem::articulated_object::ArticulatedObjectDescription>>
            datasets;
        for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
        {
            if (isDatasetDirValid(_rootDirAbs() / datasetDir))
            {
                const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
                datasets[datasetDir] = dataset;
            }
        }
        return datasets;
    }
 
    std::vector<armem::articulated_object::ArticulatedObjectDescription>
    ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset,
                                                     bool  /*checkPaths*/) const
    {
        init();
        if (!_ready())
        {
            return {};
        }
        path datasetDir = _rootDirAbs() / dataset;
        if (!isDatasetDirValid(datasetDir))
        {
            ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
                           << datasetDir;
            return {};
        }
 
        std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
        const bool local = true;
        for (const path& dir : simox::fs::list_directory(datasetDir, local))
        {
            if (fs::is_directory(datasetDir / dir))
            {
                ObjectInfo object(packageName,
                                  absPackageDataDir,
                                  relObjectsDir,
                                  dataset,
                                  dir.filename());
                std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
                if (modelFile.has_value())
                {
                    objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription{
                        .name = object.idStr(),
                        .xml = {modelFile->package, modelFile->relativePath},
                        .visualization = {},
                        .info = {}
                        // .dataset = dataset
                    });
                }
            }
        }
        return objects;
    }
 
    VirtualRobot::ManipulationObjectPtr
    ObjectFinder::loadManipulationObject(const std::optional<ObjectInfo>& ts,
                                         const VirtualRobot::ObjectIO::ObjectDescription loadMode)
    {
        if (!ts)
        {
            return nullptr;
        }
        const auto data = ts->simoxXML();
        ArmarXDataPath::FindPackageAndAddDataPath(data.package);
        std::string abs;
        if (!ArmarXDataPath::SearchReadableFile(data.relativePath, abs))
        {
            return nullptr;
        }
        return VirtualRobot::ObjectIO::loadManipulationObject(abs, loadMode);
    }
 
    VirtualRobot::ManipulationObjectPtr
    ObjectFinder::loadManipulationObject(
        const objpose::ObjectPose& obj,
        const VirtualRobot::ObjectIO::ObjectDescription loadMode) const
    {
        return loadManipulationObject(findObject(obj), loadMode);
    }
 
    VirtualRobot::ObstaclePtr
    ObjectFinder::loadObstacle(const std::optional<ObjectInfo>& ts)
    {
        if (!ts)
        {
            return nullptr;
        }
        const auto data = ts->simoxXML();
        ArmarXDataPath::FindPackageAndAddDataPath(data.package);
        std::string abs;
        if (!ArmarXDataPath::SearchReadableFile(data.relativePath, abs))
        {
            return nullptr;
        }
        return VirtualRobot::ObjectIO::loadObstacle(abs);
    }
 
    VirtualRobot::ObstaclePtr
    ObjectFinder::loadObstacle(const objpose::ObjectPose& obj) const
    {
        return loadObstacle(findObject(obj));
    }
 
    static std::vector<std::string>
    _loadNames(
        const ObjectFinder& finder,
        const ObjectID& objectID,
        const bool includeClassName,
        const std::function<std::optional<std::vector<std::string>>(const ObjectInfo&)> loadNamesFn)
    {
        std::vector<std::string> names;
        if (includeClassName)
        {
            names.push_back(simox::alg::to_lower(objectID.className()));
        }
        if (std::optional<ObjectInfo> info = finder.findObject(objectID))
        {
            if (std::optional<std::vector<std::string>> loadedNames = loadNamesFn(*info))
            {
                // Source: https://stackoverflow.com/a/201729
                names.insert(names.end(), loadedNames->begin(), loadedNames->end());
            }
        }
        return names;
    }
 
    std::vector<std::string>
    ObjectFinder::loadRecognizedNames(const ObjectID& objectID, bool includeClassName) const
    {
        return _loadNames(*this,
                          objectID,
                          includeClassName,
                          [](const ObjectInfo& info) { return info.loadRecognizedNames(); });
    }
 
    std::vector<std::string>
    ObjectFinder::loadSpokenNames(const ObjectID& objectID, bool includeClassName) const
    {
        return _loadNames(*this,
                          objectID,
                          includeClassName,
                          [](const ObjectInfo& info) { return info.loadSpokenNames(); });
    }
 
    void
    ObjectFinder::setLogObjectDiscoveryError(bool logEnabled)
    {
        logObjectDiscoveryError = logEnabled;
    }
 
    ObjectFinder::path
    ObjectFinder::_rootDirAbs() const
    {
        return absPackageDataDir / packageName / relObjectsDir;
    }
 
    ObjectFinder::path
    ObjectFinder::_rootDirRel() const
    {
        return packageName;
    }
 
    bool
    ObjectFinder::_ready() const
    {
        return !absPackageDataDir.empty();
    }
 
} // namespace armarx