From 07369a3983115e03e15521171075bebf790fa262 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 16 Feb 2024 08:46:58 +0100
Subject: [PATCH] increasing verbosity of ObjectMemory snapshot loading
---
.../server/ObjectMemory/ObjectMemory.cpp | 6 +-
.../libraries/ArmarXObjects/ObjectInfo.cpp | 247 +++++++++++-------
.../armem_objects/server/class/Segment.cpp | 20 ++
.../armem_objects/server/class/Segment.h | 2 +-
.../armem_objects/server/instance/Segment.cpp | 2 +
5 files changed, 185 insertions(+), 92 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index e00518697..bc8179ff6 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -103,15 +103,15 @@ namespace armarx::armem::server::obj
}
catch (const LocalException& e)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
}
catch (const std::exception& e)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
}
catch (...)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment for unknown reason.";
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment for unknown reason.";
}
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 6d20564f6..3b97c8040 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -1,67 +1,84 @@
#include "ObjectInfo.h"
+#include <filesystem>
+#include <optional>
+
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include "ArmarXCore/core/exceptions/LocalException.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 ObjectID& id) :
+ _packageName(packageName),
+ _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath),
+ _id(id)
{
}
- ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
+ 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)
+ const std::string& dataset,
+ const std::string& className) :
+ _packageName(packageName),
+ _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath),
+ _id(dataset, className)
{
}
- void ObjectInfo::setLogError(bool enabled)
+ void
+ ObjectInfo::setLogError(bool enabled)
{
this->_logError = enabled;
}
- std::string ObjectInfo::package() const
+ std::string
+ ObjectInfo::package() const
{
return _packageName;
}
- std::string ObjectInfo::dataset() const
+ std::string
+ ObjectInfo::dataset() const
{
return _id.dataset();
}
- std::string ObjectInfo::className() const
+ std::string
+ ObjectInfo::className() const
{
return _id.className();
}
- ObjectID ObjectInfo::id() const
+ ObjectID
+ ObjectInfo::id() const
{
return _id;
}
- std::string ObjectInfo::idStr() const
+ std::string
+ ObjectInfo::idStr() const
{
return _id.str();
}
- ObjectInfo::path ObjectInfo::objectDirectory(const bool fixDataPath) const
+ ObjectInfo::path
+ ObjectInfo::objectDirectory(const bool fixDataPath) const
{
- if(fixDataPath)
+ if (fixDataPath)
{
return _relObjectsPath / _id.dataset() / _id.className();
}
@@ -69,7 +86,10 @@ namespace armarx
return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix, const bool fixDataPath) const
+ PackageFileLocation
+ ObjectInfo::file(const std::string& _extension,
+ const std::string& suffix,
+ const bool fixDataPath) const
{
std::string extension = _extension;
if (extension.at(0) != '.')
@@ -85,23 +105,26 @@ namespace armarx
return loc;
}
-
- PackageFileLocation ObjectInfo::simoxXML() const
+ PackageFileLocation
+ ObjectInfo::simoxXML() const
{
return file(".xml");
}
- PackageFileLocation ObjectInfo::urdf() const
+ PackageFileLocation
+ ObjectInfo::urdf() const
{
return file(".urdf");
}
- PackageFileLocation ObjectInfo::sdf() const
+ PackageFileLocation
+ ObjectInfo::sdf() const
{
return file(".sdf");
}
- std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ std::optional<PackageFileLocation>
+ ObjectInfo::getModel() const
{
if (fs::is_regular_file(simoxXML().absolutePath))
{
@@ -121,22 +144,26 @@ namespace armarx
}
}
- PackageFileLocation ObjectInfo::articulatedSimoxXML() const
+ PackageFileLocation
+ ObjectInfo::articulatedSimoxXML() const
{
return file(".xml", "_articulated", true);
}
- PackageFileLocation ObjectInfo::articulatedUrdf() const
+ PackageFileLocation
+ ObjectInfo::articulatedUrdf() const
{
return file(".urdf", "_articulated", true);
}
- PackageFileLocation ObjectInfo::articulatedSdf() const
+ PackageFileLocation
+ ObjectInfo::articulatedSdf() const
{
return file(".sdf", "_articulated");
}
- std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+ std::optional<PackageFileLocation>
+ ObjectInfo::getArticulatedModel() const
{
if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
{
@@ -156,33 +183,37 @@ namespace armarx
}
}
-
-
- PackageFileLocation ObjectInfo::meshWrl() const
+ PackageFileLocation
+ ObjectInfo::meshWrl() const
{
return file(".wrl");
}
- PackageFileLocation ObjectInfo::wavefrontObj() const
+ PackageFileLocation
+ ObjectInfo::wavefrontObj() const
{
return file(".obj");
}
- PackageFileLocation ObjectInfo::boundingBoxJson() const
+ PackageFileLocation
+ ObjectInfo::boundingBoxJson() const
{
return file(".json", "_bb");
}
- PackageFileLocation ObjectInfo::namesJson() const
+ PackageFileLocation
+ ObjectInfo::namesJson() const
{
return file(".json", "_names");
}
- std::optional<simox::AxisAlignedBoundingBox> ObjectInfo::loadAABB() const
+ std::optional<simox::AxisAlignedBoundingBox>
+ ObjectInfo::loadAABB() const
{
nlohmann::json j;
try
{
+ ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
j = nlohmann::read_json(boundingBoxJson().absolutePath);
}
catch (const std::exception& e)
@@ -194,28 +225,52 @@ namespace armarx
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);
+ try
+ {
- 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();
+ 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;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ }
- return aabb;
+ return std::nullopt;
}
- std::optional<simox::OrientedBox<float>> ObjectInfo::loadOOBB() const
+ std::optional<simox::OrientedBox<float>>
+ ObjectInfo::loadOOBB() const
{
nlohmann::json j;
try
{
+ ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
j = nlohmann::read_json(boundingBoxJson().absolutePath);
}
catch (const std::exception& e)
@@ -227,48 +282,60 @@ namespace armarx
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)
+ try
+ {
+ 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;
+ }
+ catch (...)
{
- 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());
+ ARMARX_WARNING << GetHandledExceptionString();
}
- return oobb;
+
+ return std::nullopt;
}
- std::optional<std::vector<std::string>> ObjectInfo::loadRecognizedNames() const
+ std::optional<std::vector<std::string>>
+ ObjectInfo::loadRecognizedNames() const
{
return loadNames("recognized_name");
}
- std::optional<std::vector<std::string>> ObjectInfo::loadSpokenNames() const
+ 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
+ std::optional<std::vector<std::string>>
+ ObjectInfo::loadNames(const std::string& jsonKey) const
{
const PackageFileLocation file = namesJson();
if (fs::is_regular_file(file.absolutePath))
@@ -283,7 +350,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n"
+ << e.what();
}
return std::nullopt;
}
@@ -291,7 +359,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n"
+ << e.what();
}
return std::nullopt;
}
@@ -304,8 +373,8 @@ namespace armarx
}
}
-
- bool ObjectInfo::checkPaths() const
+ bool
+ ObjectInfo::checkPaths() const
{
namespace fs = std::filesystem;
bool result = true;
@@ -314,7 +383,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected simox object file for object " << *this << ": " << simoxXML().absolutePath;
+ ARMARX_WARNING << "Expected simox object file for object " << *this << ": "
+ << simoxXML().absolutePath;
}
result = false;
}
@@ -322,7 +392,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this << ": " << wavefrontObj().absolutePath;
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this
+ << ": " << wavefrontObj().absolutePath;
}
result = false;
}
@@ -330,10 +401,10 @@ namespace armarx
return result;
}
-}
-
+} // namespace armarx
-std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
+std::ostream&
+armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
return os << rhs.id();
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 5506ddbf5..a9b406a13 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -7,6 +7,7 @@
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -55,6 +56,7 @@ namespace armarx::armem::server::obj::clazz
if (p.loadFromObjectsPackage)
{
+ ARMARX_VERBOSE << "Loading objects from package";
loadByObjectFinder(p.objectsPackage);
}
}
@@ -71,6 +73,7 @@ namespace armarx::armem::server::obj::clazz
void
Segment::loadByObjectFinder(const std::string& objectsPackage)
{
+ ARMARX_TRACE;
loadByObjectFinder(ObjectFinder(objectsPackage));
}
@@ -84,6 +87,8 @@ namespace armarx::armem::server::obj::clazz
void
Segment::loadByObjectFinder()
{
+ ARMARX_TRACE;
+
const Time now = Time::Now();
const bool checkPaths = false;
@@ -96,15 +101,23 @@ namespace armarx::armem::server::obj::clazz
Commit commit;
for (ObjectInfo& info : infos)
{
+ ARMARX_TRACE;
+
+ ARMARX_VERBOSE << info.idStr();
info.setLogError(false);
EntityUpdate& update = commit.add();
update.entityID = providerID.withEntityName(info.id().str());
update.arrivedTime = update.referencedTime = update.sentTime = now;
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "object class from info";
arondto::ObjectClass objectClass = objectClassFromInfo(info);
update.instancesData = {objectClass.toAron()};
}
+
+ ARMARX_TRACE;
+
ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
<< objectFinder.getPackageName() << "'.";
iceMemory.commitLocking(commit);
@@ -179,6 +192,8 @@ namespace armarx::armem::server::obj::clazz
{
namespace fs = std::filesystem;
+ // ARMARX_VERBOSE << info.idStr();
+
arondto::ObjectClass data;
toAron(data.id, info.id());
@@ -203,9 +218,14 @@ namespace armarx::armem::server::obj::clazz
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "AABB";
auto aabb = info.loadAABB();
toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
+
+ ARMARX_TRACE;
auto oobb = info.loadOOBB();
+ ARMARX_VERBOSE << "OOBB";
toAron(data.oobb, oobb ? oobb.value() : simox::OrientedBoxf());
if (auto recogNames = info.loadRecognizedNames())
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 89c13ce4c..ba42e0072 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -41,7 +41,7 @@ namespace armarx::armem::server::obj::clazz
void visualizeClass(const MemoryID& entityID, bool showAABB = true, bool showOOBB = true);
- static arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
+ arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
private:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 1bf020045..865fa2ae3 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <filesystem>
#include <sstream>
#include <Eigen/Dense>
@@ -1013,6 +1014,7 @@ namespace armarx::armem::server::obj::instance
{
try
{
+ ARMARX_CHECK(std::filesystem::exists(path)) << path;
return simox::json::read<armarx::objects::Scene>(path);
}
catch (const simox::json::error::JsonError& e)
--
GitLab