Add ObjectFinder

source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt

@@ -10,9 +10,13 @@ set(COMPONENT_LIBS
 
 set(SOURCES
     ObjectPoseObserver.cpp
+
+    ObjectFinder.cpp
 )
 set(HEADERS
     ObjectPoseObserver.h
+
+    ObjectFinder.h
 )
 
 

source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp

+#include "ObjectFinder.h"
+
+#include <SimoxUtility/filesystem/list_directory.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
+
+namespace armarx
+{
+    namespace fs = std::filesystem;
+
+    ObjectInfo::ObjectInfo(const ObjectInfo::path& rootPath, const std::string& project, const std::string& name) :
+        rootPath(rootPath), project(project), name(name)
+    {
+        path autoObjectDir = getObjectDirectory();
+        if (std::filesystem::is_directory(autoObjectDir))
+        {
+            this->objectDirectory = autoObjectDir;
+        }
+    }
+
+    ObjectInfo::path ObjectInfo::getObjectDirectory() const
+    {
+        return objectDirectory.empty() ? (rootPath / project / name) : objectDirectory;
+    }
+
+    ObjectInfo::path ObjectInfo::getFile(const std::string& _extension, const std::string& suffix) const
+    {
+        std::string extension = _extension;
+        if (extension.at(0) != '.')
+        {
+            extension = "." + extension;
+        }
+        std::string filename = name + suffix + extension;
+
+        return getObjectDirectory() / filename;
+    }
+
+    ObjectInfo::path ObjectInfo::getSimoxXML() const
+    {
+        return getFile(".xml");
+    }
+
+    ObjectInfo::path ObjectInfo::getWavefrontObj() const
+    {
+        return getFile(".obj");
+    }
+
+    bool ObjectInfo::checkPaths()
+    {
+        namespace fs = std::filesystem;
+
+        if (!fs::is_regular_file(getSimoxXML()))
+        {
+            ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << getSimoxXML();
+            return false;
+        }
+        if (!fs::is_regular_file(getWavefrontObj()))
+        {
+            ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << getWavefrontObj();
+            return false;
+        }
+
+        return true;
+    }
+
+
+    ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageFinder(objectsPackageName)
+    {
+        Logging::setTag("ObjectFinder");
+
+        rootPath = path(packageFinder.getDataDir()) / objectsPackageName;
+        ARMARX_VERBOSE << "Object root directory: " << rootPath;
+        ARMARX_CHECK(rootPath.is_absolute()) << rootPath;
+    }
+
+    std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& project, const std::string& name)
+    {
+        if (!project.empty())
+        {
+            return ObjectInfo(rootPath, project, name);
+        }
+        // Search for object in projects.
+        const auto& projects = getProjects();
+        for (const path& project : projects)
+        {
+            if (fs::is_directory(rootPath / project / name))
+            {
+                return ObjectInfo(rootPath, project, name);
+            }
+        }
+
+        std::stringstream ss;
+        ss << "Did not find object '" << name << "' in any of these projects:\n";
+        for (const path& project : projects)
+        {
+            ss << "- " << project << "\n";
+        }
+        ss << "Root directory: " << rootPath;
+        ARMARX_WARNING << ss.str();
+
+        return std::nullopt;
+    }
+
+    std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID)
+    {
+        if (nameOrID.find("/") != nameOrID.npos)
+        {
+            const std::vector<std::string> split = armarx::split(nameOrID, "/", true);
+            ARMARX_CHECK_EQUAL(split.size(), 2) << "Expected ID of format 'Project/Name', but got: '" << nameOrID
+                                                << "' (too many '/').";
+            return findObject(split[0], split[1]);
+        }
+        else
+        {
+            return findObject("", nameOrID);
+        }
+    }
+
+    std::vector<std::string> ObjectFinder::getProjects()
+    {
+        static const bool local = true;
+        std::vector<path> paths = simox::fs::list_directory(rootPath, local);
+        return std::vector<std::string>(paths.begin(), paths.end());
+    }
+
+    std::vector<ObjectFinder::path> ObjectFinder::getProjectDirectories()
+    {
+        static const bool local = false;
+        return simox::fs::list_directory(rootPath, local);
+    }
+
+    std::vector<ObjectInfo> ObjectFinder::findAllObjects()
+    {
+        bool local = true;
+        std::vector<ObjectInfo> objects;
+        for (const path& projectDir : simox::fs::list_directory(rootPath, local))
+        {
+            if (fs::is_directory(rootPath / projectDir))
+            {
+                std::vector<ObjectInfo> project = findAllObjectsOfProject(projectDir);
+                objects.insert(objects.end(), project.begin(), project.end());
+            }
+        }
+        return objects;
+    }
+
+    std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfProject(const std::string& project)
+    {
+        path projectDir = rootPath / project;
+        if (!fs::is_directory(projectDir))
+        {
+            ARMARX_WARNING << "Expected project directory for project '" << project << "': \n"
+                           << projectDir;
+            return {};
+        }
+
+        std::vector<ObjectInfo> objects;
+        bool local = true;
+        for (const path& dir : simox::fs::list_directory(projectDir, local))
+        {
+            if (fs::is_directory(projectDir / dir))
+            {
+                ObjectInfo object(rootPath, project, dir.filename());
+                if (object.checkPaths())
+                {
+                    objects.push_back(object);
+                }
+            }
+        }
+        return objects;
+    }
+}
+
+namespace armarx
+{
+    std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
+    {
+        return os << "'" << rhs.project << "/" << rhs.name << "'";
+    }
+}
+

source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h

+#pragma once
+
+#include <filesystem>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+
+namespace armarx
+{
+
+    class ObjectInfo
+    {
+    public:
+        using path = std::filesystem::path;
+
+    public:
+
+        ObjectInfo(const path& rootPath, const std::string& project, const std::string& name);
+
+        path getObjectDirectory() const;
+        path getFile(const std::string& extension, const std::string& suffix = "") const;
+
+        path getSimoxXML() const;
+        path getWavefrontObj() const;
+
+        virtual bool checkPaths();
+
+
+    public:
+
+        path rootPath;
+        path objectDirectory;
+
+        std::string project;
+        std::string name;
+
+
+    private:
+
+        path _simoxXML;
+        path _wavefrontObj;
+
+
+    };
+
+    std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);
+
+
+    class ObjectFinder : Logging
+    {
+    public:
+        using path = std::filesystem::path;
+
+
+    public:
+
+        ObjectFinder(const std::string& objectsPackageName = "ArmarXObjects");
+
+        std::optional<ObjectInfo> findObject(const std::string& project, const std::string& name);
+        std::optional<ObjectInfo> findObject(const std::string& nameOrID);
+
+
+        std::vector<std::string> getProjects();
+        std::vector<path> getProjectDirectories();
+
+        std::vector<ObjectInfo> findAllObjects();
+        std::vector<ObjectInfo> findAllObjectsOfProject(const std::string& project);
+
+
+    private:
+
+        CMakePackageFinder packageFinder;
+        path rootPath;
+
+    };
+
+}