diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index 0289d37380d714ea71f0b427e2504c40fd31eed7..ae8531edacea24b495fac67c39fd50f51a927835 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -45,7 +45,7 @@ namespace armarx::objpose
{
providerName = ice.providerName;
objectType = ice.objectType;
- objectID = { ice.objectID.dataset, ice.objectID.name };
+ objectID = { ice.objectID.dataset, ice.objectID.className, ice.objectID.instanceName };
objectPoseRobot = armarx::objpose::toEigen(ice.objectPoseRobot);
objectPoseGlobal = armarx::objpose::toEigen(ice.objectPoseGlobal);
@@ -72,7 +72,7 @@ namespace armarx::objpose
{
ice.providerName = providerName;
ice.objectType = objectType;
- ice.objectID = { objectID.dataset(), objectID.name() };
+ ice.objectID = { objectID.dataset(), objectID.className(), objectID.instanceName() };
ice.objectPoseRobot = new Pose(objectPoseRobot);
ice.objectPoseGlobal = new Pose(objectPoseGlobal);
@@ -93,7 +93,7 @@ namespace armarx::objpose
providerName = provided.providerName;
objectType = provided.objectType;
- objectID = { provided.objectID.dataset, provided.objectID.name };
+ objectID = { provided.objectID.dataset, provided.objectID.className, provided.objectID.instanceName };
objectPoseOriginal = armarx::objpose::toEigen(provided.objectPose);
objectPoseOriginalFrame = provided.objectPoseFrame;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index f7d2eee97b3ec601420ce9cf010af23817814dd7..9343a88d5589a19d832eadfdca2c4125494e226d 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -152,10 +152,18 @@ namespace armarx
robotNode->setJointValue(value + calibration.offset);
}
- for (const auto& provided : providedPoses)
+ for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
objpose::ObjectPose& pose = objectPoses.emplace_back();
pose.fromProvidedPose(provided, robot);
+ if (pose.objectID.dataset().empty())
+ {
+ // Try to find the data set. (It might be good to cache this.)
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(pose.objectID))
+ {
+ pose.objectID = { objectInfo->dataset(), pose.objectID.className(), pose.objectID.instanceName() };
+ }
+ }
if (!(provided.localOOBB.position && provided.localOOBB.orientation && provided.localOOBB.extents))
{
pose.localOOBB = getObjectOOBB(pose.objectID);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
index 506fc289280788d783f46455ead20a2ded115454..994298d38c47faace57a85e6c4e87e169ecbeb45 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp
@@ -35,14 +35,14 @@ namespace armarx
}
- 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 << "'";
+ os << "'" << id.dataset << "/" << id.className;
+ if (!id.instanceName.empty())
+ {
+ os << "/" << id.instanceName;
+ }
+ return os << "'";
}
const simox::meta::EnumNames<objpose::ObjectTypeEnum> objpose::ObjectTypeEnumNames =
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
index b459a01cd9a7411cdbbb1c32dc2a15394c0dee25..83f9b43edb246bfaf7c0b748dfac1f2b0721726e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h
@@ -19,7 +19,6 @@ 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;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 37c660527f62b0e50549e6297ec9d31fc06ab350..e4fa766c3249b699f65fe1105977626adbd22b10 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -30,6 +30,9 @@
#include <QMessageBox>
#include <QTimer>
+#define ENABLE_INTROSPECTION 0
+
+
namespace armarx
{
struct ArVizWidgetBatchCallback : IceUtil::Shared
@@ -85,7 +88,7 @@ namespace armarx
// Layer info tree.
connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
-#if 0
+#if ENABLE_INTROSPECTION
connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
&layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value());
@@ -291,7 +294,7 @@ namespace armarx
void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
{
-#if 0
+#if ENABLE_INTROSPECTION
(void) previous;
if (!current->parent())
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 816e797f42714e8da58658d7bde649241fac6cf8..9c97a261eec0c4aadb303ac8e2f61607202bbf75 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -37,7 +37,7 @@ namespace armarx
{
widget.setupUi(getWidget());
- QStringList header = {"Dataset", "Name", "Provider", "Type"};
+ QStringList header = {"Dataset", "ClassName", "InstanceName", "Provider", "Type"};
widget.objectsTable->setColumnCount(header.size());
widget.objectsTable->setHorizontalHeaderLabels(header);
@@ -128,7 +128,9 @@ namespace armarx
widget.objectsTable->setItem(
i, col++, new QTableWidgetItem(pose.objectID.dataset.c_str()));
widget.objectsTable->setItem(
- i, col++, new QTableWidgetItem(pose.objectID.name.c_str()));
+ i, col++, new QTableWidgetItem(pose.objectID.className.c_str()));
+ widget.objectsTable->setItem(
+ i, col++, new QTableWidgetItem(pose.objectID.instanceName.c_str()));
widget.objectsTable->setItem(
i, col++, new QTableWidgetItem(pose.providerName.c_str()));
widget.objectsTable->setItem(
diff --git a/source/RobotAPI/interface/objectpose/types.ice b/source/RobotAPI/interface/objectpose/types.ice
index eaf7adf779b0ac6d51c952f343f08d75bd87d93c..097c6890165ed7c943630cc845b5bceb73c1294c 100644
--- a/source/RobotAPI/interface/objectpose/types.ice
+++ b/source/RobotAPI/interface/objectpose/types.ice
@@ -38,8 +38,12 @@ module armarx
struct ObjectID
{
- string dataset; ///< e.g. "KIT", "YCB", "SecondHands", ...
- string name; ///< e.g. "Amicelli", "001_chips_can", ...
+ /// The dataset name in ArmarXObjects, e.g. "KIT", "YCB", "SecondHands", ...
+ string dataset;
+ /// The class name in ArmarXObjects, e.g. "Amicelli", "001_chips_can", ...
+ string className;
+ /// An optional instance name, chosen by the provider.
+ string instanceName;
};
sequence<ObjectID> ObjectIDSeq;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 28634f811f1147c77dbe04271165ae4d86f45aec..a02b6bd1031836da23ce9617e7e5f55d7075d2d3 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -71,7 +71,7 @@ namespace armarx
std::optional<ObjectInfo> ObjectFinder::findObject(const ObjectID& id) const
{
- return findObject(id.dataset(), id.name());
+ return findObject(id.dataset(), id.className());
}
std::vector<std::string> ObjectFinder::getDatasets() const
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index f24af1a926487d44b5efca3135813ddf70416ceb..796c658992a1db36d884ff3f510fa994dc5bc80c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -1,7 +1,7 @@
#include "ObjectID.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx
@@ -10,8 +10,8 @@ namespace armarx
{
}
- ObjectID::ObjectID(const std::string& dataset, const std::string& name) :
- _dataset(dataset), _name(name)
+ ObjectID::ObjectID(const std::string& dataset, const std::string& className, const std::string& instancName) :
+ _dataset(dataset), _className(className), _instanceName(instancName)
{
}
@@ -19,28 +19,50 @@ namespace armarx
{
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 '/').";
+ const std::vector<std::string> split = simox::alg::split(nameOrID, "/", true);
+ ARMARX_CHECK(split.size() == 2 || split.size() == 3)
+ << "Expected ID of format 'Dataset/ClassName' or 'Dataset/ClassName/InstanceName'"
+ << ", but got: '" << nameOrID << "' (too many '/').";
_dataset = split[0];
- _name = split[1];
+ _className = split[1];
}
else
{
// dataset is left empty.
- _name = nameOrID;
+ _className = nameOrID;
}
}
+ std::string ObjectID::str() const
+ {
+ std::string _str = _dataset + "/" + _className;
+ if (!_instanceName.empty())
+ {
+ _str += "/" + _instanceName;
+ }
+ return _str;
+ }
+
bool ObjectID::operator==(const ObjectID& rhs) const
{
- return _name == rhs._name && _dataset == rhs._dataset;
+ return _className == rhs._className
+ && _dataset == rhs._dataset
+ && _instanceName == rhs._instanceName;
}
bool ObjectID::operator<(const ObjectID& rhs) const
{
- return _dataset < rhs._dataset
- || (_dataset == rhs._dataset && _name < rhs._name);
+ if (_dataset != rhs._dataset)
+ {
+ return _dataset < rhs._dataset;
+ }
+ // equal dataset
+ if (_className != rhs._className)
+ {
+ return _className < rhs._className;
+ }
+ // equal class name
+ return _instanceName < rhs._instanceName;
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 437bbaa88e3de76657c42cd52e68799896c53e17..263af5ae61d673a945c401cc344c1d6630e279b5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -6,32 +6,35 @@
namespace armarx
{
/**
- * @brief A known object ID of the form "Dataset/Name".
+ * @brief A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
*/
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& dataset, const std::string& className, const std::string& instancName = "");
+ /// Construct from either a name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
ObjectID(const std::string& nameOrID);
- std::string dataset() const
+ inline std::string dataset() const
{
return _dataset;
}
- std::string name() const
+ inline std::string className() const
{
- return _name;
+ return _className;
}
- /// Return "dataset/name".
- std::string str() const
+ inline std::string instanceName() const
{
- return _dataset + "/" + _name;
+ return _instanceName;
}
+ /// Return "dataset/className" or "dataset/className/instanceName".
+ std::string str() const;
+
+
bool operator==(const ObjectID& rhs) const;
inline bool operator!=(const ObjectID& rhs) const
{
@@ -54,8 +57,12 @@ namespace armarx
private:
+ /// The dataset name in ArmarXObjects, e.g. "KIT", "YCB", "SecondHands", ...
std::string _dataset;
- std::string _name;
+ /// The class name in ArmarXObjects, e.g. "Amicelli", "001_chips_can", ...
+ std::string _className;
+ /// An optional instance name, chosen by the user.
+ std::string _instanceName;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 83f815b7a7cab582b8605119f69aad5a8758e14f..ac272b10abe1bc26e47a52527df38034b2f24905 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -36,7 +36,7 @@ namespace armarx
std::string ObjectInfo::name() const
{
- return _id.name();
+ return _id.className();
}
ObjectID ObjectInfo::id() const
@@ -51,7 +51,7 @@ namespace armarx
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / _id.dataset() / _id.name();
+ return path(_packageName) / _id.dataset() / _id.className();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -61,7 +61,7 @@ namespace armarx
{
extension = "." + extension;
}
- std::string filename = _id.name() + suffix + extension;
+ std::string filename = _id.className() + suffix + extension;
PackageFileLocation loc;
loc.package = _packageName;