ConfigParser.cpp

Go to the documentation of this file.

#include "ConfigParser.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
namespace armarx::control::hardware_config
{
    std::string
    createErrorMessageWithContext(std::string message, RapidXmlReaderNode& context)
    {
        std::stringstream ss;
        ss << message << std::endl << "Context:" << std::endl;
        ss << context.toString();
        return ss.str();
    }
 
    ConfigParser::ConfigParser(MultiNodeRapidXMLReader reader) : hardwareConfigNode(reader)
    {
    }
 
    ConfigParser::ConfigParser(RapidXmlReaderNode& hardwareConfigNode)
    {
        this->hardwareConfigNode.addNode(hardwareConfigNode);
    }
 
    void
    ConfigParser::parse()
    {
        // Clear previous errors and reset parsed data
        errors.clear();
        config = HardwareConfig();
 
        // Parse profiles first
        for (auto& node : hardwareConfigNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Profiles")
            {
                parseProfiles(node);
            }
        }
 
        // Parse devices
        for (auto& node : hardwareConfigNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Device")
            {
                std::shared_ptr<DeviceConfig> deviceConfig;
                bool deviceValid = parseDevice(deviceConfig, node);
                if (deviceValid)
                {
                    config.addDeviceConfig(deviceConfig->getName(), deviceConfig);
                }
            }
            else if (nodeName == "Profiles")
            {
                // Skip: already read
                continue;
            }
            else if (nodeName == "include")
            {
                // Skip: parsed by file reader
                continue;
            }
            else
            {
                errors.push_back("Unexpected node " + nodeName +
                                 " at top level of HardwareConfig xml");
            }
        }
 
        // Parsing finished, inform config objects
        config.onParsingFinished();
 
        // Error message handling
        if (errors.size() > 0)
        {
            std::stringstream ss;
            for (auto& error : errors)
            {
                ss << error << std::endl;
            }
            throw ParserError(ss.str());
        }
    }
 
    std::vector<std::string>&
    ConfigParser::getWarnings()
    {
        return warnings;
    }
 
    HardwareConfig&
    ConfigParser::getHardwareConfig()
    {
        return config;
    }
 
    bool
    ConfigParser::parseDevice(std::shared_ptr<DeviceConfig>& deviceConfig,
                              RapidXmlReaderNode& deviceNode)
    {
        if (!deviceNode.has_attribute("type"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Device node is missing attribute \"type\"", deviceNode));
            return false;
        }
        if (!deviceNode.has_attribute("name"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Device node is missing attribute \"name\"", deviceNode));
            return false;
        }
 
        std::string name = deviceNode.attribute_value("name");
        std::string type = deviceNode.attribute_value("type");
 
 
        std::shared_ptr<DeviceConfigBase> profile;
        if (deviceNode.has_attribute("profile") &&
            findProfile<DeviceConfigBase>(
                profile, type, deviceNode.attribute_value("profile"), "Device", deviceProfiles))
        {
            deviceConfig = std::make_shared<DeviceConfig>(type, name, profile);
        }
        else
        {
            deviceConfig = std::make_shared<DeviceConfig>(type, name);
            warnings.push_back("Device with type " + type + " and name " + name +
                               " does not have a profile");
        }
 
        auto slaveNodes = deviceNode.nodes("Slave");
        if (slaveNodes.size() < 1)
        {
            // Maybe only a warning
            errors.push_back("Device with type " + deviceConfig->getType() + " and name " +
                             deviceConfig->getName() + " does not have any Slaves");
        }
        else
        {
            bool needsType = slaveNodes.size() != 1;
            for (auto& node : slaveNodes)
            {
                std::shared_ptr<SlaveConfig> slaveConfig;
                bool slaveValid = parseSlave(
                    slaveConfig, node, needsType ? SlaveType::WithType : SlaveType::WithoutType);
                if (slaveValid)
                {
                    deviceConfig->addSlaveConfig(slaveConfig);
                }
            }
        }
 
 
        for (auto& node : deviceNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Slave")
            {
                // Skip all slaves because they have already been parsed
                continue;
            }
 
            if (nodeName == "Subdevice")
            {
                std::shared_ptr<DeviceConfigBase> subDeviceConfig;
                bool subDeviceValid = parseSubDevice(subDeviceConfig, node);
                if (subDeviceValid)
                {
                    deviceConfig->addSubDeviceConfig(subDeviceConfig->getName(), subDeviceConfig);
                }
            }
            else if (nodeName == "Controller")
            {
                parseController(*deviceConfig, node, ConfigTag::TagCustom);
            }
            else if (nodeName == "Config")
            {
                parseConfigNode(*deviceConfig, node, ConfigTag::TagCustom);
            }
            else
            {
                errors.push_back("Device with type " + deviceConfig->getType() + " and name " +
                                 deviceConfig->getName() + " contains illegal " + nodeName +
                                 " node");
            }
        }
        return true;
    }
 
    bool
    ConfigParser::parseDeviceProfile(std::shared_ptr<DeviceConfigBase>& deviceProfileConfig,
                                     RapidXmlReaderNode& deviceNode)
    {
        if (!deviceNode.has_attribute("type"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Device profile node is missing attribute \"type\"", deviceNode));
            return false;
        }
        if (!deviceNode.has_attribute("profilename"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Device profile node is missing attribute \"profilename\"", deviceNode));
            return false;
        }
 
        std::string name = deviceNode.attribute_value("profilename");
        deviceProfileConfig =
            std::make_shared<DeviceConfigBase>(deviceNode.attribute_value("type"), name);
 
 
        for (auto& node : deviceNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Controller")
            {
                parseController(*deviceProfileConfig, node, ConfigTag::TagProfile);
            }
            else if (nodeName == "Config")
            {
                parseConfigNode(*deviceProfileConfig, node, ConfigTag::TagProfile);
            }
            else
            {
                errors.push_back("Device Profile with type " + deviceProfileConfig->getType() +
                                 " and profilename " + deviceProfileConfig->getName() +
                                 " contains illegal " + nodeName + " node");
            }
        }
        return true;
    }
 
    bool
    ConfigParser::parseSubDevice(std::shared_ptr<DeviceConfigBase>& subDeviceConfig,
                                 RapidXmlReaderNode& subDeviceNode)
    {
        if (!subDeviceNode.has_attribute("name"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Subdevice node does not have \"name\" attribute", subDeviceNode));
            return false;
        }
        if (!subDeviceNode.has_attribute("type"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Subdevice node does not have \"type\" attribute", subDeviceNode));
            return false;
        }
        std::string name = subDeviceNode.attribute_value("name");
        std::string type = subDeviceNode.attribute_value("type");
 
        std::shared_ptr<DeviceConfigBase> profile;
        if (subDeviceNode.has_attribute("profile") &&
            findProfile<DeviceConfigBase>(profile,
                                          type,
                                          subDeviceNode.attribute_value("profile"),
                                          "Subdevice",
                                          deviceProfiles))
        {
            subDeviceConfig = std::make_shared<DeviceConfigBase>(type, name, profile);
        }
        else
        {
            subDeviceConfig = std::make_shared<DeviceConfigBase>(type, name);
        }
 
 
        for (auto& node : subDeviceNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Controller")
            {
                parseController(*subDeviceConfig, node, ConfigTag::TagCustom);
            }
            else if (nodeName == "Config")
            {
                parseConfigNode(*subDeviceConfig, node, ConfigTag::TagCustom);
            }
            else
            {
                errors.push_back("SubDevice with type " + subDeviceConfig->getType() +
                                 " and name " + subDeviceConfig->getName() + " contains illegal " +
                                 nodeName + " node");
            }
        }
        return true;
    }
 
    bool
    ConfigParser::parseSlave(std::shared_ptr<SlaveConfig>& slaveConfig,
                             RapidXmlReaderNode& slaveNode,
                             SlaveType slaveType)
    {
        std::optional<std::string> type;
        if (!slaveNode.has_attribute("type"))
        {
            if (SlaveType::WithType)
            {
                errors.push_back(createErrorMessageWithContext(
                    "Slave is missing a \"type\" attribute", slaveNode));
                return false;
            }
        }
        else
        {
            type = std::make_optional(slaveNode.attribute_value("type"));
        }
 
        std::optional<std::string> name;
        if (slaveNode.has_attribute("name"))
        {
            name = std::make_optional(slaveNode.attribute_value("name"));
        }
 
        // Check number of identifier nodes and abort if necessary
        auto identifierNodes = slaveNode.nodes("Identifier");
        if (identifierNodes.size() != 1)
        {
            errors.push_back(createErrorMessageWithContext(
                "Slave does not have exactly one identifier node", slaveNode));
            return false;
        }
 
        // Parse slave identifier
        SlaveIdentifierConfig slaveIdentifier;
 
        // Parse profile and create config object
        std::shared_ptr<SlaveProfile> profile;
        if (slaveNode.has_attribute("profile") &&
            findProfile<SlaveProfile>(profile,
                                      type.value_or(""),
                                      slaveNode.attribute_value("profile"),
                                      "Slave",
                                      slaveProfiles))
        {
            slaveIdentifier = profile->getIdentifier();
            parseSlaveIdentifier(
                slaveIdentifier, identifierNodes.at(0), type, ConfigTag::TagCustom, true);
            slaveConfig = std::make_shared<SlaveConfig>(slaveIdentifier, type, name, profile);
        }
        else
        {
            parseSlaveIdentifier(
                slaveIdentifier, identifierNodes.at(0), type, ConfigTag::TagCustom, true);
            slaveConfig = std::make_shared<SlaveConfig>(slaveIdentifier, type, name);
            if (type.has_value() || name.has_value())
            {
                warnings.push_back(createErrorMessageWithContext(
                    "Slave with type " + type.value_or("<no type>") + " and name " +
                        name.value_or("<no name>") + " does not have a profile",
                    slaveNode));
            }
        }
 
        // Parse the other nodes
        for (auto& node : slaveNode.nodes())
        {
            std::string nodeName = node.name();
 
            if (nodeName == "Identifier")
            {
                // Skip identifier node because it has already been parsed
                continue;
            }
 
            if (nodeName == "Config")
            {
                parseConfigNode(*slaveConfig, node, ConfigTag::TagCustom);
            }
            else
            {
                std::stringstream ss;
                ss << "Slave with identifier (" << slaveIdentifier << ") has illegal " << nodeName
                   << " node";
                errors.push_back(createErrorMessageWithContext(ss.str(), slaveNode));
                continue;
            }
        }
        return true;
    }
 
    bool
    ConfigParser::parseSlaveProfile(std::shared_ptr<SlaveProfile>& slaveConfig,
                                    RapidXmlReaderNode& slaveNode)
    {
        if (!slaveNode.has_attribute("type"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Slave Profile is missing a \"type\" attribute", slaveNode));
            return false;
        }
        std::string type = slaveNode.attribute_value("type");
 
        if (!slaveNode.has_attribute("profilename"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Slave Profile is missing a \"profilename\" attribute", slaveNode));
            return false;
        }
        std::string name = slaveNode.attribute_value("profilename");
 
        // Parse the Identifier node first because it is needed to create the SlaveConfig object
        auto identifierNodes = slaveNode.nodes("Identifier");
        if (identifierNodes.size() > 1)
        {
            errors.push_back(createErrorMessageWithContext(
                "Slave Profile does have too many identifiers", slaveNode));
            return false;
        }
        // Parse slave identifier
        SlaveIdentifierConfig slaveIdentifier;
        if (identifierNodes.size() == 1)
        {
            // parse idenfifier, false means that we do not check if the identifier is complete / valid
            // because it is allowed to only define parts of the idenfitier in the profile
            parseSlaveIdentifier(
                slaveIdentifier, identifierNodes.at(0), {type}, ConfigTag::TagProfile, false);
        }
 
        slaveConfig = std::make_shared<SlaveProfile>(slaveIdentifier, type, name);
 
        // Parse the other nodes
        for (auto& node : slaveNode.nodes())
        {
            std::string nodeName = node.name();
 
            if (nodeName == "Identifier")
            {
                // Skip identifier node because it has already been parsed
                continue;
            }
 
            if (nodeName == "Config")
            {
                parseConfigNode(*slaveConfig, node, ConfigTag::TagProfile);
            }
            else
            {
                errors.push_back(createErrorMessageWithContext(
                    "Slave Profile has illegal " + nodeName + " node", slaveNode));
                continue;
            }
        }
        return true;
    }
 
    void
    ConfigParser::parseConfigNode(Config& config, RapidXmlReaderNode& configNode, ConfigTag tag)
    {
        for (auto& node : configNode.nodes())
        {
            std::string nodeName = node.name();
 
            if (!node.has_attribute("name"))
            {
                errors.push_back(createErrorMessageWithContext(
                    nodeName + " node is missing a \"name\" attribute", configNode));
                continue;
            }
            std::string name = node.attribute_value("name");
 
            try
            {
                if (nodeName == "Bool")
                {
                    std::string value = node.value();
                    config.setBool(name, strToBool(value), tag);
                }
                else if (nodeName == "Float")
                {
                    config.setFloat(name, node.value_as_float(), tag);
                }
                else if (nodeName == "Int")
                {
                    config.setInt(name, node.value_as_int32(), tag);
                }
                else if (nodeName == "UInt")
                {
                    config.setUint(name, node.value_as_uint32(), tag);
                }
                else if (nodeName == "BoolList")
                {
                    std::list<bool> list = parseList<bool>(
                        node.value(), [this](std::string s) { return strToBool(s); });
                    config.setBoolList(name, list, tag);
                }
                else if (nodeName == "FloatList")
                {
                    std::list<float> list =
                        parseList<float>(node.value(), [](std::string s) { return std::stof(s); });
                    config.setFloatList(name, list, tag);
                }
                else if (nodeName == "IntList")
                {
                    std::list<std::int32_t> list = parseList<std::int32_t>(
                        node.value(), [](std::string s) { return std::stol(s); });
                    config.setIntList(name, list, tag);
                }
                else if (nodeName == "UIntList")
                {
                    std::list<std::uint32_t> list = parseList<std::uint32_t>(
                        node.value(), [](std::string s) { return std::stoul(s); });
                    config.set<std::list<std::uint32_t>>(name, list, tag);
                }
                else if (nodeName == "FloatMatrix")
                {
                    auto mat = parseMatrix<float>(node, [](std::string s) { return std::stof(s); });
                    config.set<Eigen ::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>>(
                        name, mat, tag);
                }
                else if (nodeName == "IntMatrix")
                {
                    auto mat =
                        parseMatrix<std::int32_t>(node, [](std::string s) { return std::stol(s); });
                    config.set<Eigen::Matrix<std::int32_t, Eigen::Dynamic, Eigen::Dynamic>>(
                        name, mat, tag);
                }
                else if (nodeName == "UIntMatrix")
                {
                    auto mat = parseMatrix<std::uint32_t>(
                        node, [](std::string s) { return std::stoul(s); });
                    config.set<Eigen::Matrix<std::uint32_t, Eigen::Dynamic, Eigen::Dynamic>>(
                        name, mat, tag);
                }
                else if (nodeName == "LinearConvertedValue")
                {
                    if (node.has_attribute("factor") && node.has_attribute("offset"))
                    {
                        config.setLinearConfig(
                            name,
                            types::LinearConfig(node.attribute_as_float("factor"),
                                                node.attribute_as_float("offset")),
                            tag);
                    }
                    else
                    {
                        types::LinearConfig tmpCfg;
                        if (node.has_attribute("factor"))
                        {
                            float factor = node.attribute_as_float("factor");
                            tmpCfg.setFactorAbsoluteValue(std::abs(factor));
                            tmpCfg.setFactorIsNegative(factor < 0);
                        }
                        if (node.has_attribute("factorAbs"))
                        {
                            float factor = node.attribute_as_float("factorAbs");
                            if (factor < 0)
                            {
                                errors.push_back(createErrorMessageWithContext(
                                    "factorAbs attribute of LinearConvertedValue has "
                                    "a negative value",
                                    configNode));
                            }
                            tmpCfg.setFactorAbsoluteValue(std::abs(factor));
                        }
                        if (node.has_attribute("factorSign"))
                        {
                            std::string val = node.attribute_value("factorSign");
                            if (val == "+" || val == "positive")
                            {
                                tmpCfg.setFactorIsNegative(false);
                            }
                            else if (val == "-" || val == "negative")
                            {
                                tmpCfg.setFactorIsNegative(true);
                            }
                            else
                            {
                                errors.push_back(createErrorMessageWithContext(
                                    "factor attribute of LinearConvertedValue has invalid value. "
                                    "Value: `" +
                                        val +
                                        "`, allowed values are `+`, `-`, `positive` and "
                                        "`negative`.",
                                    configNode));
                            }
                        }
                        if (node.has_attribute("offset"))
                        {
                            tmpCfg.setOffset(node.attribute_as_float("offset"));
                        }
                        config.setLinearConfig(name, std::move(tmpCfg), tag);
                    }
                }
                else if (nodeName == "ModularConvertedValue")
                {
                    auto evaluate = [](const std::string& value) -> std::optional<float>
                    {
                        std::vector<std::string> parts = simox::alg::split(value, "^");
 
                        if (parts.size() == 1)
                        {
                            return std::stof(value);
                        }
                        else if (parts.size() == 2)
                        {
                            return std::pow(std::stof(parts.at(0)), std::stof(parts.at(1)));
                        }
 
                        return std::nullopt;
                    };
 
                    if (node.has_attribute("zeroOffset") and
                        node.has_attribute("discontinuityOffset") and
                        node.has_attribute("maxValue") and node.has_attribute("isInverted"))
                    {
                        std::optional<float> discontinuityOffset =
                            evaluate(node.attribute_value("discontinuityOffset"));
                        bool any_error = false;
 
                        if (not discontinuityOffset.has_value())
                        {
                            errors.push_back(createErrorMessageWithContext(
                                "Invalid value for discontinuityOffset in node " + nodeName,
                                configNode));
                            any_error = true;
                        }
 
                        std::optional<float> maxValue = evaluate(node.attribute_value("maxValue"));
 
                        if (not maxValue.has_value())
                        {
                            errors.push_back(createErrorMessageWithContext(
                                "Invalid value for maxValue in node " + nodeName, configNode));
                            any_error = true;
                        }
 
                        if (any_error)
                        {
                            continue;
                        }
 
                        config.setModularConvertedValueConfig(
                            name,
                            types::ModularConvertedValueConfig(
                                node.attribute_as_float("zeroOffset"),
                                discontinuityOffset.value(),
                                maxValue.value(),
                                node.attribute_as_bool("isInverted", "true", "false")),
                            tag);
                    }
                    else
                    {
                        errors.push_back(createErrorMessageWithContext(
                            "Not all required fields (zeroOffset, "
                            "discontinuityOffset, maxValue, isInverted) set for " +
                                nodeName,
                            configNode));
                    }
                }
                else if (nodeName == "String")
                {
                    config.setString(name, node.value(), tag);
                }
                else
                {
                    errors.push_back(createErrorMessageWithContext(
                        "Unknown node " + nodeName +
                            " in config node. Config node names are camel-cased.",
                        configNode));
                }
            }
            catch (const ConfigInsertError& e)
            {
                errors.push_back(e.what());
            }
        }
    }
 
    void
    ConfigParser::parseController(ControllerConfig& config,
                                  RapidXmlReaderNode& controllerNode,
                                  ConfigTag tag)
    {
        if (!controllerNode.has_attribute("type"))
        {
            errors.push_back(createErrorMessageWithContext(
                "Controller node does not have a type attribute", controllerNode));
            return;
        }
 
        std::string controllerType = controllerNode.attribute_value("type");
        auto it = config.controllerConfigs.find(controllerType);
        if (it != config.controllerConfigs.end())
        {
            parseConfigNode(*it->second, controllerNode, tag);
        }
        else
        {
            std::shared_ptr<Config> controllerConfig = std::make_shared<Config>();
            parseConfigNode(*controllerConfig, controllerNode, tag);
            config.addControllerConfig(controllerType, controllerConfig);
        }
    }
 
    void
    ConfigParser::parseProfiles(RapidXmlReaderNode& profilesNode)
    {
        for (auto& node : profilesNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName == "Slave")
            {
                std::shared_ptr<SlaveProfile> slaveConf;
                bool valid = parseSlaveProfile(slaveConf, node);
                if (valid)
                {
                    // Create type map if it does not alredy exist
                    if (slaveProfiles.find(slaveConf->getType()) == slaveProfiles.end())
                    {
                        slaveProfiles[slaveConf->getType()] = {};
                    }
 
                    auto& profilesWithType = slaveProfiles[slaveConf->getType()];
                    if (profilesWithType.find(slaveConf->getName()) != profilesWithType.end())
                    {
                        errors.push_back("Slave Profile with name " + slaveConf->getName() +
                                         " is defined multiple times for type " +
                                         slaveConf->getType());
                    }
                    profilesWithType[slaveConf->getName()] = slaveConf;
                }
            }
            else if (nodeName == "Device")
            {
                std::shared_ptr<DeviceConfigBase> deviceConf;
                bool valid = parseDeviceProfile(deviceConf, node);
                if (valid)
                {
                    // Create type map if it does not alredy exist
                    if (deviceProfiles.find(deviceConf->getType()) == deviceProfiles.end())
                    {
                        deviceProfiles[deviceConf->getType()] = {};
                    }
 
                    auto& profilesWithType = deviceProfiles[deviceConf->getType()];
                    std::string name = deviceConf->getName();
                    if (profilesWithType.find(name) != profilesWithType.end())
                    {
                        errors.push_back("Device Profile with name " + name +
                                         " is defined multiple times for type " +
                                         deviceConf->getType());
                    }
                    profilesWithType[name] = deviceConf;
                }
            }
            else
            {
                errors.push_back(createErrorMessageWithContext(
                    "Illegal " + nodeName + " node in Profiles node", profilesNode));
            }
        }
    }
 
    void
    ConfigParser::parseSlaveIdentifier(SlaveIdentifierConfig& identifier,
                                       RapidXmlReaderNode& identifierNode,
                                       std::optional<std::string> name,
                                       ConfigTag tag,
                                       bool checkIdentifierValid)
    {
        if (name.has_value())
        {
            identifier.setName(name.value(), tag);
        }
 
        auto vIds = identifierNode.nodes("VendorID");
        if (vIds.size() > 1)
        {
            errors.push_back(createErrorMessageWithContext(
                "Identifier of slave / profile has defined VendorID multiple times.",
                identifierNode));
        }
        else if (vIds.size() == 1)
        {
            identifier.setVendorID(vIds.at(0).value_as_uint32(), tag);
        }
 
        auto pCodes = identifierNode.nodes("ProductID");
        if (pCodes.size() > 1)
        {
            errors.push_back(createErrorMessageWithContext(
                "Identifier of slave / profile has defined ProductID multiple times.",
                identifierNode));
        }
        else if (pCodes.size() == 1)
        {
            identifier.setProductCode(pCodes.at(0).value_as_uint32(), tag);
        }
 
        auto serNums = identifierNode.nodes("Serial");
        if (serNums.size() > 1)
        {
            errors.push_back(createErrorMessageWithContext(
                "Identifier of slave / profile has defined Serial multiple times.",
                identifierNode));
        }
        else if (serNums.size() == 1)
        {
            identifier.setSerialNumber(serNums.at(0).value_as_uint32(), tag);
        }
 
        for (auto& node : identifierNode.nodes())
        {
            std::string nodeName = node.name();
            if (nodeName != "VendorID" && nodeName != "ProductID" && nodeName != "Serial")
            {
                errors.push_back(createErrorMessageWithContext(
                    "Identifier node of slave / profile contains illegal " + nodeName + " node",
                    identifierNode));
            }
        }
 
        if (checkIdentifierValid)
        {
            if (!identifier.isValid(errors))
            {
                errors.push_back(createErrorMessageWithContext(
                    "Identifier of slave / profile should be complete but is not", identifierNode));
            }
 
            testSlaveIdentifierUnique(identifier);
        }
    }
 
    template <typename T>
    bool
    ConfigParser::findProfile(std::shared_ptr<T>& ref,
                              std::string type,
                              std::string profileName,
                              std::string debugName,
                              std::map<std::string, std::map<std::string, std::shared_ptr<T>>>& map)
    {
        if (map.find(type) == map.end())
        {
            errors.push_back(debugName + " uses profile \"" + profileName + "\" and has type " +
                             type + " but there are no profiles defined for this type");
            return false;
        }
        else
        {
            auto& profilesWithType = map[type];
            if (profilesWithType.find(profileName) == profilesWithType.end())
            {
                errors.push_back(debugName + " uses profile \"" + profileName +
                                 "\" that is not defined for its type (type: " + type + ")");
                return false;
            }
            else
            {
                ref = profilesWithType[profileName];
                return true;
            }
        }
    }
 
    template <typename T>
    std::list<T>
    ConfigParser::parseList(std::string str, std::function<T(std::string)> func)
    {
        std::list<T> list;
        for (auto& s : armarx::Split(str, "\n \t\r", true, true))
        {
            try
            {
                list.push_back(func(s));
            }
            catch (...)
            {
                std::stringstream ss;
                ss << " A list has illegal value: Raw value: '" << s
                   << "', entire content string: '" << str << "'";
                throw ConfigInsertError(ss.str());
            }
        }
        return list;
    }
 
    template <typename T>
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
    ConfigParser::parseMatrix(RapidXmlReaderNode& node, std::function<T(std::string)> func)
    {
        if (!node.has_attribute("rows"))
        {
            throw ConfigInsertError("Matrix node missing \"rows\" attribute");
        }
        if (!node.has_attribute("columns"))
        {
            throw ConfigInsertError("Matrix node missing \"columns\" attribute");
        }
 
        std::uint32_t rows = node.attribute_as_uint("rows");
        std::uint32_t columns = node.attribute_as_uint("columns");
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> mat(rows, columns);
        unsigned int i = 0;
 
        for (auto& s : armarx::Split(node.value(), "\t \n\r", true, true))
        {
            if (i >= rows * columns)
            {
                std::stringstream ss;
                ss << " A matrix with " << rows << " rows and " << columns
                   << " columns has too many entries: Raw value: " << node.value();
                throw ConfigInsertError(ss.str());
            }
 
            try
            {
                mat(i / columns, i % columns) = func(s);
                i++;
            }
            catch (...)
            {
                std::string msg = createErrorMessageWithContext(
                    " A matrix has illegal value: Raw value: '" + s + "'", node);
                throw ConfigInsertError(msg);
            }
        }
 
        if (i < rows * columns)
        {
            std::stringstream ss;
            ss << " A matrix with " << rows << " rows and " << columns
               << " columns has not enough entries: Raw value: " << node.value();
            throw ConfigInsertError(ss.str());
        }
 
        return mat;
    }
 
    bool
    ConfigParser::strToBool(std::string s)
    {
        if (s == "true" || s == "True" || s == "1")
        {
            return true;
        }
        else if (s == "false" || s == "False" || s == "0")
        {
            return false;
        }
        else
        {
            errors.push_back("Bool has the following illegal value: " + s);
            return false;
        }
    }
 
    void
    ConfigParser::testSlaveIdentifierUnique(SlaveIdentifierConfig& identifier)
    {
        for (auto& id : identifiers)
        {
            if (id == identifier)
            {
                std::stringstream ss;
                ss << "Slave Identifier: " << identifier << " is not unique in HardwareConfig";
                errors.push_back(ss.str());
                return;
            }
        }
        identifiers.push_back(identifier);
    }
} // namespace armarx::control::hardware_config