From 1412bb3798967a7e4a6b270d7591a495a9dded57 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Wed, 9 Nov 2022 17:47:28 +0100
Subject: [PATCH] Add support for getting NDArray values from remote gui and
updating config
---
.../AronComponentConfigExample.cpp | 5 +-
.../RemoteGuiVisitors.cpp | 75 ++++++++++++++++++-
.../aron_component_config/RemoteGuiVisitors.h | 2 +
3 files changed, 77 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
index 683fcef10..1b2b3df50 100644
--- a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
@@ -57,10 +57,7 @@ namespace armarx
ARMARX_TRACE;
auto& config =
aron_component_config_plugin_->config_.getWriteBuffer();
- for (const auto& [key, value] : config.subMember.subsubMember.intDictMember)
- {
- ARMARX_INFO << key << value;
- }
+// ARMARX_INFO << VAROUT(config.orientation) << VAROUT(config.pose) << VAROUT(config.position);
config.intMember--;
aron_component_config_plugin_->config_.commitWrite();
}, 1000);
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
index 348edcb9e..03d6f049f 100644
--- a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
@@ -23,6 +23,7 @@
#include "RemoteGuiVisitors.h"
#include <SimoxUtility/math/convert/quat_to_rpy.h>
+#include <SimoxUtility/math/convert/rpy_to_quat.h>
#include <ArmarXCore/util/CPPUtility/Iterator.h>
@@ -259,7 +260,7 @@ namespace armarx::aron::component_config
auto group = RemoteGui::makeHBoxLayout(name + "_layout");
group.addChild(RemoteGui::makeLabel(name + "_label").value(q->getPath().getLastElement()));
group.addHSpacer();
- group.addChild(RemoteGui::makeVector3fSpinBoxes(name).value(quat).min(-M_PI).max(M_PI).toolTip(name));
+ group.addChild(RemoteGui::makeVector3fSpinBoxes(name).value(quat).min(-M_PI).max(M_PI).steps(601).decimals(2).toolTip(name));
group_hierarchy_.back()->addChild(group);
}
@@ -519,6 +520,78 @@ namespace armarx::aron::component_config
{
return tab_rebuild_required_;
}
+ void
+ GetValueFromMapVisitor::visitMatrix(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ INPUT_GUARD(o);
+ auto value = data::NDArray::DynamicCastAndCheck(o);
+ auto type = type::Matrix::DynamicCastAndCheck(t);
+ const std::string name = pathToName(o);
+ const auto& cols = type->getCols();
+ const auto& rows = type->getRows();
+ // TODO: does double work here?
+ if (cols == 4 && rows == 4)
+ {
+ auto gui_value = proxy_->getValue<Eigen::Matrix4f>(name);
+ auto config_value = converter::AronEigenConverter::ConvertToMatrix4f(value);
+
+ if (config_value != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant = converter::AronEigenConverter::ConvertFromMatrix(gui_value.get());
+ value->setData(gui_value.get().size() * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(config_value, name);
+ }
+ }
+ } else if ((cols == 3 and rows == 1) or (cols == 1 and rows == 3))
+ {
+ auto gui_value = proxy_->getValue<Eigen::Vector3f>(name);
+ auto config_value = converter::AronEigenConverter::ConvertToVector3f(value);
+
+ if (config_value != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant = converter::AronEigenConverter::ConvertFromMatrix(gui_value.get());
+ value->setData(gui_value.get().size() * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(config_value, name);
+ }
+ }
+ }
+
+ }
+ void
+ GetValueFromMapVisitor::visitQuaternion(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ INPUT_GUARD(o);
+ auto value = data::NDArray::DynamicCastAndCheck(o);
+ const std::string name = pathToName(o);
+ // TODO: does double work here?
+ auto gui_value = proxy_->getValue<Eigen::Vector3f>(name);
+ const auto& quat = simox::math::quat_to_rpy(converter::AronEigenConverter::ConvertToQuaternionf(value));
+ ARMARX_INFO << VAROUT(quat) << VAROUT(gui_value.get());
+ if (quat != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant = converter::AronEigenConverter::ConvertFromQuaternion(simox::math::rpy_to_quat(gui_value.get()));
+ value->setData(4 * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(quat, name);
+ }
+ }
+ }
void
GetValueFromMapVisitor::visitDictOnEnter(std::shared_ptr<data::Variant>& o,
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
index 65fd6c153..e9324bf15 100644
--- a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
@@ -118,6 +118,8 @@ namespace armarx::aron::component_config
void visitBool(DataInput&, TypeInput&) override;
void visitString(DataInput&, TypeInput&) override;
+ void visitMatrix(DataInput& elementData, TypeInput& elementType) override;
+ void visitQuaternion(DataInput& elementData, TypeInput& elementType) override;
bool tabRebuildRequired() const;
--
GitLab