diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
index 0a8bbb2f936f3ac863cbfb1dba62c0dcfc501591..bc81675822ad2ffa567ce29a1d40de523d5f8f9f 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>522</width>
- <height>378</height>
+ <width>621</width>
+ <height>350</height>
</rect>
</property>
<property name="windowTitle">
@@ -56,7 +56,75 @@
</attribute>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
- <widget class="QTableWidget" name="objectsTable"/>
+ <widget class="QTreeWidget" name="objectsTree">
+ <property name="alternatingRowColors">
+ <bool>true</bool>
+ </property>
+ <column>
+ <property name="text">
+ <string>ID</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Provider</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Type</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>OOBB</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Confidence</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Timestamp</string>
+ </property>
+ </column>
+ <item>
+ <property name="text">
+ <string>Provider2</string>
+ </property>
+ <item>
+ <property name="text">
+ <string>Dataset1/Object2</string>
+ </property>
+ <property name="text">
+ <string>Provider2</string>
+ </property>
+ </item>
+ </item>
+ <item>
+ <property name="text">
+ <string>Provider1</string>
+ </property>
+ <item>
+ <property name="text">
+ <string>Dataset2/Object2/instance1</string>
+ </property>
+ <property name="text">
+ <string>Provider1</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>Dataset1/Object1</string>
+ </property>
+ <property name="text">
+ <string>Provider1</string>
+ </property>
+ </item>
+ </item>
+ </widget>
</item>
</layout>
</widget>
@@ -72,18 +140,12 @@
<property name="alternatingRowColors">
<bool>true</bool>
</property>
- <property name="sortingEnabled">
- <bool>true</bool>
- </property>
<property name="columnCount">
<number>3</number>
</property>
<attribute name="headerCascadingSectionResizes">
<bool>true</bool>
</attribute>
- <attribute name="headerShowSortIndicator" stdset="0">
- <bool>true</bool>
- </attribute>
<attribute name="headerStretchLastSection">
<bool>true</bool>
</attribute>
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index ae76b958a2a21ea42459627413b7a81f5163567f..5742df60a925fb61a927772085dd66871caff540 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -26,6 +26,8 @@
#include <QTimer>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
#include <RobotAPI/components/ObjectPoseObserver/ice_conversions.h>
#include <RobotAPI/components/ObjectPoseObserver/ObjectPose.h>
@@ -37,10 +39,8 @@ namespace armarx
{
widget.setupUi(getWidget());
- QStringList header = {"Dataset", "ClassName", "InstanceName", "Provider", "Type"};
- widget.objectsTable->setColumnCount(header.size());
- widget.objectsTable->setHorizontalHeaderLabels(header);
-
+ widget.objectsTree->clear();
+ widget.objectsTree->sortItems(0, Qt::SortOrder::AscendingOrder);
widget.requestTree->clear();
widget.requestTree->sortItems(0, Qt::SortOrder::AscendingOrder);
@@ -53,7 +53,20 @@ namespace armarx
connect(widget.requestButton, &QPushButton::pressed, this, &This::requestSelectedObjects);
- // QTimer* timer = new QTimer(this,);
+ QTimer* timer = new QTimer(this);
+ timer->setInterval(500);
+ connect(timer, &QTimer::timeout, this, &This::updateTab);
+ connect(widget.autoUpdateCheckBox, &QCheckBox::toggled, this, [timer](bool checked)
+ {
+ if (checked)
+ {
+ timer->start();
+ }
+ else
+ {
+ timer->stop();
+ }
+ });
}
@@ -130,6 +143,196 @@ namespace armarx
}
}
+
+ template <class ContainerT>
+ struct TreeVisitor
+ {
+ using ElementT = typename ContainerT::value_type;
+
+ /// Return < 0 if `element < item`, 0 if `element == item`, and > 0 if `element > item`.
+ using CompareFn = std::function<int(const ElementT& element, QTreeWidgetItem* item)>;
+ using NameFn = std::function<std::string(const ElementT& element)>;
+ using MakeItemFn = std::function<QTreeWidgetItem*(const ElementT& element)>;
+ using UpdateItemFn = std::function<bool(const ElementT& element, QTreeWidgetItem* item)>;
+
+ TreeVisitor() = default;
+ TreeVisitor(const ContainerT&)
+ {}
+
+ static bool NoUpdate(const ElementT& element, QTreeWidgetItem* item)
+ {
+ (void) element, (void) item;
+ return true;
+ }
+
+ void enableSort(QTreeWidget* tree)
+ {
+ tree->sortItems(0, Qt::SortOrder::AscendingOrder);
+ tree->isSortingEnabled();
+ }
+
+ template <typename ParentT>
+ void updateTree(ParentT* parent, const ContainerT& elements,
+ CompareFn compareFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate)
+ {
+ int currentIndex = 0;
+ for (const auto& element : elements)
+ {
+ QTreeWidgetItem* item = nullptr;
+ if (currentIndex >= getItemCount(parent))
+ {
+ // Add elements to the end of the list.
+ item = makeItemFn(element);
+ insertItem(parent, getItemCount(parent), item);
+ ++currentIndex;
+ }
+ else
+ {
+ QTreeWidgetItem* currentItem = getItem(parent, currentIndex);
+ while (currentItem != nullptr && compareFn(element, currentItem) > 0)
+ {
+ delete takeItem(parent, currentIndex);
+ currentItem = getItem(parent, currentIndex);
+ }
+ if (currentItem == nullptr || compareFn(element, currentItem) < 0)
+ {
+ // Insert new item before child.
+ item = makeItemFn(element);
+ insertItem(parent, currentIndex, item);
+ ++currentIndex;
+ }
+ else if (currentItem != nullptr && compareFn(element, currentItem) == 0)
+ {
+ // Already existing.
+ item = currentItem;
+ ++currentIndex;
+ }
+ }
+ ARMARX_CHECK_NOT_NULL(item);
+ if (!updateItemFn(element, item))
+ {
+ break;
+ }
+ }
+ }
+
+ template <typename ParentT>
+ void updateTree(ParentT* parent, const ContainerT& elements,
+ NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate)
+ {
+ auto compare = [nameFn](const ElementT & element, QTreeWidgetItem * item)
+ {
+ std::string name = nameFn(element);
+ return name.compare(item->text(0).toStdString());
+ };
+ updateTree(parent, elements, compare, makeItemFn, updateItemFn);
+ }
+
+ private:
+
+ int getItemCount(QTreeWidget* tree)
+ {
+ return tree->topLevelItemCount();
+ }
+ QTreeWidgetItem* getItem(QTreeWidget* tree, int index)
+ {
+ return tree->topLevelItem(index);
+ }
+ void insertItem(QTreeWidget* tree, int index, QTreeWidgetItem* item)
+ {
+ tree->insertTopLevelItem(index, item);
+ }
+ QTreeWidgetItem* takeItem(QTreeWidget* tree, int index)
+ {
+ return tree->takeTopLevelItem(index);
+ }
+
+ int getItemCount(QTreeWidgetItem* parent)
+ {
+ return parent->childCount();
+ }
+ QTreeWidgetItem* getItem(QTreeWidgetItem* parent, int index)
+ {
+ return parent->child(index);
+ }
+ QTreeWidgetItem* takeItem(QTreeWidgetItem* parent, int index)
+ {
+ return parent->takeChild(index);
+ }
+ void insertItem(QTreeWidgetItem* parent, int index, QTreeWidgetItem* item)
+ {
+ parent->insertChild(index, item);
+ }
+ };
+
+ template <class KeyT, class ValueT>
+ struct MapTreeVisitor : private TreeVisitor<std::map<KeyT, ValueT>>
+ {
+ using MapT = std::map<KeyT, ValueT>;
+ using Base = TreeVisitor<MapT>;
+ using ElementT = typename Base::ElementT;
+
+ using NameFn = std::function<std::string(const KeyT& key, const ValueT& value)>;
+
+ using MakeItemFn = std::function<QTreeWidgetItem*(const KeyT& key, const ValueT& value)>;
+ using UpdateItemFn = std::function<bool(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)>;
+
+ MapTreeVisitor() = default;
+ MapTreeVisitor(const MapT&)
+ {}
+
+ static std::string KeyIsName(const KeyT& key, const ValueT& value)
+ {
+ (void) value;
+ if constexpr(std::is_same<KeyT, std::string>())
+ {
+ return key;
+ }
+ else
+ {
+ std::stringstream ss;
+ ss << key;
+ return ss.str();
+ }
+ }
+
+ static bool NoUpdate(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)
+ {
+ (void) key, (void) value, (void) item;
+ return true;
+ }
+
+ void updateTree(QTreeWidget* tree, const MapT& elements,
+ MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate)
+ {
+ updateTree(tree, elements, KeyIsName, makeItemFn, updateItemFn);
+ }
+
+ template <class ParentT>
+ void updateTree(ParentT* parent, const MapT& elements,
+ NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate)
+ {
+ auto name = [nameFn](const ElementT & element)
+ {
+ const auto& [key, value] = element;
+ return nameFn(key, value);
+ };
+ auto makeItem = [makeItemFn](const ElementT & element)
+ {
+ const auto& [key, value] = element;
+ return makeItemFn(key, value);
+ };
+ auto updateItem = [updateItemFn](const ElementT & element, QTreeWidgetItem * item)
+ {
+ const auto& [key, value] = element;
+ return updateItemFn(key, value, item);
+ };
+
+ Base::updateTree(parent, elements, name, makeItem, updateItem);
+ }
+ };
+
+
void ObjectPoseGuiWidgetController::updateObjectsTab()
{
if (!objectPoseObserver)
@@ -140,42 +343,72 @@ namespace armarx
IceUtil::Time start = IceUtil::Time::now();
ARMARX_INFO << "Getting object poses...";
- // const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPoseObserver->getObjectPoses());
+ const objpose::data::ObjectPoseSeq objectPosesIce = objectPoseObserver->getObjectPoses();
+ ARMARX_INFO << "Got " << objectPosesIce.size() << " object poses. "
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
- const objpose::data::ObjectPoseSeq objectPoses = objectPoseObserver->getObjectPoses();
- ARMARX_INFO << "Got " << objectPoses.size() << " object poses. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms.)";
+ const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPosesIce);
- start = IceUtil::Time::now();
- 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.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(
- i, col++, new QTableWidgetItem(objpose::ObjectTypeEnumNames.to_name(pose.objectType).c_str()));
+ std::map<std::string, objpose::ObjectPoseSeq> objectPosesByProvider;
+ for (const auto& pose : objectPoses)
+ {
+ objectPosesByProvider[pose.providerName].push_back(pose);
}
- ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms.";
- }
+ start = IceUtil::Time::now();
+
+ QTreeWidget* tree = widget.objectsTree;
+ MapTreeVisitor visitor(objectPosesByProvider);
+ auto makeProviderItem = [](const std::string & provider, const objpose::ObjectPoseSeq&)
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
+ item->setExpanded(true);
+ return item;
+ };
+ auto updateItem = [](const std::string&, const objpose::ObjectPoseSeq & objectPoses, QTreeWidgetItem * item)
+ {
+ auto name = [](const objpose::ObjectPose & pose)
+ {
+ return pose.objectID.str();
+ };
+ auto makeItem = [](const objpose::ObjectPose&)
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
+ return item;
+ };
+ auto updateItem = [](const objpose::ObjectPose & pose, QTreeWidgetItem * item)
+ {
+ int col = 0;
+ item->setText(col++, QString::fromStdString(pose.objectID.str()));
+ item->setText(col++, QString::fromStdString(pose.providerName));
+ item->setText(col++, QString::fromStdString(objpose::ObjectTypeEnumNames.to_name(pose.objectType)));
+
+ std::stringstream ss;
+ if (pose.localOOBB)
+ {
+ static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
+ ss << pose.localOOBB->dimensions().format(iof);
+ }
+ else
+ {
+ ss << "None";
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
+ item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
+ item->setText(col++, QString::fromStdString(TimeUtil::toStringDateTime(pose.timestamp)));
+ return true;
+ };
+ TreeVisitor visitor(objectPoses);
+ visitor.updateTree(item, objectPoses, name, makeItem, updateItem);
+
+ return true;
+ };
+ visitor.updateTree(tree, objectPosesByProvider, makeProviderItem, updateItem);
+
+ ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ }
- struct ProviderInfo
- {
- std::string name;
- objpose::ObjectPoseProviderPrx proxy;
- objpose::ObjectTypeEnum objectType;
- };
void ObjectPoseGuiWidgetController::updateRequestTab()
{
@@ -188,46 +421,66 @@ namespace armarx
IceUtil::Time start = IceUtil::Time::now();
objpose::ProviderInfoMap availableProvidersInfo = objectPoseObserver->getAvailableProvidersInfo();
ARMARX_INFO << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms.)";
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
// Restructure data.
- std::map<std::string, std::map<std::string, std::map<std::string, ProviderInfo>>> data;
+ std::map<std::string, std::set<std::pair<std::string, std::string>>> data;
for (const auto& [providerName, info] : availableProvidersInfo)
{
for (const auto& id : info.supportedObjects)
{
- ProviderInfo& dinfo = data[id.dataset][id.className][providerName];
- dinfo.name = providerName;
- dinfo.proxy = info.proxy;
- dinfo.objectType = info.objectType;
+ data[id.dataset].insert(std::make_pair(id.className, providerName));
}
}
+ start = IceUtil::Time::now();
+
QTreeWidget* tree = widget.requestTree;
- tree->clear();
- start = IceUtil::Time::now();
- for (const auto& [dataset, datasetData] : data)
+ MapTreeVisitor visitor(data);
+ auto makeDatasetItem = [](const std::string & dataset, const auto&)
{
- QTreeWidgetItem* datasetItem = new QTreeWidgetItem({QString::fromStdString(dataset)});
- tree->addTopLevelItem(datasetItem);
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
+ return item;
+ };
+ auto updateItem = [tree](const std::string & dataset, const auto & datasetData, QTreeWidgetItem * datasetItem)
+ {
+ (void) dataset;
- for (const auto& [className, providerData] : datasetData)
+ auto compareFn = [](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem * item)
{
- for (const auto& [providerName, providerInfo] : providerData)
+ auto rhs = std::make_pair(item->text(0).toStdString(), item->text(1).toStdString());
+ if (lhs < rhs)
+ {
+ return -1;
+ }
+ return lhs == rhs ? 0 : 1;
+ };
+ auto makeItemFn = [](const std::pair<std::string, std::string>& element)
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem({ QString::fromStdString(element.first), QString::fromStdString(element.second)});
+ return item;
+ };
+ auto updateItemFn = [tree](const std::pair<std::string, std::string>& element, QTreeWidgetItem * item)
+ {
+ (void) element;
+ if (!tree->itemWidget(item, 2))
{
- QTreeWidgetItem* classItem = new QTreeWidgetItem(
- { QString::fromStdString(className), QString::fromStdString(providerName)});
- datasetItem->addChild(classItem);
-
QCheckBox* requestCheckBox = new QCheckBox();
- tree->setItemWidget(classItem, 2, requestCheckBox);
+ tree->setItemWidget(item, 2, requestCheckBox);
}
- }
- }
+ return true;
+ };
+
+ TreeVisitor visitor(datasetData);
+ visitor.updateTree(datasetItem, datasetData, compareFn, makeItemFn, updateItemFn);
+
+ return true;
+ };
+ visitor.updateTree(tree, data, makeDatasetItem, updateItem);
- ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms.";
+ ARMARX_INFO << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
void ObjectPoseGuiWidgetController::requestSelectedObjects()