diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 867170f06d57371fee6e25cda2f28ea4dffdda79..f369dd0247620277b719e59891bbff9c88d13e6e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -5,6 +5,7 @@ set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces
ArmarXGuiComponentPlugins
RobotAPIArmarXObjects RobotAPIComponentPlugins
+ ArViz
${PROJECT_NAME}Interfaces
)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index ae8531edacea24b495fac67c39fd50f51a927835..af808d1de3634821a761e2ae4bb3183aa1e2bcfd 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -113,6 +113,25 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+
+ std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
+ {
+ if (localOOBB)
+ {
+ return localOOBB->transformed(objectPoseRobot);
+ }
+ return std::nullopt;
+ }
+
+ std::optional<simox::OrientedBoxf> ObjectPose::oobbGlobal() const
+ {
+ if (localOOBB)
+ {
+ return localOOBB->transformed(objectPoseGlobal);
+ }
+ return std::nullopt;
+ }
+
}
namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
index 78cbfbfc6bbe46bd0dc8c24995dfc33973f68e3b..d00e548b82bc9bdd72a46aeb997a6eb297e89ba4 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -54,7 +54,15 @@ namespace armarx::objpose
IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
/// Object bounding box in object's local coordinate frame.
+ /// @see oobbRobot(), oobbGlobal()
std::optional<simox::OrientedBoxf> localOOBB;
+
+
+ /// Get the OOBB in the robot frame (according to `objectPoseRobot`).
+ std::optional<simox::OrientedBoxf> oobbRobot() const;
+ /// Get the OOBB in the global frame (according to `objectPoseGlobal`).
+ std::optional<simox::OrientedBoxf> oobbGlobal() const;
+
};
using ObjectPoseSeq = std::vector<ObjectPose>;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 8f24af53ed2dca19b6c8b273da70842bbc26dc48..fda9571084893b61eef4cdde94a599720bc3fd34 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -53,6 +53,9 @@ namespace armarx
defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+ defs->optional(visu.oobbs, "visu.oobbs", "Enable showing oriented bounding boxes.");
+ defs->optional(visu.objectFrames, "visu.objectFrames", "Enable showing object frames.");
+ defs->optional(visu.objectFramesScale, "visu.objectFramesScale", "Scaling of object frames.");
defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated");
defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated");
@@ -100,6 +103,14 @@ namespace armarx
tab.visuAlpha.setRange(0, 1.0);
tab.visuAlpha.setValue(visu.alpha);
tab.visuOOBBs.setValue(visu.oobbs);
+ tab.visuObjectFrames.setValue(visu.objectFrames);
+ {
+ float max = 10000;
+ tab.visuObjectFramesScale.setRange(0, max);
+ tab.visuObjectFramesScale.setDecimals(2);
+ tab.visuObjectFramesScale.setSteps(int(10 * max));
+ tab.visuObjectFramesScale.setValue(visu.objectFramesScale);
+ }
GroupBox visuGroup;
{
@@ -109,10 +120,13 @@ namespace armarx
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});
+ grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1}, {1, 3});
row++;
grid.add(Label("OOBB"), {row, 0}).add(tab.visuOOBBs, {row, 1});
row++;
+ grid.add(Label("Object Frames"), {row, 0}).add(tab.visuObjectFrames, {row, 1});
+ grid.add(Label("Scale:"), {row, 2}).add(tab.visuObjectFramesScale, {row, 3});
+ row++;
visuGroup.setLabel("Visualization");
visuGroup.addChild(grid);
@@ -131,6 +145,8 @@ namespace armarx
visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
visu.alpha = tab.visuAlpha.getValue();
visu.oobbs = tab.visuOOBBs.getValue();
+ visu.objectFrames = tab.visuObjectFrames.getValue();
+ visu.objectFramesScale = tab.visuObjectFramesScale.getValue();
}
@@ -372,26 +388,23 @@ namespace armarx
const armarx::ObjectID id = objectPose.objectID;
std::string key = id.str();
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
-
Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
-
- if (objectInfo)
{
- viz::Object object = viz::Object(key)
- .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
- .pose(pose);
+ viz::Object object = viz::Object(key).pose(pose);
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
if (visu.alpha < 1)
{
object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
}
-
layer.add(object);
}
- else
- {
- ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
- }
if (visu.oobbs && objectPose.localOOBB)
{
@@ -400,6 +413,10 @@ namespace armarx
.pose(pose * simox::math::pose(oobb.center(), oobb.rotation()))
.size(oobb.dimensions()).color(simox::Color::lime(255, 64)));
}
+ if (visu.objectFrames)
+ {
+ layer.add(viz::Pose(key + " Pose").pose(pose).scale(visu.objectFramesScale));
+ }
}
arviz.commit(layer);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index c6271e22e5edcc83b1cb9d532da6405a31920d30..1e7197d3b068ad58fad6a01912b6f9e7d6b51a6f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -152,6 +152,8 @@ namespace armarx
bool inGlobalFrame = true;
float alpha = 1.0;
bool oobbs = false;
+ bool objectFrames = false;
+ float objectFramesScale = 1.0;
};
Visu visu;
@@ -169,6 +171,8 @@ namespace armarx
RemoteGui::Client::CheckBox visuInGlobalFrame;
RemoteGui::Client::FloatSlider visuAlpha;
RemoteGui::Client::CheckBox visuOOBBs;
+ RemoteGui::Client::CheckBox visuObjectFrames;
+ RemoteGui::Client::FloatSpinBox visuObjectFramesScale;
};
RemoteGuiTab tab;
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index 796c658992a1db36d884ff3f510fa994dc5bc80c..b756dc4aaac803245000b0deac6524ec7615a7bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -25,10 +25,14 @@ namespace armarx
<< ", but got: '" << nameOrID << "' (too many '/').";
_dataset = split[0];
_className = split[1];
+ if (split.size() == 3)
+ {
+ _instanceName = split[2];
+ }
}
else
{
- // dataset is left empty.
+ // Dataset, instanceName are left empty.
_className = nameOrID;
}
}
@@ -43,6 +47,11 @@ namespace armarx
return _str;
}
+ bool ObjectID::equalClass(const ObjectID& rhs) const
+ {
+ return _className == rhs._className && _dataset == rhs._dataset;
+ }
+
bool ObjectID::operator==(const ObjectID& rhs) const
{
return _className == rhs._className
@@ -52,14 +61,16 @@ namespace armarx
bool ObjectID::operator<(const ObjectID& rhs) const
{
- if (_dataset != rhs._dataset)
+ int c = _dataset.compare(rhs._dataset);
+ if (c != 0)
{
- return _dataset < rhs._dataset;
+ return c < 0;
}
// equal dataset
- if (_className != rhs._className)
+ c = _className.compare(rhs._className);
+ if (c != 0)
{
- return _className < rhs._className;
+ return c < 0;
}
// equal class name
return _instanceName < rhs._instanceName;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 2b953ea4c2d126caaa0b41db612704e0649d284f..0d3bfb128d35ce5fa879c7abbbb24ff34404e7b7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -14,7 +14,7 @@ namespace armarx
ObjectID();
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").
+ /// Construct from either a class name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
ObjectID(const std::string& nameOrID);
@@ -38,7 +38,10 @@ namespace armarx
/// Return "dataset/className" or "dataset/className/instanceName".
std::string str() const;
+ /// Indicates whether dataset and class name are equal.
+ bool equalClass(const ObjectID& rhs) const;
+ /// Indicates whether dataset, class name and instance name are equal.
bool operator==(const ObjectID& rhs) const;
inline bool operator!=(const ObjectID& rhs) const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
index 2ad25f7ea27f28908cf50d0c950ba4170487fc85..4fe275d872221c3c49e83d9d295917e5d809c401 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/CMakeLists.txt
@@ -3,3 +3,4 @@
SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
armarx_add_test(ArmarXObjectsTest ArmarXObjectsTest.cpp "${LIBS}")
+armarx_add_test(ArmarXObjects_ObjectIDTest ObjectIDTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ca0f7001ebdeeff9e164b44179fca8156a10f003
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
@@ -0,0 +1,121 @@
+/*
+ * 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::ArmarXObjects::ArmarXObjects
+ * @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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::ArmarXObjects
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArmarXObjects.h"
+
+#include "../ObjectID.h"
+
+#include <iostream>
+
+
+namespace
+{
+ struct Fixture
+ {
+ std::vector<armarx::ObjectID> dcs
+ {
+ { "Data/Class/0" },
+ { "Data/Class/1" },
+ { "Data/Class/2" }
+ };
+ armarx::ObjectID dc0 { "Data/Class/0" };
+
+ std::vector<armarx::ObjectID> ots
+ {
+ { "Other/Type/0" },
+ { "Other/Type/1" },
+ { "Other/Type/2" }
+ };
+ armarx::ObjectID ot0 { "Other/Type/0" };
+ };
+}
+
+BOOST_FIXTURE_TEST_SUITE(ObjectIDTests, Fixture)
+
+BOOST_AUTO_TEST_CASE(test_construction_from_string)
+{
+ for (std::size_t i = 0; i < dcs.size(); ++i)
+ {
+ BOOST_CHECK_EQUAL(dcs[i].dataset(), "Data");
+ BOOST_CHECK_EQUAL(dcs[i].className(), "Class");
+ BOOST_CHECK_EQUAL(dcs[i].instanceName(), std::to_string(i));
+
+ BOOST_CHECK_EQUAL(ots[i].dataset(), "Other");
+ BOOST_CHECK_EQUAL(ots[i].className(), "Type");
+ BOOST_CHECK_EQUAL(ots[i].instanceName(), std::to_string(i));
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_equals_operator)
+{
+ for (std::size_t i = 0; i < dcs.size(); ++i)
+ {
+ BOOST_TEST_CONTEXT("i=" << i)
+ {
+ BOOST_CHECK_EQUAL(dcs[i], dcs[i]);
+ BOOST_CHECK_NE(dcs[i], ots[i]);
+ if (i != 0)
+ {
+ BOOST_CHECK_NE(dcs[i], dc0);
+ }
+ }
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_less_than_operator)
+{
+ for (std::size_t i = 0; i < dcs.size(); ++i)
+ {
+ for (std::size_t j = i; j < dcs.size(); ++j)
+ {
+ BOOST_CHECK_LE(dcs[i], dcs[j]);
+ BOOST_CHECK_GE(dcs[j], dcs[i]);
+ if (i != j)
+ {
+ BOOST_CHECK_LT(dcs[i], dcs[j]);
+ BOOST_CHECK_GT(dcs[j], dcs[i]);
+ }
+ }
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_equalClass)
+{
+ for (std::size_t i = 0; i < dcs.size(); ++i)
+ {
+ for (std::size_t j = 0; j < dcs.size(); ++j)
+ {
+ BOOST_CHECK(dcs[i].equalClass(dcs[j]));
+ BOOST_CHECK(ots[i].equalClass(ots[j]));
+
+ BOOST_CHECK(!dcs[i].equalClass(ots[j]));
+ BOOST_CHECK(!ots[i].equalClass(dcs[j]));
+ }
+ }
+}
+
+BOOST_AUTO_TEST_SUITE_END()