PropertyDefinitionVisitors.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    robdekon
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       06.09.22
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PropertyDefinitionVisitors.h"
 
#include <SimoxUtility/algorithm/string/string_conversion_eigen.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
#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>
 
#include "Util.h"
 
#define INPUT_GUARD(i)                                                                             \
    ARMARX_TRACE;                                                                                  \
    ARMARX_CHECK_NOT_NULL(i);                                                                      \
    if (in_list_)                                                                                  \
        return;
 
inline std::string
joinNamespaces(const std::vector<std::string>& namespaces)
{
    std::string result = "";
    std::for_each(
        namespaces.begin(), namespaces.end(), [&result](const auto& i) { result += i + "."; });
    return result;
}
 
namespace armarx::aron::component_config
{
 
    void
    PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& /*elementType*/)
    {
        INPUT_GUARD(i);
        auto value = data::Int::DynamicCastAndCheck(i);
        auto name = global_namespace_ + pathToName(i);
        property_user_->getProperty(value->getValue(), name);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& /*elementType*/)
    {
        INPUT_GUARD(f);
        auto value = data::Float::DynamicCastAndCheck(f);
        auto name = global_namespace_ + pathToName(f);
        property_user_->getProperty(value->getValue(), name);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& /*elementType*/)
    {
        INPUT_GUARD(d);
        auto value = data::Double::DynamicCastAndCheck(d);
        auto name = global_namespace_ + pathToName(d);
        property_user_->getProperty(value->getValue(), name);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& /*elementType*/)
    {
        INPUT_GUARD(b);
        auto value = data::Bool::DynamicCastAndCheck(b);
        auto name = global_namespace_ + pathToName(b);
        property_user_->getProperty(value->getValue(), name);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& /*elementType*/)
    {
        INPUT_GUARD(string);
        auto value = data::String::DynamicCastAndCheck(string);
        auto name = global_namespace_ + pathToName(string);
        auto property = property_user_->getProperty<std::string>(name);
        if (not property.getValue().empty())
        {
            value->getValue() = property.getValueAndReplaceAllVars();
        }
        else
        {
            value->getValue() = "";
        }
    }
 
    void
    PropertyDefinitionGetterVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
    {
        ARMARX_TRACE;
        in_list_ = true;
        const auto& name = global_namespace_ + pathToName(o);
        const auto& type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
        if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
            implementedListDescriptors.end())
        {
            return;
        }
        auto old_list = data::List::DynamicCast(o);
        old_list->clear();
        auto get_list = [this, &name]() -> std::vector<std::string>
        {
            std::string list;
            property_user_->getProperty(list, name);
            return simox::alg::split(list, ",");
        };
        int i = 0;
        std::vector<std::string> vector = get_list();
        std::for_each(vector.begin(),
                      vector.end(),
                      [&old_list, &i, &type](const auto& el)
                      {
                          old_list->addElement(factories::VariantHelper::make(type)->from_string(
                              el, old_list->getPath().withIndex(i)));
                          i++;
                      });
    }
 
    void
    PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& /*o*/, TypeInput& /*t*/)
    {
        in_list_ = false;
    }
 
    void
    PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& /*elementType*/)
    {
        ARMARX_TRACE;
        auto data = data::Int::DynamicCastAndCheck(enumData);
        auto name = global_namespace_ + pathToName(enumData);
        property_user_->getProperty(data->getValue(), name);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& /*elementType*/)
    {
        ARMARX_WARNING << "Unknown data encountered: "
                       << (unknown ? unknown->getFullName() : "nullptr");
    }
 
    PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(
        const armarx::PropertyUser& defs,
        const std::string& globalNamespace) :
        property_user_(std::experimental::make_observer(&defs)),
        global_namespace_(globalNamespace),
        in_list_(false)
    {
    }
 
    void
    PropertyDefinitionGetterVisitor::visitDictOnEnter(DataInput& o, TypeInput& t)
    {
        ARMARX_TRACE;
        in_list_ = true;
        const auto& name = global_namespace_ + pathToName(o);
        const auto& type = type::Dict::DynamicCast(t)->getAcceptedType()->getDescriptor();
        if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
            implementedListDescriptors.end())
        {
            return;
        }
        auto old_dict = data::Dict::DynamicCast(o);
        old_dict->clear();
        auto get_list = [this, &name]() -> std::vector<std::string>
        {
            std::string list;
            property_user_->getProperty(list, name);
            return simox::alg::split(list, ",");
        };
        std::vector<std::string> vector = get_list();
        std::for_each(
            vector.begin(),
            vector.end(),
            [&old_dict, &type](const auto& el)
            {
                auto key_value = simox::alg::split(el, ":");
                old_dict->addElement(
                    key_value.front(),
                    factories::VariantHelper::make(type)->from_string(
                        key_value.back(), old_dict->getPath().withElement(key_value.front())));
            });
    }
 
    void
    PropertyDefinitionGetterVisitor::visitDictOnExit(DataInput& /*elementData*/,
                                                     TypeInput& /*elementType*/)
    {
        in_list_ = false;
    }
 
    PropertyDefinitionGetterVisitor::MapElements
    PropertyDefinitionGetterVisitor::getObjectElements(DataInput& o, TypeInput& t)
    {
        return component_config::getObjectElements(o, t);
    }
 
    PropertyDefinitionGetterVisitor::MapElements
    PropertyDefinitionGetterVisitor::getDictElements(DataInput& o, TypeInput& t)
    {
        return component_config::getDictElements(o, t);
    }
 
    PropertyDefinitionGetterVisitor::ListElements
    PropertyDefinitionGetterVisitor::getListElements(DataInput& o, TypeInput& t)
    {
        return component_config::getListElements(o, t);
    }
 
    PropertyDefinitionGetterVisitor::PairElements
    PropertyDefinitionGetterVisitor::getPairElements(DataInput& o, TypeInput& t)
    {
        return component_config::getPairElements(o, t);
    }
 
    PropertyDefinitionGetterVisitor::TupleElements
    PropertyDefinitionGetterVisitor::getTupleElements(DataInput& o, TypeInput& t)
    {
        return component_config::getTupleElements(o, t);
    }
 
    type::Descriptor
    PropertyDefinitionGetterVisitor::getDescriptor(DataInput& o, TypeInput& t)
    {
        return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
    }
 
    void
    PropertyDefinitionGetterVisitor::visitMatrix(DataInput& matrix, TypeInput& /*type*/)
    {
        INPUT_GUARD(matrix);
        auto data = data::NDArray::DynamicCastAndCheck(matrix);
        const auto name = global_namespace_ + pathToName(matrix);
        const std::string property = property_user_->getProperty<std::string>(name).getValue();
        const auto vec = simox::alg::to_eigen_vec(property, ", ");
        ARMARX_CHECK_EQUAL(sizeof(float) * vec.size(), data->getDataAsVector().size())
            << "Matrix recovered from properties does not match the aron type";
 
        data->setData(data->getDataAsVector().size(),
                      reinterpret_cast<const unsigned char*>(vec.data()));
    }
 
    void
    PropertyDefinitionGetterVisitor::visitQuaternion(DataInput& quaternion, TypeInput& /*type*/)
    {
        INPUT_GUARD(quaternion);
        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(DataInput& i, TypeInput& j)
    {
        INPUT_GUARD(i);
        if (global_namespace_.empty())
        {
            auto t = type::Object::DynamicCastAndCheck(j);
            global_namespace_ = t->getObjectNameWithoutNamespace() + ".";
        }
    }
 
    PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(
        const PropertyDefinitionsPtr& defs,
        const std::string& globalNamespace) :
        property_definitions_(std::experimental::make_observer(defs.get())),
        global_namespace_(globalNamespace)
    {
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i,
                                                      const type::IntPtr& /*unused*/)
    {
        INPUT_GUARD(i);
        auto name = global_namespace_ + pathToName(i);
        property_definitions_->defineOptionalProperty<int>(name, i->getValue());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::FloatPtr& f,
                                                      const type::FloatPtr& /*unused*/)
    {
        INPUT_GUARD(f);
        auto name = global_namespace_ + pathToName(f);
        property_definitions_->defineOptionalProperty<float>(name, f->getValue());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::DoublePtr& d,
                                                      const type::DoublePtr& /*unused*/)
    {
        INPUT_GUARD(d);
        auto name = global_namespace_ + pathToName(d);
        property_definitions_->defineOptionalProperty<double>(name, d->getValue());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b,
                                                      const type::BoolPtr& /*unused*/)
    {
        INPUT_GUARD(b);
        auto name = global_namespace_ + pathToName(b);
        property_definitions_->defineOptionalProperty<bool>(name, b->getValue());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::StringPtr& string,
                                                      const type::StringPtr& /*type*/)
    {
        INPUT_GUARD(string);
        auto name = global_namespace_ + pathToName(string);
        property_definitions_->defineOptionalProperty<std::string>(name, string->getValue());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i,
                                                      const type::IntEnumPtr& t)
    {
        INPUT_GUARD(i);
        auto name = global_namespace_ + pathToName(i);
        property_definitions_->defineOptionalProperty<int>(name, i->getValue())
            .map(t->getAcceptedValueMap());
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::ListPtr& l,
                                                             const type::ListPtr& t)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(l);
        in_list_ = true;
        const auto& name = global_namespace_ + pathToName(l);
        const auto& type = t->getAcceptedType()->getDescriptor();
        if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
            implementedListDescriptors.end())
        {
            return;
        }
        const auto& input = l->getElements();
        std::string str;
        std::vector<std::string> vector;
        std::transform(input.begin(),
                       input.end(),
                       std::back_inserter(vector),
                       [&type](const auto& el)
                       { return factories::VariantHelper::make(type)->to_string(el); });
        str = simox::alg::to_string(vector, ", ");
        property_definitions_->defineOptionalProperty<std::string>(name, str);
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr& /*unused*/,
                                                            const type::ListPtr& /*unused*/)
    {
        in_list_ = false;
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::DictPtr& d,
                                                             const type::DictPtr& t)
    {
        ARMARX_TRACE;
        in_list_ = true;
        const auto& name = global_namespace_ + pathToName(d);
        const auto& type = t->getAcceptedType()->getDescriptor();
        if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
            implementedListDescriptors.end())
        {
            return;
        }
        const auto& input = d->getElements();
        std::stringstream ss;
        for (const auto& [key, el] : input)
        {
            ss << key << ":";
            ss << factories::VariantHelper::make(type)->to_string(el);
            ss << ",";
        }
        std::string value = ss.str();
        if (not value.empty())
        {
            value.pop_back();
        }
        property_definitions_->defineOptionalProperty<std::string>(name, value);
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr& /*unused*/,
                                                            const type::DictPtr& /*unused*/)
    {
        in_list_ = false;
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::DictPtr& obj,
                                                             const type::ObjectPtr& type)
    {
        INPUT_GUARD(obj);
        if (global_namespace_.empty())
        {
            global_namespace_ = type->getObjectNameWithoutNamespace() + ".";
        }
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& quaternion,
                                                      const type::QuaternionPtr& /*type*/)
    {
        INPUT_GUARD(quaternion);
        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(), ", "));
    }
 
    void
    PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& matrix,
                                                      const type::MatrixPtr& /*type*/)
    {
        INPUT_GUARD(matrix);
        auto name = global_namespace_ + pathToName(matrix);
        auto shape = matrix->getShape();
        // TODO: why is shape 3 for a matrix?
        if (shape.size() != 3 or (shape[0] != 1 and shape[1] != 1))
        {
            ARMARX_WARNING << "Matrix is not a vector, ignoring";
            return;
        }
        const auto mat = data::converter::AronEigenConverter::ConvertToMatrix<float>(*matrix);
        std::string value = "";
        for (int i = 0; i < shape[0]; i++)
        {
            for (int j = 0; j < shape[1]; j++)
            {
                value += std::to_string(mat(i, j)) + ", ";
            }
        }
        if (not value.empty())
        {
            value.pop_back();
            value.pop_back();
        }
        property_definitions_->defineOptionalProperty<std::string>(name, value);
    }
} // namespace armarx::aron::component_config
 
#undef INPUT_GUARD