diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
index af1dadc6edb222a64c836ba88d5648db6db0dc2e..4779f70a41dda97a309ed7606b37f192f142a29a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.cpp
@@ -1,6 +1,8 @@
#include "ObjectFinder.h"
#include <SimoxUtility/filesystem/list_directory.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -17,7 +19,7 @@ namespace armarx
Logging::setTag("ObjectFinder");
}
- void ObjectFinder::init()
+ void ObjectFinder::init() const
{
if (packageDataDir.empty())
{
@@ -36,28 +38,28 @@ namespace armarx
}
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& project, const std::string& name)
+ std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
{
init();
- if (!project.empty())
+ if (!dataset.empty())
{
- return ObjectInfo(packageName, packageDataDir, project, name);
+ return ObjectInfo(packageName, packageDataDir, dataset, name);
}
- // Search for object in projects.
- const auto& projects = getProjects();
- for (const path& project : projects)
+ // Search for object in datasets.
+ const auto& datasets = getDatasets();
+ for (const path& dataset : datasets)
{
- if (fs::is_directory(_rootDirAbs() / project / name))
+ if (fs::is_directory(_rootDirAbs() / dataset / name))
{
- return ObjectInfo(packageName, packageDataDir, project, name);
+ return ObjectInfo(packageName, packageDataDir, dataset, name);
}
}
std::stringstream ss;
- ss << "Did not find object '" << name << "' in any of these projects:\n";
- for (const path& project : projects)
+ ss << "Did not find object '" << name << "' in any of these datasets:\n";
+ for (const path& dataset : datasets)
{
- ss << "- " << project << "\n";
+ ss << "- " << dataset << "\n";
}
ss << "Objects root directory: " << _rootDirAbs();
ARMARX_WARNING << ss.str();
@@ -65,13 +67,13 @@ namespace armarx
return std::nullopt;
}
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID)
+ std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID) const
{
init();
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
+ ARMARX_CHECK_EQUAL(split.size(), 2) << "Expected ID of format 'Dataset/Name', but got: '" << nameOrID
<< "' (too many '/').";
return findObject(split[0], split[1]);
}
@@ -81,7 +83,7 @@ namespace armarx
}
}
- std::vector<std::string> ObjectFinder::getProjects()
+ std::vector<std::string> ObjectFinder::getDatasets() const
{
init();
const bool local = true;
@@ -89,48 +91,51 @@ namespace armarx
return std::vector<std::string>(paths.begin(), paths.end());
}
- std::vector<ObjectFinder::path> ObjectFinder::getProjectDirectories()
+ std::vector<ObjectFinder::path> ObjectFinder::getDatasetDirectories() const
{
init();
const bool local = false;
return simox::fs::list_directory(_rootDirAbs(), local);
}
- std::vector<ObjectInfo> ObjectFinder::findAllObjects()
+ std::vector<ObjectInfo> ObjectFinder::findAllObjects(bool checkPaths) const
{
init();
const bool local = true;
std::vector<ObjectInfo> objects;
- for (const path& projectDir : simox::fs::list_directory(_rootDirAbs(), local))
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / projectDir))
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
{
- std::vector<ObjectInfo> project = findAllObjectsOfProject(projectDir);
- objects.insert(objects.end(), project.begin(), project.end());
+ std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
+ for (const auto& o : dataset)
+ {
+ objects.push_back(o);
+ }
}
}
return objects;
}
- std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfProject(const std::string& project)
+ std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
- path projectDir = _rootDirAbs() / project;
- if (!fs::is_directory(projectDir))
+ path datasetDir = _rootDirAbs() / dataset;
+ if (!fs::is_directory(datasetDir))
{
- ARMARX_WARNING << "Expected project directory for project '" << project << "': \n"
- << projectDir;
+ 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(projectDir, local))
+ for (const path& dir : simox::fs::list_directory(datasetDir, local))
{
- if (fs::is_directory(projectDir / dir))
+ if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, packageDataDir, project, dir.filename());
- if (object.checkPaths())
+ ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
}
@@ -151,8 +156,8 @@ namespace armarx
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const std::string& project, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _project(project), _name(name)
+ const std::string& dataset, const std::string& name) :
+ _packageName(packageName), _packageDataDir(packageDataDir), _dataset(dataset), _name(name)
{
}
@@ -161,9 +166,9 @@ namespace armarx
return _packageName;
}
- std::string ObjectInfo::project() const
+ std::string ObjectInfo::dataset() const
{
- return _project;
+ return _dataset;
}
std::string ObjectInfo::name() const
@@ -173,12 +178,12 @@ namespace armarx
std::string ObjectInfo::id() const
{
- return _project + "/" + _name;
+ return _dataset + "/" + _name;
}
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / _project / _name;
+ return path(_packageName) / _dataset / _name;
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -207,6 +212,21 @@ namespace armarx
return file(".obj");
}
+ PackageFileLocation ObjectInfo::boundingBoxJson() const
+ {
+ return file(".json", "_bb");
+ }
+
+ simox::AxisAlignedBoundingBox ObjectInfo::aabb() const
+ {
+ nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath);
+ nlohmann::json jaabb = j.at("aabb");
+ auto min = jaabb.at("min").get<Eigen::Vector3f>();
+ auto max = jaabb.at("max").get<Eigen::Vector3f>();
+
+ return simox::AxisAlignedBoundingBox(min, max);
+ }
+
bool ObjectInfo::checkPaths() const
{
namespace fs = std::filesystem;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
index 548fb3a82b95d27244490e7227ccfcc7a1d04023..e7e8fdf98b9669d2f52ac746200d53113a3c7333 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectFinder.h
@@ -4,6 +4,10 @@
#include <ArmarXCore/core/logging/Logging.h>
+namespace simox
+{
+ struct AxisAlignedBoundingBox;
+}
namespace armarx
{
@@ -11,6 +15,11 @@ namespace armarx
class ObjectInfo;
+ /**
+ * @brief Used to find objects in the ArmarX objects repository [1].
+ *
+ * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
+ */
class ObjectFinder : Logging
{
public:
@@ -20,20 +29,20 @@ namespace armarx
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::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
+ std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
- std::vector<std::string> getProjects();
- std::vector<path> getProjectDirectories();
+ std::vector<std::string> getDatasets() const;
+ std::vector<path> getDatasetDirectories() const;
- std::vector<ObjectInfo> findAllObjects();
- std::vector<ObjectInfo> findAllObjectsOfProject(const std::string& project);
+ std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
+ std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
private:
- void init();
+ void init() const;
path _rootDirAbs() const;
path _rootDirRel() const;
@@ -42,10 +51,10 @@ namespace armarx
/// Name of package containing the object models (ArmarXObjects by default).
- std::string packageName;
+ mutable std::string packageName;
/// Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
- path packageDataDir;
+ mutable path packageDataDir;
};
@@ -55,11 +64,16 @@ namespace armarx
/// Name of the ArmarX package.
std::string package;
+ /// Relative to the package's data directory.
std::string relativePath;
+ /// The absolute path (in the host's file system).
std::filesystem::path absolutePath;
};
+ /**
+ * @brief Accessor for the object files.
+ */
class ObjectInfo
{
public:
@@ -68,13 +82,13 @@ namespace armarx
public:
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& project, const std::string& name);
+ const std::string& dataset, const std::string& name);
std::string package() const;
- std::string project() const;
+ std::string dataset() const;
std::string name() const;
- /// Return "project/name".
+ /// Return "dataset/name".
std::string id() const;
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
@@ -82,6 +96,11 @@ namespace armarx
PackageFileLocation simoxXML() const;
PackageFileLocation wavefrontObj() const;
+ PackageFileLocation boundingBoxJson() const;
+
+ simox::AxisAlignedBoundingBox aabb() const;
+
+
/**
* @brief Checks the existence of expected files.
* If a file is does not exist, emits a warning returns false.
@@ -100,7 +119,7 @@ namespace armarx
std::string _packageName;
path _packageDataDir;
- std::string _project;
+ std::string _dataset;
std::string _name;
};
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index dd60b2c6523726e1233478eef2cd83cf45b97d4b..ced81f772c2f435687712d76de30620a84ec9bc5 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -22,7 +22,9 @@
#include "ObjectPoseObserver.h"
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/meta/EnumNames.hpp>
+
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
@@ -32,7 +34,7 @@
std::ostream& armarx::objpose::operator<<(std::ostream& os, const ObjectID& id)
{
- return os << "'" << id.project << "/" << id.name << "'";
+ return os << "'" << id.dataset << "/" << id.name << "'";
}
@@ -177,16 +179,21 @@ namespace armarx
objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
objpose::ObjectPoseSeq result;
for (const auto& [name, poses] : objectPoses)
{
- result.insert(result.end(), poses.begin(), poses.end());
+ for (const auto& pose : poses)
+ {
+ result.push_back(pose);
+ }
}
return result;
}
objpose::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return objectPoses.at(providerName);
}
@@ -201,7 +208,7 @@ namespace armarx
{
for (const auto& [name, info] : providers)
{
- // ToDo: optimize look.
+ // ToDo: optimize look up.
if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
{
requests[name].push_back(objectID);
@@ -224,17 +231,14 @@ namespace armarx
objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return providers;
}
Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
{
- Ice::StringSeq names;
- for (const auto& [name, _] : providers)
- {
- names.push_back(name);
- }
- return names;
+ std::scoped_lock lock(dataMutex);
+ return simox::get_keys(providers);
}
objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
@@ -258,19 +262,20 @@ namespace armarx
bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return providers.count(providerName) > 0;
}
-
Ice::Int ObjectPoseObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
{
- return {};
+ std::scoped_lock lock(dataMutex);
+ return updateCounters.at(providerName);
}
StringIntDictionary ObjectPoseObserver::getAllUpdateCounters(const Ice::Current&)
{
- return {};
+ return updateCounters;
}
@@ -318,9 +323,9 @@ namespace armarx
for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
{
const objpose::ObjectID id = objectPose.objectID;
- std::string key = id.project + "/" + id.name;
+ std::string key = id.dataset + "/" + id.name;
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.project, id.name);
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.dataset, id.name);
if (!objectInfo)
{
ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
@@ -329,7 +334,7 @@ namespace armarx
PoseBasePtr pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
- viz::Object object = viz::Object(id.project + "/" + id.name)
+ viz::Object object = viz::Object(id.dataset + "/" + id.name)
.file(objectInfo->package(), objectInfo->simoxXML().relativePath)
.pose(armarx::PosePtr::dynamicCast(pose)->toEigen());
if (visu.alpha < 1)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index a50d3244b104722b417e283a0d90a9dfebf8f7b4..0bb447d0530bcd56052ac7c3a98dae468d6cb777 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -136,6 +136,7 @@ namespace armarx
VirtualRobot::RobotPtr robot;
std::mutex dataMutex;
+
objpose::ProviderInfoMap providers;
std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
index 698c7d37592c76b9eb9f1b2aa52ea9b4cec65499..f15a5b813e130bec95ffaa6ce4339b52be87d6ec 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseProviderPlugin.h
@@ -39,7 +39,7 @@ namespace armarx
*/
class ObjectPoseProviderPluginUser :
virtual public ManagedIceObject
- , virtual objpose::ObjectPoseProvider
+ , virtual public objpose::ObjectPoseProvider
{
public:
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 487d8c983680c549235b779226bbc39064ca7265..a58a76d5cd724a7905bd9f35d2305a25788b3d6f 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -41,7 +41,7 @@ module armarx
struct ObjectID
{
- string project; ///< e.g. "KIT", "YCB", "SecondHands", ...
+ string dataset; ///< e.g. "KIT", "YCB", "SecondHands", ...
string name; ///< e.g. "Amicelli", "001_chips_can", ...
};
sequence<ObjectID> ObjectIDSeq;