diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp index 8c70a93aac07dae51aa5b82014038e009f2a8cd1..348a5b25419813a22717f71c92d0c3484042361c 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp @@ -25,6 +25,11 @@ namespace armarx { } + void ObjectInfo::setLogError(bool enabled) + { + this->_logError = enabled; + } + std::string ObjectInfo::package() const { return _packageName; @@ -100,7 +105,10 @@ namespace armarx } catch (const std::exception& e) { - ARMARX_ERROR << e.what(); + if (_logError) + { + ARMARX_ERROR << e.what(); + } return std::nullopt; } @@ -113,10 +121,10 @@ namespace armarx simox::AxisAlignedBoundingBox aabb(min, max); static const float prec = 1e-4f; - ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose(); - ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose(); - ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose(); - ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose(); + 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; } @@ -130,7 +138,10 @@ namespace armarx } catch (const std::exception& e) { - ARMARX_ERROR << e.what(); + if (_logError) + { + ARMARX_ERROR << e.what(); + } return std::nullopt; } @@ -147,9 +158,21 @@ namespace armarx ori.col(2) * extents(2)); static const float prec = 1e-3f; - ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec) << oobb.center().transpose() << "\n" << pos.transpose(); - ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori; - ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec) << oobb.dimensions().transpose() << "\n" << extents.transpose(); + 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; } @@ -176,12 +199,18 @@ namespace armarx } catch (const nlohmann::json::exception& e) { - ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what(); + if (_logError) + { + ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what(); + } return std::nullopt; } catch (const std::exception& e) { - ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what(); + if (_logError) + { + ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what(); + } return std::nullopt; } @@ -201,12 +230,18 @@ namespace armarx if (!fs::is_regular_file(simoxXML().absolutePath)) { - ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath; + if (_logError) + { + ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath; + } result = false; } if (!fs::is_regular_file(wavefrontObj().absolutePath)) { - ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath; + if (_logError) + { + ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath; + } result = false; } diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h index 10dbd5a08fd04c710cec3c41030357cc70c2243d..634944e4cc52379977f784c370f48af1e84ed28d 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h @@ -50,6 +50,9 @@ namespace armarx virtual ~ObjectInfo() = default; + void setLogError(bool enabled); + + std::string package() const; std::string dataset() const; @@ -114,6 +117,8 @@ namespace armarx ObjectID _id; + bool _logError = true; + }; std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);