From 351965003e09667c7c3c42fb4017306d4b8dd0e6 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 20 Apr 2021 13:45:10 +0200
Subject: [PATCH] Show ARON Pose, Position and Quat in MemoryViewer
---
.../libraries/armem_gui/CMakeLists.txt | 2 +-
.../instance/TreeDataVisitorBase.cpp | 6 +--
.../armem_gui/instance/TreeDataVisitorBase.h | 12 ++---
.../instance/TreeTypedDataVisitor.cpp | 43 +++++++++++++++--
.../armem_gui/instance/TreeTypedDataVisitor.h | 46 +++++++++++++++++--
5 files changed, 90 insertions(+), 19 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 4b4985677..a3fe99e2f 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -10,7 +10,7 @@ set(LIBRARIES
# ArmarXGui
SimpleConfigDialog
# RobotAPI
- armem
+ aroneigenconverter armem
)
set(SOURCES
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
index 39ba3d5e3..70a84dd7d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
@@ -62,7 +62,7 @@ namespace armarx::armem::gui
return true;
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const
{
if (n.getValue())
{
@@ -74,12 +74,12 @@ namespace armarx::armem::gui
}
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const
{
ss << "'" << n.getValue() << "'";
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const
{
ss << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
index edee54219..cb6d4152a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
@@ -40,7 +40,7 @@ namespace armarx::armem::gui
return true;
}
- QStringList makeValueRowStrings(const std::string& key, const std::string& value, const std::string& typeName)
+ QStringList makeValueRowStrings(const std::string& key, const std::string& value, const std::string& typeName) const
{
QStringList cols;
cols.insert(columnKey, QString::fromStdString(key));
@@ -50,7 +50,7 @@ namespace armarx::armem::gui
}
template <class Navigator>
- QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName)
+ QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName) const
{
std::stringstream value;
streamValueText(n, value);
@@ -58,13 +58,13 @@ namespace armarx::armem::gui
}
template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss)
+ void streamValueText(Navigator& n, std::stringstream& ss) const
{
ss << n.getValue();
}
- void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss);
+ void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const;
public:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
index 3d1ac1390..4dd70e8ac 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
@@ -1,9 +1,13 @@
#include "TreeTypedDataVisitor.h"
+#include <iomanip> // std::setprecision
+
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/armem/core.h>
@@ -25,11 +29,43 @@ namespace armarx::armem::gui
return n;
}
- QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, LongDataNavigator& data, const TimeTypeNavigator& type)
+ QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const
+ {
+ QTreeWidgetItem* item = makeItem<NDArrayDataNavigator, PoseTypeNavigator>(key, data, type);
+ // ToDo: Do something special.
+ return item;
+ }
+
+ void TreeTypedDataVisitor::streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const
{
+ (void) type;
armem::Time time = armem::Time::microSeconds(data.getValue());
armem::toDateTimeMilliSeconds(time);
- return new QTreeWidgetItem(this->makeValueRowStrings(key, armem::toDateTimeMilliSeconds(time), sanitizeTypeName(type.getName())));
+ ss << armem::toDateTimeMilliSeconds(time);
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", "\n", "", "", "", ""));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ", "", "", "(", ")"));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << quat.w() << " | " << quat.x() << " " << quat.x() << " " << quat.y() << " " << quat.z();
}
QStringList TreeTypedDataVisitor::serializePath(const aron::Path& path)
@@ -56,7 +92,4 @@ namespace armarx::armem::gui
return path;
}
-
-
-
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
index 9d3402313..9e38a1324 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
@@ -138,17 +138,55 @@ namespace armarx::armem::gui
item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, int(type.getDescriptor()));
+
+ if (false)
+ {
+ QFont font;
+ font.setFamily("Consolas");
+ font.setStyleHint(QFont::Monospace);
+ font.setFixedPitch(true);
+ font.setPointSize(10);
+ item->setFont(columnValue, font);
+ }
}
return true;
}
- template <class DataNavigatorT>
- QTreeWidgetItem* makeItem(const std::string& key, DataNavigatorT& data, const TypeNavigator& type)
+ template <class DataNavigatorT, class TypeNavigatorT>
+ QTreeWidgetItem* makeItem(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type) const
+ {
+ std::stringstream ss;
+ try
+ {
+ this->streamValueText(data, type, ss);
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ss << "x ";
+ TreeDataVisitorBase::streamValueText(data, ss);
+ std::stringstream es;
+ es << e.what();
+ ss << simox::alg::replace_all(es.str(), "\n", " | ");
+ }
+ return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getName())));
+ }
+
+ QTreeWidgetItem* makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const;
+
+
+ template <class DataNavigatorT, class TypeNavigatorT>
+ void streamValueText(DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
{
- return new QTreeWidgetItem(this->makeValueRowStrings(key, data, sanitizeTypeName(type.getName())));
+ // Fallback to type-agnostic (but data-aware).
+ (void) type;
+ TreeDataVisitorBase::streamValueText(data, ss);
}
- QTreeWidgetItem* makeItem(const std::string& key, LongDataNavigator& data, const TimeTypeNavigator& type);
+ using TreeDataVisitorBase::streamValueText;
+ void streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const;
public:
--
GitLab