Skip to content
Snippets Groups Projects
Commit 75506df3 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Rename aabb() and oobb() to loadAABB/OOBB() and make them return std::nullopt...

Rename aabb() and oobb() to loadAABB/OOBB() and make them return std::nullopt on error (instead of throwing exception)
parent a1405ef2
No related branches found
No related tags found
1 merge request!84Object Pose Observer: Enable requesting
......@@ -456,7 +456,7 @@ namespace armarx
{
try
{
oobb = objectInfo->oobb();
oobb = objectInfo->loadOOBB();
}
catch (const std::ios_base::failure& e)
{
......
......@@ -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();
......
#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;
/**
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment