RobotInstanceFactory.cpp

Go to the documentation of this file.

#include "robot_instance/RobotInstanceFactory.h"

using namespace std;
using namespace log4cpp;

void RobotInstanceFactory::populateRobotInstanceFromFile(RobotInstancePtr instance, const std::string& configPath, const std::string& configSafetyPath, const std::string& filenameNoPath)
{
    if (not instance)
    {
        instance = boost::make_shared<RobotInstance>();
    }

    instance->nodeTokenMap.clear();
    instance->mechanismRegisterMap.clear();
    instance->mechanismClassFileMap.clear();
    instance->mechanismJointFileMap.clear();
    instance->mechanismClassSafetyFileMap.clear();
    instance->mechanismCommandMap.clear();
    instance->mechanismControlMap.clear();
    instance->mechanisms.clear();
    instance->devices.clear();
    instance->commandableJoints.clear();
    instance->channelTypeMap.clear();
    instance->mechanismTypeMap.clear();
    instance->mechanismJointsMap.clear();
    instance->mechanismActuatorsMap.clear();
    instance->mechanismNodeMap.clear();
    instance->mechanismScheduleMap.clear();
    instance->jointScheduleNodeMap.clear();
    instance->actuatorScheduleNodeMap.clear();
    instance->deviceNodeMap.clear();
    instance->deviceScheduleMap.clear();
    instance->deviceRegisterMap.clear();
    instance->jointTypeMap.clear();
    instance->jointCommandPathMap.clear();
    instance->componentCpfsMap.clear();
    instance->deviceCalFileMap.clear();
    instance->deviceTypeMap.clear();
    instance->crew.clear();

    instance->configurationBasePath = configPath + "/";
    instance->configurationSafetyBasePath = configSafetyPath + "/";

    std::string filename = instance->configurationSafetyBasePath + RobotInstance::INSTANCE_PATH + filenameNoPath;

    // Load File
    TiXmlDocument file(filename.c_str());
    bool loadOkay = file.LoadFile();

    if (!loadOkay)
    {
        stringstream err;
        err << "Failed to load file [" << filename << "]";
        NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::FATAL, err.str());
        throw runtime_error(err.str());
    }

    TiXmlHandle doc(&file);
    NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::INFO, "RobotInstance file [" + filename + "] successfully loaded.");

    // Check for RobotInstance
    TiXmlHandle robotInstanceElement(doc.FirstChildElement("RobotInstance"));

    if (robotInstanceElement.ToElement())
    {
        instance->robotInstance = Private::getAttribute(robotInstanceElement, "id");
    }
    else
    {
        stringstream err;
        err << "The file " << filename << " has no element named [RobotInstance]";
        NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    // Check for ComponentProperties
    TiXmlHandle componentPropertiesElement(robotInstanceElement.FirstChildElement("ComponentProperties"));

    if (componentPropertiesElement.ToElement())
    {
        TiXmlHandle currentComponentElement = 0;
        TiXmlHandle currentCpfElement       = 0;
        string      currentComponentName;

        std::vector< std::pair<std::string, std::string> > cpfFiles;

        // Iterate over components
        currentComponentElement = componentPropertiesElement.FirstChild("ComponentCpfs");

        while (currentComponentElement.ToElement())
        {
            currentComponentName = Private::getAttribute(currentComponentElement, "id");

            // Iterate over CPF files
            cpfFiles.clear();
            currentCpfElement = currentComponentElement.FirstChild("ComponentCpf");

            while (currentCpfElement.ToElement())
            {
                cpfFiles.push_back(std::make_pair(Private::getAttribute(currentCpfElement, "value"), Private::getAttribute(currentCpfElement, "type")));

                currentCpfElement = currentCpfElement.ToElement()->NextSiblingElement("ComponentCpf");
            }

            // Store the file set in the map
            instance->componentCpfsMap[currentComponentName] = cpfFiles;

            currentComponentElement = currentComponentElement.ToElement()->NextSiblingElement("ComponentCpfs");
        }
    }

    // Check for MachineName
    TiXmlHandle machineNameElement(robotInstanceElement.FirstChildElement("MachineName"));

    if (machineNameElement.ToElement())
    {
        instance->machineName = Private::getAttribute(machineNameElement, "id");
    }
    else
    {
        stringstream err;
        err << "The file " << filename << " has no element named [MachineName]";
        NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    // Check for Friends
    TiXmlHandle crewElement(robotInstanceElement.FirstChildElement("Crew"));

    if (crewElement.ToElement())
    {
        TiXmlHandle currentMemberElement = 0;
        string currentMemberName;

        // Iterate over Friends
        currentMemberElement = crewElement.FirstChild("Member");

        while (currentMemberElement.ToElement())
        {
            currentMemberName = Private::getAttribute(currentMemberElement, "id");
            //cout << currentNodeName << endl;

            instance->crew.push_back(currentMemberName);

            currentMemberElement = currentMemberElement.ToElement()->NextSiblingElement("Member");
        }
    }

    // Check for KinematicModelFile
    TiXmlHandle kinematicModelElement(robotInstanceElement.FirstChildElement("KinematicModelFile"));

    if (kinematicModelElement.ToElement())
    {
        instance->kinematicModelFile = Private::getAttribute(kinematicModelElement, "id");
    }

    // Check for DynamicModelFile
    TiXmlHandle dynamicModelElement(robotInstanceElement.FirstChildElement("DynamicModelFile"));

    if (dynamicModelElement.ToElement())
    {
        instance->dynamicModelFile = Private::getAttribute(dynamicModelElement, "id");
    }

    // Check for MasterTokenFile
    TiXmlHandle masterTokenFileElement(robotInstanceElement.FirstChildElement("MasterTokenFile"));

    if (masterTokenFileElement.ToElement())
    {
        instance->masterTokenFile = Private::getAttribute(masterTokenFileElement, "id");
        //cout << instance->masterTokenFile << endl;
    }

    // Check for Nodes
    TiXmlHandle nodesElement(robotInstanceElement.FirstChildElement("Nodes"));

    if (nodesElement.ToElement())
    {
        TiXmlHandle currentNodeElement = 0;
        string      currentNodeName;
        string      currentNodeToken;

        // Iterate over joints
        currentNodeElement = nodesElement.FirstChild("Node");

        while (currentNodeElement.ToElement())
        {
            currentNodeName = Private::getAttribute(currentNodeElement, "id");

            instance->nodeTokenMap[currentNodeName] = Private::getAttribute(currentNodeElement, "token");

            currentNodeElement = currentNodeElement.ToElement()->NextSiblingElement("Node");
        }
    }

    // Check for Active Channels
    TiXmlHandle channelsElement(robotInstanceElement.FirstChildElement("ChannelModes"));

    if (nodesElement.ToElement())
    {
        TiXmlHandle currentChannelElement = 0;
        string      currentChannelName;
        string      currentChannelType;

        // Iterate over joints
        currentChannelElement = channelsElement.FirstChild("Channel");

        while (currentChannelElement.ToElement())
        {
            currentChannelName = Private::getAttribute(currentChannelElement, "id");

            currentChannelType = Private::getAttribute(currentChannelElement, "type");
            instance->channelTypeMap[currentChannelName] = currentChannelType;

            currentChannelElement = currentChannelElement.ToElement()->NextSiblingElement("Channel");
        }
    }

    // Check for Mechanisms
    TiXmlHandle mechanismsElement(robotInstanceElement.FirstChildElement("Mechanisms"));

    if (mechanismsElement.ToElement())
    {
        TiXmlHandle currentMechanismElement = 0;
        TiXmlHandle jointsElement           = 0;
        TiXmlHandle currentJointElement     = 0;
        TiXmlHandle actuatorsElement        = 0;
        TiXmlHandle currentActuatorElement  = 0;
        TiXmlHandle schedulesElement        = 0;
        TiXmlHandle currentScheduleElement  = 0;
        string      currentMechanismName;
        string      currentMechanismType;
        string      currentJointName;
        string      currentJointNameFQ;
        string      currentJointType;
        string      currentActuatorName;
        string      currentActuatorNameFQ;
        string      currentActuatorNode;
        string      currentActuatorSchedule;

        // Iterate over channels
        currentMechanismElement = mechanismsElement.FirstChild("Mechanism");

        while (currentMechanismElement.ToElement())
        {
            currentMechanismName = Private::getAttribute(currentMechanismElement, "id");
            instance->mechanisms.push_back(currentMechanismName);

            currentMechanismType = Private::getAttribute(currentMechanismElement, "type");
            instance->mechanismTypeMap[currentMechanismName] = currentMechanismType;

            instance->mechanismRegisterMap[currentMechanismName]    = Private::getSubelementAttribute(currentMechanismElement, "RegisterFile",    "id", false);

            if (instance->mechanismRegisterMap[currentMechanismName] == "")
            {
                instance->mechanismRegisterMap.erase(currentMechanismName);
            }

            instance->mechanismClassFileMap[currentMechanismName]       = Private::getSubelementAttribute(currentMechanismElement, "ClassFile",         "id");
            instance->mechanismJointFileMap[currentMechanismName]       = Private::getSubelementAttribute(currentMechanismElement, "JointFile",         "id");
            instance->mechanismClassSafetyFileMap[currentMechanismName] = Private::getSubelementAttribute(currentMechanismElement, "ClassSafetyFile",   "id");
            instance->mechanismJointSafetyFileMap[currentMechanismName] = Private::getSubelementAttribute(currentMechanismElement, "JointSafetyFile",   "id");
            instance->mechanismCommandMap[currentMechanismName]         = Private::getSubelementAttribute(currentMechanismElement, "CommandApiMapFile", "id");
            instance->mechanismControlMap[currentMechanismName]         = Private::getSubelementAttribute(currentMechanismElement, "ControlApiMapFile", "id");
            instance->mechanismSetpointMap[currentMechanismName]        = Private::getSubelementAttribute(currentMechanismElement, "SetpointFile",      "id", false);

            if (instance->mechanismSetpointMap[currentMechanismName] == "")
            {
                instance->mechanismSetpointMap.erase(currentMechanismName);
            }

            if (currentMechanismType == "simple")
            {
                instance->mechanismNodeMap[currentMechanismName]     = Private::getSubelementAttribute(currentMechanismElement, "Node",         "id");
                instance->mechanismScheduleMap[currentMechanismName] = Private::getSubelementAttribute(currentMechanismElement, "ScheduleFile", "id");
                instance->jointTypeMap[currentMechanismName]         = "active";
                instance->jointCommandPathMap[currentMechanismName]  = Private::getAttribute(currentMechanismElement, "commandPath");
                instance->commandableJoints.push_back(currentMechanismName);
                instance->mechanismJointsMap[currentMechanismName].push_back(currentMechanismName);
            }
            else if (currentMechanismType == "complex")
            {
                // Check for Joints
                jointsElement = currentMechanismElement.FirstChildElement("Joints");

                if (jointsElement.ToElement())
                {
                    // Iterate over joints
                    currentJointElement = jointsElement.FirstChild("Joint");

                    while (currentJointElement.ToElement())
                    {
                        currentJointName = Private::getAttribute(currentJointElement, "id");

                        currentJointNameFQ = currentMechanismName + StringUtilities::TOKEN_DELIMITER + currentJointName;

                        currentJointType = Private::getAttribute(currentJointElement, "type");
                        instance->jointTypeMap[currentJointNameFQ] = currentJointType;

                        if (currentJointType == "active")
                        {
                            instance->mechanismJointsMap[currentMechanismName].push_back(currentJointNameFQ);
                            instance->commandableJoints.push_back(currentJointNameFQ);
                        }
                        else if (currentJointType != "passive")
                        {
                            stringstream err;
                            err << "Joint " << currentJointNameFQ << " has unknown type [" << currentJointType << "]. Defaulting to passive.";
                            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::WARN, err.str());
                        }

                        instance->mechanismAllJointsMap[currentMechanismName].push_back(currentJointNameFQ);
                        instance->allJoints.push_back(currentJointNameFQ);

                        instance->jointCommandPathMap[currentJointNameFQ]  = Private::getAttribute(currentMechanismElement, "commandPath");

                        schedulesElement = currentJointElement.FirstChildElement("ScheduleFiles");

                        if (schedulesElement.ToElement())
                        {
                            currentScheduleElement = schedulesElement.FirstChild("ScheduleFile");

                            while (currentScheduleElement.ToElement())
                            {
                                instance->jointScheduleNodeMap[currentJointNameFQ].push_back(make_pair(Private::getAttribute(currentScheduleElement, "id"), Private::getAttribute(currentScheduleElement, "node")));

                                currentScheduleElement = currentScheduleElement.ToElement()->NextSiblingElement("ScheduleFile");
                            }
                        }
                        else
                        {
                            stringstream err;
                            err << "The file " << filename << " has no element named [ScheduleFiles]";
                            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
                            throw runtime_error(err.str());
                        }

                        currentJointElement = currentJointElement.ToElement()->NextSiblingElement("Joint");
                    }
                }
                else
                {
                    stringstream err;
                    err << "The file " << filename << " has no element named [Joints]";
                    NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
                    throw runtime_error(err.str());
                }

                // Check for Actuators
                actuatorsElement = currentMechanismElement.FirstChildElement("Actuators");

                if (actuatorsElement.ToElement())
                {
                    // Iterate over actuators
                    currentActuatorElement = actuatorsElement.FirstChild("Actuator");

                    while (currentActuatorElement.ToElement())
                    {
                        currentActuatorName = Private::getAttribute(currentActuatorElement, "id");

                        currentActuatorNameFQ = currentMechanismName + StringUtilities::TOKEN_DELIMITER + currentActuatorName;

                        instance->mechanismActuatorsMap[currentMechanismName].push_back(currentActuatorNameFQ);

                        schedulesElement = currentActuatorElement.FirstChildElement("ScheduleFiles");

                        if (schedulesElement.ToElement())
                        {
                            currentScheduleElement = schedulesElement.FirstChild("ScheduleFile");

                            while (currentScheduleElement.ToElement())
                            {
                                instance->actuatorScheduleNodeMap[currentActuatorNameFQ].push_back(make_pair(Private::getAttribute(currentScheduleElement, "id"), Private::getAttribute(currentScheduleElement, "node")));

                                currentScheduleElement = currentScheduleElement.ToElement()->NextSiblingElement("ScheduleFile");
                            }
                        }
                        else
                        {
                            stringstream err;
                            err << "The file " << filename << " has no element named [ScheduleFiles]";
                            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
                            throw runtime_error(err.str());
                        }

                        currentActuatorElement = currentActuatorElement.ToElement()->NextSiblingElement("Actuator");
                    }
                }
                else
                {
                    stringstream err;
                    err << "The file " << filename << " has no element named [Actuators]";
                    NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
                    throw runtime_error(err.str());
                }
            }

            currentMechanismElement = currentMechanismElement.ToElement()->NextSiblingElement("Mechanism");
        }
    }

    // Check for Devices
    TiXmlHandle devicesElement(robotInstanceElement.FirstChildElement("Devices"));

    if (devicesElement.ToElement())
    {
        TiXmlHandle currentDeviceElement = 0;
        string      currentDeviceName;
        string      currentDeviceNode;
        string      currentDeviceSchedule;

        // Iterate over joints
        currentDeviceElement = devicesElement.FirstChild("Device");

        while (currentDeviceElement.ToElement())
        {
            currentDeviceName = Private::getAttribute(currentDeviceElement, "id");

            instance->devices.push_back(currentDeviceName);

            instance->deviceNodeMap[currentDeviceName]     = Private::getAttribute(currentDeviceElement, "node");
            instance->deviceScheduleMap[currentDeviceName] = Private::getAttribute(currentDeviceElement, "schedule");
            instance->deviceRegisterMap[currentDeviceName]  = Private::getAttribute(currentDeviceElement, "register", false);

            if (instance->deviceRegisterMap[currentDeviceName] == "")
            {
                instance->deviceRegisterMap.erase(currentDeviceName);
            }

            instance->deviceCalFileMap[currentDeviceName] = Private::getAttribute(currentDeviceElement, "calFile", false);

            if (instance->deviceCalFileMap[currentDeviceName] == "")
            {
                instance->deviceCalFileMap.erase(currentDeviceName);
            }

            instance->deviceTypeMap[currentDeviceName] = Private::getAttribute(currentDeviceElement, "type", false);

            if (instance->deviceTypeMap[currentDeviceName] == "")
            {
                instance->deviceTypeMap.erase(currentDeviceName);
            }

            currentDeviceElement = currentDeviceElement.ToElement()->NextSiblingElement("Device");
        }
    }

    NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::NOTICE, "RobotInstance file [" + filename + "] successfully parsed.");
}

RobotInstancePtr RobotInstanceFactory::createRobotInstanceFromFile(const std::string& configPath, const std::string& configSafetyPath, const string& filename)
{
    RobotInstancePtr instance = boost::make_shared<RobotInstance>();

    populateRobotInstanceFromFile(instance, configPath, configSafetyPath, filename);

    return instance;
}

const string RobotInstanceFactory::Private::getAttribute(TiXmlHandle handle, const string& attribute, bool required)
{
    if (handle.ToElement()->Attribute(attribute))
    {
        return *(handle.ToElement()->Attribute(attribute));
    }
    else
    {
        if (required)
        {
            stringstream err;
            err << "The attribute [" << attribute << "] was not found for element [" << handle.ToElement()->ValueStr() << "]";
            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        return "";
    }
}

const string RobotInstanceFactory::Private::getSubelementAttribute(TiXmlHandle handle, const string& subelementName, const string& attribute, bool required)
{
    if (handle.FirstChildElement(subelementName).ToElement())
    {
        return Private::getAttribute(handle.FirstChildElement(subelementName), attribute, required);
    }
    else
    {
        if (required)
        {
            stringstream err;
            err << "No [" << subelementName << "] subelement found for element [" << handle.ToElement()->ValueStr() << "]";
            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::INFO, err.str());
            throw runtime_error(err.str());
        }

        return "";
    }
}

const string RobotInstanceFactory::Private::getNodeFile(TiXmlHandle handle, const string& subelementName, const string& defaultFileName)
{
    try
    {
        return Private::getSubelementAttribute(handle, subelementName, "id");
    }
    catch (runtime_error& e)
    {
        if (defaultFileName.size() > 0)
        {
            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::INFO, "No [" + subelementName + "] element found for [" + getAttribute(handle, "id") + "], using default value.");
            return defaultFileName;
        }
        else
        {
            stringstream err;
            err << "No [" << subelementName << "] element found and no default value to use";
            NasaCommonLogging::Logger::log("gov.nasa.configuration.RobotInstanceFactory", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }
    }
}