Add property definitions for quaternions

source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp

@@ -26,6 +26,7 @@
 #include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
 #include <ArmarXCore/core/application/properties/PropertyUser.h>
 
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
 #include <RobotAPI/libraries/aron/core/data/variant/All.h>
 #include <RobotAPI/libraries/aron_component_config/VariantHelperFactory.h>
 
@@ -157,8 +158,11 @@ namespace armarx::aron::component_config
     }
 
     PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(
-        const armarx::PropertyUser& defs, const std::string& global_namespace):
-        property_user_(std::experimental::make_observer(&defs)), global_namespace_(global_namespace), in_list_(false)
+        const armarx::PropertyUser& defs,
+        const std::string& global_namespace) :
+        property_user_(std::experimental::make_observer(&defs)),
+        global_namespace_(global_namespace),
+        in_list_(false)
     {
     }
 
@@ -241,6 +245,19 @@ namespace armarx::aron::component_config
         return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
     }
 
+    void
+    PropertyDefinitionGetterVisitor::visitQuaternion(
+        std::shared_ptr<data::Variant>& quaternion,
+        const std::shared_ptr<type::Variant>& type)
+    {
+        auto value = data::NDArray::DynamicCastAndCheck(quaternion);
+        auto name = global_namespace_ + pathToName(quaternion);
+        const std::string property = property_user_->getProperty<std::string>(name).getValue();
+        const Eigen::Quaternion quat(simox::alg::to_eigen_vec(property, ", ").data());
+        const auto& newQuat = data::converter::AronEigenConverter::ConvertFromQuaternion(quat);
+        value->setData(newQuat->getDataAsVector().size(), newQuat->getData());
+    }
+
     void
     PropertyDefinitionGetterVisitor::visitObjectOnEnter(std::shared_ptr<data::Variant>& i,
                                                         const std::shared_ptr<type::Variant>& j)
@@ -254,8 +271,10 @@ namespace armarx::aron::component_config
     }
 
     PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(
-        const PropertyDefinitionsPtr& defs, const std::string& global_namespace) :
-        property_definitions_(std::experimental::make_observer(defs.get())), global_namespace_(global_namespace)
+        const PropertyDefinitionsPtr& defs,
+        const std::string& global_namespace) :
+        property_definitions_(std::experimental::make_observer(defs.get())),
+        global_namespace_(global_namespace)
     {
     }
 
@@ -391,6 +410,17 @@ namespace armarx::aron::component_config
             global_namespace_ = type->getObjectNameWithoutNamespace() + ".";
         }
     }
+
+    void
+    PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& quaternion,
+                                                      const type::QuaternionPtr& type)
+    {
+        auto name = global_namespace_ + pathToName(quaternion);
+        const auto quat =
+            data::converter::AronEigenConverter::ConvertToQuaternion<float>(quaternion);
+        property_definitions_->defineOptionalProperty<std::string>(
+            name, simox::alg::to_string(quat.coeffs(), ", "));
+    }
 } // namespace armarx::aron::component_config
 
 #undef INPUT_GUARD
\ No newline at end of file

source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h

@@ -48,6 +48,8 @@ namespace armarx::aron::component_config
 
         void visitAronVariant(const data::StringPtr&, const type::StringPtr&) override;
 
+        void visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&) override;
+
         void visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) override;
 
         void visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) override;
@@ -95,6 +97,8 @@ namespace armarx::aron::component_config
 
         void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
 
+        void visitQuaternion(DataInput& elementData, TypeInput& elementType) override;
+
         void visitListOnEnter(DataInput& elementData, TypeInput& elementType) override;
 
         void visitListOnExit(DataInput& elementData, TypeInput& elementType) override;