From b380e7171169fa761a4cafea372484640869d17f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 26 Apr 2021 11:20:35 +0200
Subject: [PATCH] Allow to view object classes via remote gui
---
.../server/ObjectMemory/ObjectMemory.cpp | 3 +
.../armem_objects/server/class/Segment.cpp | 156 ++++++++++++++++--
.../armem_objects/server/class/Segment.h | 38 ++++-
3 files changed, 183 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 8df7556a9..ed3937cb2 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -109,6 +109,9 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::arviz,
debugObserver
);
+ classSegment.connect(
+ ArVizComponentPluginUser::arviz
+ );
createRemoteGuiTab();
RemoteGui_startRunningTask();
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 1a42f6968..eb460d47f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
@@ -7,6 +8,8 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
@@ -46,6 +49,11 @@ namespace armarx::armem::server::obj::clazz
}
}
+ void Segment::connect(viz::Client arviz)
+ {
+ this->arviz = arviz;
+ }
+
void Segment::loadByObjectFinder(const std::string& objectsPackage)
{
@@ -90,6 +98,64 @@ namespace armarx::armem::server::obj::clazz
iceMemory.commit(commit);
}
+ void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
+ {
+ const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ viz::Layer layerOrigin = arviz.layer("Origin");
+ layerOrigin.add(viz::Pose("Origin"));
+
+ viz::Layer layerObject = arviz.layer("Class Model");
+ viz::Layer layerAABB = arviz.layer("Class AABB");
+ viz::Layer layerOOBB = arviz.layer("Class OOBB");
+
+ if (coreSegment)
+ {
+ try
+ {
+ armem::Entity& entity = coreSegment->getEntity(entityID);
+ armem::EntityInstance& instance = entity.getLatestSnapshot().getInstance(0);
+
+ arondto::ObjectClass aron;
+ aron.fromAron(instance.data());
+
+ if (!aron.simoxXmlPath.package.empty())
+ {
+ layerObject.add(viz::Object(entityID.str())
+ .file(aron.simoxXmlPath.package, aron.simoxXmlPath.path)
+ .pose(pose));
+ }
+
+ if (showAABB)
+ {
+ layerAABB.add(viz::Box("AABB")
+ .pose(pose * simox::math::pose(aron.aabb.center))
+ .size(aron.aabb.extents)
+ .color(simox::Color::cyan(255, 64)));
+ }
+ if (showOOBB)
+ {
+ layerOOBB.add(viz::Box("OOBB")
+ .pose(pose * simox::math::pose(aron.oobb.center, aron.oobb.orientation))
+ .size(aron.oobb.extents)
+ .color(simox::Color::lime(255, 64)));
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ }
+
+ arviz.commit(layerObject, layerOrigin, layerAABB, layerOOBB);
+ }
+
arondto::ObjectClass Segment::objectClassFromInfo(const ObjectInfo& info)
{
namespace fs = std::filesystem;
@@ -129,13 +195,34 @@ namespace armarx::armem::server::obj::clazz
}
- void Segment::RemoteGui::setup(const Segment& data)
+ void Segment::RemoteGui::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ data.setup(segment);
+ visu.setup(segment);
+
+ VBoxLayout layout;
+ layout.addChildren({data.group, visu.group});
+
+ group = {};
+ group.setLabel("Class");
+ group.addChildren({layout, VSpacer()});
+ }
+
+ void Segment::RemoteGui::update(Segment& segment)
+ {
+ data.update(segment);
+ visu.update(segment);
+ }
+
+ void Segment::RemoteGui::Data::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
- maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ infiniteHistory.setValue(segment.p.maxHistorySize == -1);
GridLayout grid;
int row = 0;
@@ -145,19 +232,68 @@ namespace armarx::armem::server::obj::clazz
row++;
group = {};
- group.setLabel("Class");
- group.addChildren({grid, VSpacer()});
+ group.setLabel("Data");
+ group.addChild(grid);
}
- void Segment::RemoteGui::update(Segment& data)
+ void Segment::RemoteGui::Data::update(Segment& segment)
{
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- std::scoped_lock lock(data.memoryMutex);
- data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (data.coreSegment)
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.p.maxHistorySize));
+ }
+ }
+ }
+
+
+
+ void Segment::RemoteGui::Visu::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showComboBox = {};
+ showOptionsIndex.clear();
+ for (const auto& [_, prov] : *segment.coreSegment)
+ {
+ for (const auto& [_, entity] : prov)
+ {
+ std::stringstream option;
+ option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
+ showComboBox.addOption(option.str());
+ showOptionsIndex.push_back(entity.id());
+ }
+ }
+ if (showOptionsIndex.empty())
+ {
+ showComboBox.addOption("<none>");
+ }
+ showButton.setLabel("Visualize Object Class");
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(showComboBox, {row, 0}, {1, 2});
+ row++;
+ grid.add(showButton, {row, 0}, {1, 2});
+ row++;
+
+ group = {};
+ group.setLabel("Visualization");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::Visu::update(Segment& segment)
+ {
+ if (showButton.wasClicked())
+ {
+ const size_t index = static_cast<size_t>(showComboBox.getIndex());
+ if (/*index >= 0 &&*/ index < showOptionsIndex.size())
{
- data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.visualizeClass(showOptionsIndex.at(index));
}
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 6ba5409cd..97c0af8d6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -8,6 +8,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
@@ -29,11 +30,15 @@ namespace armarx::armem::server::obj::clazz
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
void init();
+ void connect(viz::Client arviz);
void loadByObjectFinder(const std::string& objectsPackage);
void loadByObjectFinder(const ObjectFinder& finder);
void loadByObjectFinder();
+ void visualizeClass(const MemoryID& entityID, bool showAABB = true, bool showOOBB = true);
+
+
static arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
@@ -45,6 +50,8 @@ namespace armarx::armem::server::obj::clazz
ObjectFinder objectFinder;
+ viz::Client arviz;
+
struct Properties
{
@@ -63,11 +70,34 @@ namespace armarx::armem::server::obj::clazz
{
armarx::RemoteGui::Client::GroupBox group;
- armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ struct Data
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Data data;
+
+ struct Visu
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ std::vector<MemoryID> showOptionsIndex;
+ armarx::RemoteGui::Client::ComboBox showComboBox;
+ armarx::RemoteGui::Client::Button showButton;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Visu visu;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
- void setup(const Segment& data);
- void update(Segment& data);
};
};
--
GitLab