CoeffMapLoader.cpp

Go to the documentation of this file.

#include "robot_instance/CoeffMapLoader.h"

using namespace std;
using namespace log4cpp;

/***************************************************************************/
void CoeffMapLoader::loadElements(RobotInstancePtr instance, CoeffMapPtr motorCoeffMap, CoeffMapPtr brainstemCoeffMap)
{
    FileMap::const_iterator itr;

    if (not motorCoeffMap)
    {
        motorCoeffMap.reset();
    }

    motorCoeffMap->clear();

    if (not brainstemCoeffMap)
    {
        brainstemCoeffMap.reset();
    }

    brainstemCoeffMap->clear();

    for (itr = instance->mechanismClassFileMap.begin(); itr != instance->mechanismClassFileMap.end(); ++itr)
    {
        const std::string& filename = instance->configurationBasePath + RobotInstance::COEFF_BASE_PATH + RobotInstance::COEFF_CLASS_PATH + (*itr).second;

        try
        {
            Private::loadElementsFromFile(filename, (*itr).first, motorCoeffMap, brainstemCoeffMap);
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", log4cpp::Priority::NOTICE, "Skipping non-safety coeff file [" + filename + "]");
        }
    }

    for (itr = instance->mechanismJointFileMap.begin(); itr != instance->mechanismJointFileMap.end(); ++itr)
    {
        const std::string& filename = instance->configurationBasePath + RobotInstance::COEFF_BASE_PATH + RobotInstance::COEFF_JOINT_PATH + instance->robotInstance + "/" + (*itr).second;

        try
        {
            Private::loadElementsFromFile(filename, (*itr).first, motorCoeffMap, brainstemCoeffMap);
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", log4cpp::Priority::NOTICE, "Skipping non-safety coeff file [" + filename + "]");
        }
    }

    for (itr = instance->mechanismClassSafetyFileMap.begin(); itr != instance->mechanismClassSafetyFileMap.end(); ++itr)
    {
        try
        {
            Private::loadElementsFromFile(instance->configurationSafetyBasePath + RobotInstance::COEFF_BASE_PATH + RobotInstance::COEFF_CLASS_SAFETY_PATH + (*itr).second, (*itr).first, motorCoeffMap, brainstemCoeffMap);
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", log4cpp::Priority::FATAL, e.what());
            throw runtime_error(e.what());
        }
    }

    for (itr = instance->mechanismJointSafetyFileMap.begin(); itr != instance->mechanismJointSafetyFileMap.end(); ++itr)
    {
        try
        {
            Private::loadElementsFromFile(instance->configurationSafetyBasePath + RobotInstance::COEFF_BASE_PATH + RobotInstance::COEFF_JOINT_SAFETY_PATH + instance->robotInstance + "/" + (*itr).second, (*itr).first, motorCoeffMap, brainstemCoeffMap);
        }
        catch (std::exception& e)
        {
            NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", log4cpp::Priority::FATAL, e.what());
            throw runtime_error(e.what());
        }
    }
}

/***************************************************************************/
void CoeffMapLoader::Private::loadElementsFromFile(const string& filename, const string& namePrefix, CoeffMapPtr motorCoeffMap, CoeffMapPtr brainstemCoeffMap)
{
    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.CoeffMapLoader", Priority::FATAL, err.str());
        throw runtime_error(err.str());
    }

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

    string elementString;

    // Check for CoeffData
    if (!(doc.FirstChildElement("CoeffData").ToElement()))
    {
        stringstream err;
        err << "The file " << filename << " has no element named [CoeffData]";
        //NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    TiXmlHandle currentCoeffElement = 0;

    // Check for Motor Coeffs
    TiXmlHandle motorCoeffsElement(doc.FirstChildElement("CoeffData").FirstChildElement("MotorCoeffs"));

    if (motorCoeffsElement.ToElement())
    {
        // Iterate over Coeffs
        currentCoeffElement = motorCoeffsElement.FirstChild("Coeff");

        while (currentCoeffElement.ToElement())
        {
            elementString.clear();
            elementString << *(currentCoeffElement.ToElement());
            elementString.append("\n");

            CoeffMap::value_type newCoeffMapMember(CoeffFactory::fromXml(elementString, namePrefix));

            if (motorCoeffMap->find(newCoeffMapMember.first) == motorCoeffMap->end())
            {
                motorCoeffMap->insert(newCoeffMapMember);
            }
            else
            {
                NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", Priority::WARN, "motorCoeffMap already contains element with key = " + newCoeffMapMember.first);
            }

            currentCoeffElement = currentCoeffElement.ToElement()->NextSiblingElement("Coeff");
        }
    }

    // Check for Brainstem Coeffs
    TiXmlHandle brainstemCoeffsElement(doc.FirstChildElement("CoeffData").FirstChildElement("BrainstemCoeffs"));

    if (brainstemCoeffsElement.ToElement())
    {
        // Iterate over Coeffs
        currentCoeffElement = brainstemCoeffsElement.FirstChild("Coeff");

        while (currentCoeffElement.ToElement())
        {
            elementString.clear();
            elementString << *(currentCoeffElement.ToElement());
            elementString.append("\n");

            CoeffMap::value_type newCoeffMapMember(CoeffFactory::fromXml(elementString, namePrefix));

            if (brainstemCoeffMap->find(newCoeffMapMember.first) == brainstemCoeffMap->end())
            {
                brainstemCoeffMap->insert(newCoeffMapMember);
            }
            else
            {
                NasaCommonLogging::Logger::log("gov.nasa.configuration.CoeffMapLoader", Priority::WARN, "brainstemCoeffMap already contains element with key = " + newCoeffMapMember.first);
            }

            currentCoeffElement = currentCoeffElement.ToElement()->NextSiblingElement("Coeff");
        }
    }
}