diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 6db9b9ad21f4f6cddeb48998b3aa4a82068c0d4e..cfb470f8f5ccddb59e64bba6fc4627a343824aa1 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -456,7 +456,7 @@ namespace armarx { try { - oobb = objectInfo->oobb(); + oobb = objectInfo->loadOOBB(); } catch (const std::ios_base::failure& e) { diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp index ac272b10abe1bc26e47a52527df38034b2f24905..1dc1903ea050d8c2f9fba6596d04fc12d197f5ba 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp @@ -85,11 +85,20 @@ namespace armarx return file(".json", "_bb"); } - simox::AxisAlignedBoundingBox ObjectInfo::aabb() const + std::optional<simox::AxisAlignedBoundingBox> ObjectInfo::loadAABB() const { - nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath); - nlohmann::json jaabb = j.at("aabb"); + nlohmann::json j; + try + { + j = nlohmann::read_json(boundingBoxJson().absolutePath); + } + catch (const std::exception& e) + { + ARMARX_ERROR << e.what(); + 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>(); @@ -106,9 +115,19 @@ namespace armarx return aabb; } - simox::OrientedBox<float> ObjectInfo::oobb() const + std::optional<simox::OrientedBox<float>> ObjectInfo::loadOOBB() const { - nlohmann::json j = nlohmann::read_json(boundingBoxJson().absolutePath); + nlohmann::json j; + try + { + j = nlohmann::read_json(boundingBoxJson().absolutePath); + } + catch (const std::exception& e) + { + ARMARX_ERROR << e.what(); + 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(); diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h index c141bd9d467857c8869b60c2b5c5eebdd48697d4..5f50ec95691bb881b00be5e64703f39c9e9e7b02 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h +++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h @@ -1,6 +1,7 @@ #pragma once #include <filesystem> +#include <optional> #include <string> #include "ObjectID.h" @@ -63,8 +64,17 @@ namespace armarx PackageFileLocation boundingBoxJson() const; - simox::AxisAlignedBoundingBox aabb() const; - simox::OrientedBox<float> oobb() const; + /** + * @brief Load the AABB (axis-aligned bounding-box) from the bounding box JSON file. + * @return Return the AABB if successful, `std::nullopt` if file does not exist. + */ + std::optional<simox::AxisAlignedBoundingBox> loadAABB() const; + /** + * @brief Load the OOBB (object-oriented bounding box) from the bounding box JSON file. + * The OOBB is defined the object's local frame. + * @return Return the OOBB if successful, `std::nullopt` if file does not exist. + */ + std::optional<simox::OrientedBox<float>> loadOOBB() const; /**