diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index dd24a1ecc8cc9cf390684902112e5ac44a49c624..2fd894a53894cd778ceabbedab556f185036aee4 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -8,10 +8,24 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include "ice_conversions.h"
+
namespace armarx::objpose
{
+ Eigen::Vector3f toEigen(const Vector3BasePtr pose)
+ {
+ auto cast = Vector3Ptr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+ Eigen::Matrix3f toEigen(const QuaternionBasePtr pose)
+ {
+ auto cast = QuaternionPtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
Eigen::Matrix4f toEigen(const PoseBasePtr pose)
{
auto cast = PosePtr::dynamicCast(pose);
@@ -32,7 +46,7 @@ namespace armarx::objpose
{
providerName = ice.providerName;
objectType = ice.objectType;
- objectID = ice.objectID;
+ objectID = { ice.objectID.dataset, ice.objectID.name };
objectPoseRobot = toEigen(ice.objectPoseRobot);
objectPoseGlobal = toEigen(ice.objectPoseGlobal);
@@ -45,7 +59,7 @@ namespace armarx::objpose
confidence = ice.confidence;
timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
- localOOBB = ice.localOOBB;
+ objpose::fromIce(ice.localOOBB, localOOBB);
}
data::ObjectPose ObjectPose::toIce() const
@@ -59,7 +73,7 @@ namespace armarx::objpose
{
ice.providerName = providerName;
ice.objectType = objectType;
- ice.objectID = objectID;
+ ice.objectID = { objectID.dataset(), objectID.name() };
ice.objectPoseRobot = new Pose(objectPoseRobot);
ice.objectPoseGlobal = new Pose(objectPoseGlobal);
@@ -72,7 +86,7 @@ namespace armarx::objpose
ice.confidence = confidence;
ice.timestampMicroSeconds = timestamp.toMicroSeconds();
- ice.localOOBB = localOOBB;
+ objpose::toIce(ice.localOOBB, localOOBB);
}
void ObjectPose::fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
@@ -80,7 +94,7 @@ namespace armarx::objpose
providerName = provided.providerName;
objectType = provided.objectType;
- objectID = provided.objectID;
+ objectID = { provided.objectID.dataset, provided.objectID.name };
objectPoseOriginal = toEigen(provided.objectPose);
objectPoseOriginalFrame = provided.objectPoseFrame;
@@ -97,7 +111,7 @@ namespace armarx::objpose
confidence = provided.confidence;
timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
- localOOBB = provided.localOOBB;
+ objpose::fromIce(provided.localOOBB, localOOBB);
}
}
@@ -158,3 +172,7 @@ namespace armarx
}
+
+
+
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
index 6a237e319daaae4c6e43ff0d6cd933990d2ae2f2..814ba29c50b3021663bf3d50163eea65f8aced9f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -2,11 +2,13 @@
#include <Eigen/Core>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
#include <IceUtil/Time.h>
#include <RobotAPI/interface/objectpose/types.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
namespace armarx::objpose
@@ -34,7 +36,7 @@ namespace armarx::objpose
ObjectTypeEnum objectType = AnyObject;
/// The object ID, i.e. dataset and name.
- ObjectID objectID;
+ armarx::ObjectID objectID;
Eigen::Matrix4f objectPoseRobot;
Eigen::Matrix4f objectPoseGlobal;
@@ -50,7 +52,7 @@ namespace armarx::objpose
IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
/// Object bounding box in object's local coordinate frame.
- Box localOOBB;
+ simox::OrientedBoxf localOOBB;
};
using ObjectPoseSeq = std::vector<ObjectPose>;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index c9e12b8e92a20aed2b2c9a1d60b5b75d3dd7f2a7..f7d2eee97b3ec601420ce9cf010af23817814dd7 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -23,7 +23,9 @@
#include "ObjectPoseObserver.h"
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/meta/EnumNames.hpp>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
@@ -57,7 +59,6 @@ namespace armarx
return defs;
}
-
std::string ObjectPoseObserver::getDefaultName() const
{
return "ObjectPoseObserver";
@@ -71,6 +72,9 @@ namespace armarx
void ObjectPoseObserver::onConnectObserver()
{
this->robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
void ObjectPoseObserver::onDisconnectComponent()
@@ -82,42 +86,50 @@ namespace armarx
}
- void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const Ice::Current&)
+ void ObjectPoseObserver::createRemoteGuiTab()
{
- objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
- if (!provider)
+ using namespace armarx::RemoteGui::Client;
+
+ tab.visuEnabled.setValue(visu.enabled);
+ tab.visuInGlobalFrame.setValue(visu.inGlobalFrame);
+ tab.visuAlpha.setRange(0, 1.0);
+ tab.visuAlpha.setValue(visu.alpha);
+ tab.visuOOBBs.setValue(visu.oobbs);
+
+ GroupBox visuGroup;
{
- ARMARX_WARNING << "Received availability signal from provider '" << providerName << "', "
- << "but could not get provider proxy.";
- return;
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(tab.visuEnabled, {row, 1});
+ row++;
+ grid.add(Label("Global Frame"), {row, 0}).add(tab.visuInGlobalFrame, {row, 1});
+ row++;
+ grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1});
+ row++;
+ grid.add(Label("OOBB"), {row, 0}).add(tab.visuOOBBs, {row, 1});
+ row++;
+
+ visuGroup.setLabel("Visualization");
+ visuGroup.addChild(grid);
}
- objpose::ProviderInfo info = provider->getProviderInfo();
- {
- std::scoped_lock lock(dataMutex);
- if (providers.count(providerName) == 0)
- {
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "New provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- }
- providers[providerName] = info;
+ VBoxLayout root = {visuGroup, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
- if (updateCounters.count(providerName) == 0)
- {
- updateCounters[providerName] = 0;
- }
- }
+ void ObjectPoseObserver::RemoteGui_update()
+ {
+ visu.enabled = tab.visuEnabled.getValue();
+ visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
+ visu.alpha = tab.visuAlpha.getValue();
+ visu.oobbs = tab.visuOOBBs.getValue();
+ }
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
+
+ void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const Ice::Current&)
+ {
+ static const bool refresh = true;
+ fetchProviderInfo(providerName, refresh);
}
void ObjectPoseObserver::reportObjectPoses(
@@ -125,10 +137,13 @@ namespace armarx
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
+ static const bool refresh = false;
+ fetchProviderInfo(providerName, refresh);
+
objpose::ObjectPoseSeq objectPoses;
{
std::scoped_lock lock(dataMutex);
- synchronizeLocalClone(robot);
+ RobotState::synchronizeLocalClone("robot");
if (robot->hasRobotNode(calibration.robotNode))
{
@@ -141,6 +156,10 @@ namespace armarx
{
objpose::ObjectPose& pose = objectPoses.emplace_back();
pose.fromProvidedPose(provided, robot);
+ if (!(provided.localOOBB.position && provided.localOOBB.orientation && provided.localOOBB.extents))
+ {
+ pose.localOOBB = getObjectOOBB(pose.objectID);
+ }
}
}
@@ -152,9 +171,59 @@ namespace armarx
}
+ void ObjectPoseObserver::fetchProviderInfo(const std::string& providerName, bool refresh)
+ {
+ {
+ std::scoped_lock lock(dataMutex);
+ if (!refresh && providers.count(providerName) > 0)
+ {
+ return; // Do nothing.
+ }
+ }
+
+ objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
+ if (!provider)
+ {
+ ARMARX_WARNING << "Received availability signal from provider '" << providerName << "', "
+ << "but could not get provider proxy.";
+ return;
+ }
+ objpose::ProviderInfo info = provider->getProviderInfo();
+
+ {
+ std::scoped_lock lock(dataMutex);
+ if (providers.count(providerName) == 0)
+ {
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "New provider '" << providerName << "' available.\n"
+ << "Supported objects: \n" << ss.str();
+ }
+ providers[providerName] = info;
+
+ if (updateCounters.count(providerName) == 0)
+ {
+ updateCounters[providerName] = 0;
+ }
+ }
+
+ if (!existsChannel(providerName))
+ {
+ offerChannel(providerName, "Channel of provider '" + providerName + "'.");
+ }
+ offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
+ }
+
+
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
+ // IceUtil::Time start = IceUtil::Time::now();
std::scoped_lock lock(dataMutex);
+ // ARMARX_INFO << "Locking took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
+ // start = IceUtil::Time::now();
objpose::data::ObjectPoseSeq result;
for (const auto& [name, poses] : objectPoses)
{
@@ -163,6 +232,7 @@ namespace armarx
result.push_back(pose.toIce());
}
}
+ // ARMARX_INFO << "getObjectPoses() took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
return result;
}
@@ -254,20 +324,6 @@ namespace armarx
}
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- VBoxLayout root = {};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
-
- }
-
-
void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
{
// Initialized to 0 on first access.
@@ -297,27 +353,37 @@ namespace armarx
for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
{
- const objpose::ObjectID id = objectPose.objectID;
- std::string key = id.dataset + "/" + id.name;
+ const armarx::ObjectID id = objectPose.objectID;
+ std::string key = id.str();
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.dataset, id.name);
- if (!objectInfo)
- {
- ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
- continue;
- }
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
- viz::Object object = viz::Object(id.dataset + "/" + id.name)
- .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
- .pose(pose);
- if (visu.alpha < 1)
+ if (objectInfo)
+ {
+ viz::Object object = viz::Object(key)
+ .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
+ .pose(pose);
+ if (visu.alpha < 1)
+ {
+ object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
+ }
+
+ layer.add(object);
+ }
+ else
{
- object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
+ ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
}
- layer.add(object);
+ if (visu.oobbs)
+ {
+ simox::OrientedBox<float> oobb = objectPose.localOOBB;
+ layer.add(viz::Box(key + " OOBB")
+ .pose(pose * simox::math::pose(oobb.center(), oobb.rotation()))
+ .size(oobb.dimensions()).color(simox::Color::lime(255, 64)));
+ }
}
arviz.commit(layer);
@@ -328,5 +394,32 @@ namespace armarx
return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
}
+ simox::OrientedBoxf ObjectPoseObserver::getObjectOOBB(const ObjectID& id)
+ {
+ if (auto it = oobbCache.find(id); it != oobbCache.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ // Try to get OOBB from repository.
+ simox::OrientedBoxf oobb;
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ oobb = objectInfo->oobb();
+ }
+ catch (const std::ios_base::failure&)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".";
+ }
+ }
+ oobbCache[id] = oobb; // Store result in any case.
+ return oobb;
+ }
+ }
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 2d6452899537ad83df272130162802062f544700..0c631b1e65bffbfd1199abccc016e98c5f1be724 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -120,11 +120,14 @@ namespace armarx
private:
+ void fetchProviderInfo(const std::string& providerName, bool refresh = false);
void handleProviderUpdate(const std::string& providerName);
void visProviderUpdate(const std::string& providerName);
objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
+ simox::OrientedBoxf getObjectOOBB(const ObjectID& id);
+
private:
@@ -140,13 +143,14 @@ namespace armarx
ObjectFinder objectFinder;
+ std::map<ObjectID, simox::OrientedBox<float>> oobbCache;
struct Visu
{
bool enabled = false;
bool inGlobalFrame = true;
-
float alpha = 1.0;
+ bool oobbs = false;
};
Visu visu;
@@ -160,7 +164,10 @@ namespace armarx
struct RemoteGuiTab : RemoteGui::Client::Tab
{
-
+ RemoteGui::Client::CheckBox visuEnabled;
+ RemoteGui::Client::CheckBox visuInGlobalFrame;
+ RemoteGui::Client::FloatSlider visuAlpha;
+ RemoteGui::Client::CheckBox visuOOBBs;
};
RemoteGuiTab tab;
};
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
deleted file mode 100644
index ecf05de2b31c102245e4fccfbf9663d43ab19f9b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include "ProvidedObjectPose.h"
-
-
-namespace armarx::objpose
-{
- ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
- {
-
- }
-}
-
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
deleted file mode 100644
index afc47fe515c61511c1ab225df37b2c96a48eaa2f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ProvidedObjectPose.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#pragma once
-
-#include <Eigen/Core>
-
-#include <RobotAPI/interface/objectpose/types.h>
-
-
-namespace armarx::objpose
-{
-
- struct ProvidedObjectPose
- {
- ProvidedObjectPose();
- ProvidedObjectPose(const data::ProvidedObjectPose& ice);
-
-
- /// Name of the providing component.
- std::string providerName;
- /// Known or unknown object.
- ObjectTypeEnum objectType = AnyObject;
-
- /// The object ID, i.e. dataset and name.
- ObjectID objectID;
-
- /// Pose in `objectPoseFrame`.
- Eigen::Matrix4f objectPose;
- std::string objectPoseFrame;
-
- /// Confidence in [0, 1] (1 = full, 0 = none).
- float confidence = 0;
- /// Source timestamp.
- long timestampMicroSeconds = -1;
-
- /// Object bounding box in object's local coordinate frame.
- Box localOOBB;
- };
-
-
-
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index 0085e76c8cca45298aad7c3cf11617c08c3e4ed8..506fc289280788d783f46455ead20a2ded115454 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -15,6 +15,31 @@
namespace armarx
{
+ Eigen::Vector3f toEigen(const Vector3BasePtr pose)
+ {
+ auto cast = Vector3Ptr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+ Eigen::Matrix3f toEigen(const QuaternionBasePtr pose)
+ {
+ auto cast = QuaternionPtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+ Eigen::Matrix4f toEigen(const PoseBasePtr pose)
+ {
+ auto cast = PosePtr::dynamicCast(pose);
+ ARMARX_CHECK_NOT_NULL(cast);
+ return cast->toEigen();
+ }
+
+
+ std::string objpose::getID(const ObjectID& id)
+ {
+ return id.dataset + "/" + id.name;
+ }
+
std::ostream& objpose::operator<<(std::ostream& os, const ObjectID& id)
{
return os << "'" << id.dataset << "/" << id.name << "'";
@@ -36,16 +61,53 @@ namespace armarx
return ice;
}
- objpose::Box objpose::toIce(const simox::OrientedBox<float>& oobb)
+
+ void objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb)
{
- objpose::Box ice;
- ice.position = new Vector3(oobb.center());
- ice.orientation = new Quaternion(oobb.rotation().eval());
- ice.extents = new Vector3(oobb.dimensions());
- return ice;
+ try
+ {
+ Eigen::Vector3f pos = toEigen(box.position);
+ Eigen::Matrix3f ori = toEigen(box.orientation);
+ Eigen::Vector3f extents = toEigen(box.extents);
+ Eigen::Vector3f corner = pos - ori * extents / 2;
+
+ oobb = simox::OrientedBox<float> (corner,
+ ori.col(0) * extents(0),
+ ori.col(1) * extents(1),
+ ori.col(2) * extents(2));
+ }
+ catch (const armarx::LocalException&)
+ {
+ // No OOBB information.
+ oobb = {};
+ }
+ }
+
+ simox::OrientedBoxf objpose::fromIce(const Box& box)
+ {
+ simox::OrientedBoxf oobb;
+ fromIce(box, oobb);
+ return oobb;
+ }
+
+ void objpose::toIce(Box& box, const simox::OrientedBoxf& oobb)
+ {
+ box.position = new Vector3(oobb.center());
+ box.orientation = new Quaternion(oobb.rotation().eval());
+ box.extents = new Vector3(oobb.dimensions());
}
+ objpose::Box objpose::toIce(const simox::OrientedBoxf& oobb)
+ {
+ objpose::Box box;
+ toIce(box, oobb);
+ return box;
+ }
+
+
}
+
+
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
index d71b34cae0b1e09f19ea32e37e5c5564aa70e2ea..b459a01cd9a7411cdbbb1c32dc2a15394c0dee25 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -19,12 +19,19 @@ namespace simox
namespace armarx::objpose
{
+ std::string getID(const ObjectID& id);
std::ostream& operator<<(std::ostream& os, const ObjectID& id);
extern const simox::meta::EnumNames<objpose::ObjectTypeEnum> ObjectTypeEnumNames;
objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb);
- objpose::Box toIce(const simox::OrientedBox<float>& oobb);
+
+
+ void fromIce(const Box& box, simox::OrientedBox<float>& oobb);
+ simox::OrientedBox<float> fromIce(const Box& box);
+
+ void toIce(Box& box, const simox::OrientedBox<float>& oobb);
+ Box toIce(const simox::OrientedBox<float>& oobb);
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
index c75c9d76a5398d870a06ad06597f405ade209cbb..88d11dc86bbd10ed4f324ee2b281c1b969583319 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
@@ -27,11 +27,42 @@
#include <RobotAPI/Test.h>
#include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
+#include <RobotAPI/components/ObjectPoseObserver/ice_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
+using namespace armarx;
+
+
+BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
{
- armarx::ObjectPoseObserver instance;
+ Eigen::Vector3f pos(-100, -200, -300);
+ Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
+ Eigen::Vector3f extents(40, 50, 60);
+
+ armarx::objpose::Box box;
+ box.position = new Vector3(pos);
+ box.orientation = new Quaternion(ori);
+ box.extents = new Vector3(extents);
+
+
+ const float prec = 1e-3;
+
+ simox::OrientedBoxf oobb;
+ armarx::objpose::fromIce(box, oobb);
+ ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
+ ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
+ ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
+
+ armarx::objpose::Box boxOut;
+ armarx::objpose::toIce(boxOut, oobb);
+
+ Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
+ Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
+ Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
- BOOST_CHECK_EQUAL(true, true);
+ ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
+ ARMARX_CHECK(oriOut.isApprox(ori, prec));
+ ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
}
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index 95c3e16c4995fd97aaaf7fb43b1aea114fc3d3f8..efa75d05dcf3051fcdb7ae16d29b151bcf4ebee4 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -15,3 +15,4 @@ add_subdirectory(CartesianWaypointControlGui)
add_subdirectory(DebugDrawerGuiPlugin)
add_subdirectory(ArViz)
add_subdirectory(CartesianNaturalPositionController)
+add_subdirectory(ObjectPoseGui)
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d4d29bd3e4181aa055f7dee976a778f75d42a877
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
@@ -0,0 +1,41 @@
+armarx_set_target("ObjectPoseGuiPlugin")
+
+# most qt components will be linked against in the call armarx_gui_library
+#armarx_find_qt(QtCore QtGui QtDesigner)
+
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+set(SOURCES
+ ObjectPoseGuiPlugin.cpp ObjectPoseGuiWidgetController.cpp
+)
+
+# do not rename this variable, it is used in armarx_gui_library()...
+set(HEADERS
+ ObjectPoseGuiPlugin.h ObjectPoseGuiWidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS
+ ObjectPoseGuiWidget.ui
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS
+ SimpleConfigDialog
+ ObjectPoseObserver
+)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library(ObjectPoseGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+
+ #find_package(MyLib QUIET)
+ #armarx_build_if(MyLib_FOUND "MyLib not available")
+ # all target_include_directories must be guarded by if(Xyz_FOUND)
+ # for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+ #if(MyLib_FOUND)
+ # target_include_directories(ObjectPoseGuiPlugi PUBLIC ${MyLib_INCLUDE_DIRS})
+ #endif()
+endif()
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01cff7dcbe628fc900c62a5ad74c75b3aa6fab4d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::ObjectPoseGuiPlugin
+ * \author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectPoseGuiPlugin.h"
+
+#include "ObjectPoseGuiWidgetController.h"
+
+namespace armarx
+{
+ ObjectPoseGuiPlugin::ObjectPoseGuiPlugin()
+ {
+ addWidget < ObjectPoseGuiWidgetController > ();
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..69625e6dc30760ebca74ba87db6b9338316185ac
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::ObjectPoseGui
+ * \author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+ /**
+ * \class ObjectPoseGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief ObjectPoseGuiPlugin brief description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT ObjectPoseGuiPlugin:
+ public armarx::ArmarXGuiPlugin
+ {
+ Q_OBJECT
+ Q_INTERFACES(ArmarXGuiInterface)
+ Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
+ public:
+ /**
+ * All widgets exposed by this plugin are added in the constructor
+ * via calls to addWidget()
+ */
+ ObjectPoseGuiPlugin();
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..83f77d3ae4886eaced3509fa4c1af1c62a21d80f
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ObjectPoseGuiWidget</class>
+ <widget class="QWidget" name="ObjectPoseGuiWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>400</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>ObjectPoseGuiWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QGroupBox" name="objectsGroupBox">
+ <property name="title">
+ <string>Objects</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="updateObjectsButton">
+ <property name="text">
+ <string>Update</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="autoUpdateCheckBox">
+ <property name="text">
+ <string>Auto Update</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QTableWidget" name="objectsTable"/>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..816e797f42714e8da58658d7bde649241fac6cf8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -0,0 +1,140 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::ObjectPoseGuiWidgetController
+ * \author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * \date 2020
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectPoseGuiWidgetController.h"
+
+#include <string>
+
+#include <QTimer>
+
+#include <RobotAPI/components/ObjectPoseObserver/ice_conversions.h>
+#include <RobotAPI/components/ObjectPoseObserver/ObjectPose.h>
+
+
+namespace armarx
+{
+
+ ObjectPoseGuiWidgetController::ObjectPoseGuiWidgetController()
+ {
+ widget.setupUi(getWidget());
+
+ QStringList header = {"Dataset", "Name", "Provider", "Type"};
+ widget.objectsTable->setColumnCount(header.size());
+ widget.objectsTable->setHorizontalHeaderLabels(header);
+
+ using This = ObjectPoseGuiWidgetController;
+
+ connect(widget.updateObjectsButton, &QPushButton::clicked, this, &This::updateObjects);
+ // QTimer* timer = new QTimer(this,);
+ }
+
+
+ ObjectPoseGuiWidgetController::~ObjectPoseGuiWidgetController()
+ {
+
+ }
+
+
+ void ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
+ {
+ (void) settings;
+ }
+
+ void ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
+ {
+ (void) settings;
+ }
+
+ QString ObjectPoseGuiWidgetController::GetWidgetName()
+ {
+ return "ObjectPoseGui";
+ }
+
+ static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseObserver";
+
+ QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (!configDialog)
+ {
+ configDialog = new SimpleConfigDialog(parent);
+ configDialog->addProxyFinder<armarx::objpose::ObjectPoseObserverInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "ObjectPoseObserver"});
+ }
+ return qobject_cast<QDialog*>(configDialog);
+ }
+
+ void ObjectPoseGuiWidgetController::configured()
+ {
+ if (configDialog)
+ {
+ objectPoseObserverName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
+ }
+ }
+
+ void ObjectPoseGuiWidgetController::onInitComponent()
+ {
+ if (!objectPoseObserverName.empty())
+ {
+ usingProxy(objectPoseObserverName);
+ }
+ }
+
+ void ObjectPoseGuiWidgetController::onConnectComponent()
+ {
+ if (!objectPoseObserverName.empty())
+ {
+ getProxy(objectPoseObserver, objectPoseObserverName);
+ }
+ }
+
+ void ObjectPoseGuiWidgetController::updateObjects()
+ {
+ if (!objectPoseObserver)
+ {
+ return;
+ }
+
+ ARMARX_INFO << "Get object poses...";
+ // const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPoseObserver->getObjectPoses());
+
+ const objpose::data::ObjectPoseSeq objectPoses = objectPoseObserver->getObjectPoses();
+ ARMARX_INFO << "Got " << objectPoses.size() << " object poses.";
+
+ widget.objectsTable->setRowCount(int(objectPoses.size()));
+
+ for (int i = 0; i < int(objectPoses.size()); ++i)
+ {
+ const objpose::data::ObjectPose& pose = objectPoses.at(size_t(i));
+ int col = 0;
+
+ widget.objectsTable->setItem(
+ i, col++, new QTableWidgetItem(pose.objectID.dataset.c_str()));
+ widget.objectsTable->setItem(
+ i, col++, new QTableWidgetItem(pose.objectID.name.c_str()));
+ widget.objectsTable->setItem(
+ i, col++, new QTableWidgetItem(pose.providerName.c_str()));
+ widget.objectsTable->setItem(
+ i, col++, new QTableWidgetItem(objpose::ObjectTypeEnumNames.to_name(pose.objectType).c_str()));
+
+ }
+ }
+
+}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..d9b716d7cc4186cbbff58858aaf25088726fa283
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -0,0 +1,113 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::gui-plugins::ObjectPoseGuiWidgetController
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/gui-plugins/ObjectPoseGui/ui_ObjectPoseGuiWidget.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+
+
+namespace armarx
+{
+ /**
+ @page RobotAPI-GuiPlugins-ObjectPoseGui ObjectPoseGui
+ @brief The ObjectPoseGui allows visualizing ...
+
+ @image html ObjectPoseGui.png
+ The user can
+
+ API Documentation @ref ObjectPoseGuiWidgetController
+
+ @see ObjectPoseGuiGuiPlugin
+ */
+
+ /**
+ * @class ObjectPoseGuiWidgetController
+ * @brief ObjectPoseGuiWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ ObjectPoseGuiWidgetController:
+ public armarx::ArmarXComponentWidgetControllerTemplate < ObjectPoseGuiWidgetController >
+ {
+ Q_OBJECT
+
+ public:
+ /// Controller Constructor
+ explicit ObjectPoseGuiWidgetController();
+
+ /// Controller destructor
+ virtual ~ObjectPoseGuiWidgetController() override;
+
+ /// Returns the Widget name displayed in the ArmarXGui to create an instance of this class.
+ static QString GetWidgetName();
+
+
+ /// @see ArmarXWidgetController::loadSettings()
+ void loadSettings(QSettings* settings) override;
+ /// @see ArmarXWidgetController::saveSettings()
+ void saveSettings(QSettings* settings) override;
+
+
+ QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+ void configured() override;
+
+
+ /// @see armarx::Component::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::Component::onConnectComponent()
+ void onConnectComponent() override;
+
+
+ public slots:
+ /* QT slot declarations */
+
+ void updateObjects();
+
+
+ signals:
+ /* QT signal declarations */
+
+ private:
+
+ /// Widget Form
+ Ui::ObjectPoseGuiWidget widget;
+
+ QPointer<SimpleConfigDialog> configDialog;
+
+
+ std::string objectPoseObserverName;
+ armarx::objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+
+
+ };
+}
+
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index c819dd3e7e82293c031e5c6d19267cb9a2b1c533..07ae9ab2f69565288eedace181c8a790506b0800 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -10,14 +10,16 @@ set(LIBS
set(LIB_FILES
ArmarXObjects.cpp
- ObjectFinder.cpp
+ ObjectID.cpp
ObjectInfo.cpp
+ ObjectFinder.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
- ObjectFinder.h
+ ObjectID.h
ObjectInfo.h
+ ObjectFinder.h
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index b7a74d925c6fc005cd506fff1f5a28558106c752..28634f811f1147c77dbe04271165ae4d86f45aec 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -4,7 +4,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
namespace armarx
@@ -67,18 +66,12 @@ namespace armarx
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 'Dataset/Name', but got: '" << nameOrID
- << "' (too many '/').";
- return findObject(split[0], split[1]);
- }
- else
- {
- return findObject("", nameOrID);
- }
+ return findObject(ObjectID(nameOrID));
+ }
+
+ std::optional<ObjectInfo> ObjectFinder::findObject(const ObjectID& id) const
+ {
+ return findObject(id.dataset(), id.name());
}
std::vector<std::string> ObjectFinder::getDatasets() const
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index e7f3170c1954e810a077c88c0ea2f3aa1cc9f898..80907d7b4a4fd5cdaf12d7c6372576987ee2df6e 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -25,6 +25,7 @@ namespace armarx
std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
+ std::optional<ObjectInfo> findObject(const ObjectID& id) const;
std::vector<std::string> getDatasets() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f24af1a926487d44b5efca3135813ddf70416ceb
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -0,0 +1,52 @@
+#include "ObjectID.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
+
+namespace armarx
+{
+ ObjectID::ObjectID()
+ {
+ }
+
+ ObjectID::ObjectID(const std::string& dataset, const std::string& name) :
+ _dataset(dataset), _name(name)
+ {
+ }
+
+ ObjectID::ObjectID(const std::string& nameOrID)
+ {
+ 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 'Dataset/Name', but got: '" << nameOrID
+ << "' (too many '/').";
+ _dataset = split[0];
+ _name = split[1];
+ }
+ else
+ {
+ // dataset is left empty.
+ _name = nameOrID;
+ }
+ }
+
+ bool ObjectID::operator==(const ObjectID& rhs) const
+ {
+ return _name == rhs._name && _dataset == rhs._dataset;
+ }
+
+ bool ObjectID::operator<(const ObjectID& rhs) const
+ {
+ return _dataset < rhs._dataset
+ || (_dataset == rhs._dataset && _name < rhs._name);
+ }
+
+}
+
+
+std::ostream& armarx::operator<<(std::ostream& os, const ObjectID& id)
+{
+ return os << "'" << id.str() << "'";
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
new file mode 100644
index 0000000000000000000000000000000000000000..437bbaa88e3de76657c42cd52e68799896c53e17
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <string>
+
+
+namespace armarx
+{
+ /**
+ * @brief A known object ID of the form "Dataset/Name".
+ */
+ class ObjectID
+ {
+ public:
+
+ ObjectID();
+ ObjectID(const std::string& dataset, const std::string& name);
+ /// Construct from either a name ("myobject") or ID ("mydataset/myobject").
+ ObjectID(const std::string& nameOrID);
+
+
+ std::string dataset() const
+ {
+ return _dataset;
+ }
+ std::string name() const
+ {
+ return _name;
+ }
+ /// Return "dataset/name".
+ std::string str() const
+ {
+ return _dataset + "/" + _name;
+ }
+
+ bool operator==(const ObjectID& rhs) const;
+ inline bool operator!=(const ObjectID& rhs) const
+ {
+ return !operator==(rhs);
+ }
+ bool operator< (const ObjectID& rhs) const;
+ inline bool operator> (const ObjectID& rhs) const
+ {
+ return rhs < (*this);
+ }
+ inline bool operator<=(const ObjectID& rhs) const
+ {
+ return !operator> (rhs);
+ }
+ inline bool operator>=(const ObjectID& rhs) const
+ {
+ return !operator< (rhs);
+ }
+
+
+ private:
+
+ std::string _dataset;
+ std::string _name;
+
+ };
+
+ std::ostream& operator<<(std::ostream& os, const ObjectID& rhs);
+
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index feaf2f203aca7445081f2f91e3d511a7b5bbdbd5..83f815b7a7cab582b8605119f69aad5a8758e14f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -12,9 +12,15 @@ namespace armarx
namespace fs = std::filesystem;
+ ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
+ const ObjectID& id) :
+ _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+ {
+ }
+
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
const std::string& dataset, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _dataset(dataset), _name(name)
+ _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
{
}
@@ -25,22 +31,27 @@ namespace armarx
std::string ObjectInfo::dataset() const
{
- return _dataset;
+ return _id.dataset();
}
std::string ObjectInfo::name() const
{
- return _name;
+ return _id.name();
+ }
+
+ ObjectID ObjectInfo::id() const
+ {
+ return _id;
}
- std::string ObjectInfo::id() const
+ std::string ObjectInfo::idStr() const
{
- return _dataset + "/" + _name;
+ return _id.str();
}
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / _dataset / _name;
+ return path(_packageName) / _id.dataset() / _id.name();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -50,7 +61,7 @@ namespace armarx
{
extension = "." + extension;
}
- std::string filename = _name + suffix + extension;
+ std::string filename = _id.name() + suffix + extension;
PackageFileLocation loc;
loc.package = _packageName;
@@ -86,10 +97,11 @@ namespace armarx
simox::AxisAlignedBoundingBox aabb(min, max);
- ARMARX_CHECK(aabb.center().isApprox(center)) << aabb.center().transpose() << "\n" << center.transpose();
- ARMARX_CHECK(aabb.extents().isApprox(extents)) << aabb.extents().transpose() << "\n" << extents.transpose();
- ARMARX_CHECK(aabb.min().isApprox(min)) << aabb.min().transpose() << "\n" << min.transpose();
- ARMARX_CHECK(aabb.max().isApprox(max)) << aabb.max().transpose() << "\n" << max.transpose();
+ 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();
return aabb;
}
@@ -109,9 +121,10 @@ namespace armarx
ori.col(1) * extents(1),
ori.col(2) * extents(2));
- ARMARX_CHECK(oobb.center().isApprox(pos)) << oobb.center().transpose() << "\n" << pos.transpose();
- ARMARX_CHECK(oobb.rotation().isApprox(ori)) << oobb.rotation() << "\n" << ori;
- ARMARX_CHECK(oobb.dimensions().isApprox(extents)) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+ 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();
return oobb;
}
@@ -136,11 +149,10 @@ namespace armarx
}
-namespace armarx
+
+std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
- std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs)
- {
- return os << "'" << rhs.id() << "'";
- }
+ return os << rhs.id();
}
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 0fd1ef03216ba8fdbb620462306b9779baeabde9..c141bd9d467857c8869b60c2b5c5eebdd48697d4 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -3,6 +3,8 @@
#include <filesystem>
#include <string>
+#include "ObjectID.h"
+
namespace simox
{
@@ -37,9 +39,12 @@ namespace armarx
public:
+ ObjectInfo(const std::string& packageName, const path& packageDataDir,
+ const ObjectID& id);
ObjectInfo(const std::string& packageName, const path& packageDataDir,
const std::string& dataset, const std::string& name);
+
virtual ~ObjectInfo() = default;
@@ -48,7 +53,8 @@ namespace armarx
std::string dataset() const;
std::string name() const;
/// Return "dataset/name".
- std::string id() const;
+ ObjectID id() const;
+ std::string idStr() const;
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
@@ -79,11 +85,36 @@ namespace armarx
std::string _packageName;
path _packageDataDir;
- std::string _dataset;
- std::string _name;
+ ObjectID _id;
};
std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);
+
+ inline bool operator==(const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() == rhs.id();
+ }
+ inline bool operator!=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() != rhs.id();
+ }
+ inline bool operator< (const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() < rhs.id();
+ }
+ inline bool operator> (const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() > rhs.id();
+ }
+ inline bool operator<=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() <= rhs.id();
+ }
+ inline bool operator>=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+ {
+ return lhs.id() >= rhs.id();
+ }
+
}