From a2fed110e428986ae676af62f40dbbe1ba56fbae Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Mon, 28 Jun 2021 19:06:57 +0200
Subject: [PATCH] Improve handling of "objects" dir, finding articulated
objects
---
.../components/ArViz/Client/Elements.cpp | 11 ++-
.../components/ArViz/Client/Elements.h | 9 ++-
.../libraries/ArmarXObjects/ObjectFinder.cpp | 75 ++++++-------------
.../libraries/ArmarXObjects/ObjectFinder.h | 12 ++-
.../libraries/ArmarXObjects/ObjectInfo.cpp | 39 ++++++++--
.../libraries/ArmarXObjects/ObjectInfo.h | 30 ++++++--
6 files changed, 103 insertions(+), 73 deletions(-)
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 3ce3ea6f4..daec8bc65 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -9,15 +9,18 @@ namespace armarx::viz
{
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+ const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
- Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+ return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
}
- Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- ObjectInfo info(objectsPackage, "", objectID);
+ ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 66f07992e..e9157a24d 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -461,6 +461,7 @@ namespace armarx::viz
{
private:
static const std::string DefaultObjectsPackage;
+ static const std::string DefaultRelativeObjectsDirectory;
public:
using ElementOps::ElementOps;
@@ -483,8 +484,12 @@ namespace armarx::viz
* @param objectsPackage The objects package ("ArmarXObjects" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
- Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
- Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& fileByObjectFinder(const armarx::ObjectID& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(const std::string& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
Object& alpha(float alpha);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index f964364b9..53642e1f5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,12 +1,10 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <boost/algorithm/string.hpp>
+#include <set>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
-#include <VirtualRobot/XML/RobotIO.h>
-
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -17,7 +15,9 @@ namespace armarx
{
namespace fs = std::filesystem;
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+ packageName(objectsPackageName), relObjectsDir(relObjectsDir)
{
Logging::setTag("ObjectFinder");
}
@@ -25,7 +25,7 @@ namespace armarx
void ObjectFinder::setPath(const std::string& path)
{
packageName = path;
- packageDataDir.clear();
+ absPackageDataDir.clear();
}
std::string ObjectFinder::getPackageName() const
@@ -35,11 +35,11 @@ namespace armarx
void ObjectFinder::init() const
{
- if (packageDataDir.empty())
+ if (absPackageDataDir.empty())
{
CMakePackageFinder packageFinder(packageName);
- packageDataDir = packageFinder.getDataDir();
- if (packageDataDir.empty())
+ absPackageDataDir = packageFinder.getDataDir();
+ if (absPackageDataDir.empty())
{
ARMARX_WARNING << "Could not find package '" << packageName << "'.";
// throw LocalException() << "Could not find package '" << packageName << "'.";
@@ -49,7 +49,7 @@ namespace armarx
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
}
}
}
@@ -70,7 +70,7 @@ namespace armarx
}
if (!dataset.empty())
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
// Search for object in datasets.
const std::vector<std::string>& datasets = getDatasets();
@@ -78,7 +78,7 @@ namespace armarx
{
if (fs::is_directory(_rootDirAbs() / dataset / name))
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
}
@@ -217,7 +217,7 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -268,51 +268,22 @@ namespace armarx
return {};
}
- const std::vector<std::string> validExtensions{".urdf", ".xml"};
-
- const auto hasValidExtension = [&](const std::string & file) -> bool
- {
- return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
- };
-
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
const bool local = true;
for (const path& dir : simox::fs::list_directory(datasetDir, local))
{
- if (not fs::is_directory(datasetDir / dir))
- {
- continue;
- }
-
- for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ if (fs::is_directory(datasetDir / dir))
{
- const std::string xml = std::filesystem::path(file).string();
-
- if (hasValidExtension(xml))
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
+ if (modelFile.has_value())
{
- try
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
- const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
- if (robot != nullptr && robot->isPassive())
- {
- const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
-
- objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
- {
- .name = robot->getName(),
- .xml = {packageName, relativeXMLPath}
- // .dataset = dataset
- });
- }
- }
- catch (const armarx::LocalException& ex)
- {
- ARMARX_WARNING << ex.what();
- }
- catch (...)
- {
-
- }
+ .name = object.idStr(),
+ .xml = {modelFile->package, modelFile->relativePath}
+ // .dataset = dataset
+ });
}
}
}
@@ -401,7 +372,7 @@ namespace armarx
ObjectFinder::path ObjectFinder::_rootDirAbs() const
{
- return packageDataDir / packageName / "objects";
+ return absPackageDataDir / packageName / relObjectsDir;
}
ObjectFinder::path ObjectFinder::_rootDirRel() const
@@ -411,7 +382,7 @@ namespace armarx
bool ObjectFinder::_ready() const
{
- return !packageDataDir.empty();
+ return !absPackageDataDir.empty();
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 18c23dbdb..0dcf339d3 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -7,10 +7,11 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
#include "ObjectInfo.h"
#include "ObjectPose.h"
-#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx
{
@@ -25,10 +26,12 @@ namespace armarx
public:
using path = std::filesystem::path;
inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
+ inline static const std::string DefaultObjectsDirectory = "objects";
public:
- ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
+ ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
+ const path& relObjectsDir = DefaultObjectsDirectory);
ObjectFinder(ObjectFinder&&) = default;
ObjectFinder(const ObjectFinder&) = default;
@@ -110,7 +113,10 @@ namespace armarx
* @brief Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
* Empty if package could not be found.
*/
- mutable path packageDataDir;
+ mutable path absPackageDataDir;
+
+ /// Path to the directory containing objects in the package's data directory.
+ path relObjectsDir;
};
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index bbc89162c..162f4570f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -14,14 +14,17 @@ namespace armarx
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const ObjectID& id) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+ 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 std::string& dataset, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(dataset, className)
{
}
@@ -40,7 +43,7 @@ namespace armarx
return _id.dataset();
}
- std::string ObjectInfo::name() const
+ std::string ObjectInfo::className() const
{
return _id.className();
}
@@ -57,7 +60,7 @@ namespace armarx
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / "objects" / _id.dataset() / _id.className();
+ return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -72,7 +75,7 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _packageDataDir / loc.relativePath;
+ loc.absolutePath = _absPackageDataDir / loc.relativePath;
return loc;
}
@@ -86,6 +89,28 @@ namespace armarx
return file(".xml", "_articulated");
}
+ PackageFileLocation ObjectInfo::articulatedUrdf() const
+ {
+ return file(".urdf", "_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
+ {
+ return std::nullopt;
+ }
+ }
+
+
PackageFileLocation ObjectInfo::meshWrl() const
{
return file(".wrl");
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 9e91fa2bb..e55c21dc0 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -41,10 +41,19 @@ namespace armarx
public:
+ /**
+ * @brief ObjectInfo
+ *
+ * @param packageName The ArmarX package.
+ * @param absPackageDataDir Absolute path to the package's data directory.
+ * @param localObjectsPath The path where objects are stored in the data directory.
+ * @param id The object class ID (with dataset and class name).
+ */
+ ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
+ const path& relObjectsPath, const ObjectID& id);
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const ObjectID& id);
- ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& dataset, const std::string& name);
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className);
virtual ~ObjectInfo() = default;
@@ -56,7 +65,13 @@ namespace armarx
std::string package() const;
std::string dataset() const;
- std::string name() const;
+ std::string className() const;
+ [[deprecated("This function is deprecated. Use className() instead.")]]
+ std::string name() const
+ {
+ return className();
+ }
+
/// Return "dataset/name".
ObjectID id() const;
std::string idStr() const;
@@ -64,7 +79,11 @@ namespace armarx
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
PackageFileLocation simoxXML() const;
+
PackageFileLocation articulatedSimoxXML() const;
+ PackageFileLocation articulatedUrdf() const;
+ /// Return the articulated Simox XML or URDF, if one exists.
+ std::optional<PackageFileLocation> getArticulatedModel() const;
PackageFileLocation meshWrl() const;
PackageFileLocation wavefrontObj() const;
@@ -116,7 +135,8 @@ namespace armarx
private:
std::string _packageName;
- path _packageDataDir;
+ path _absPackageDataDir;
+ path _relObjectsPath;
ObjectID _id;
--
GitLab