JointControlActualFinger.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/JointControlActualFinger.h"
/***************************************************************************/
JointControlActualFinger::JointControlActualFinger(const std::string& mechanism, IoFunctions ioFunctions)
    : JointControlActualInterface(mechanism, ioFunctions), logCategory("gov.nasa.JointControlActualFinger")
{
    if (io.hasLiveCoeff.empty()    or
            io.getLiveCoeff.empty()    or
            io.setLiveCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.hasLiveCoeff', 'io.getLiveCoeff', 'io.setLiveCoeff' be non-empty.";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    setParameters();
}

JointControlActualFinger::~JointControlActualFinger()
{
    // Nothing
}

void JointControlActualFinger::setParameters()
{
    std::string parameterFile = io.getControlFile(mechanism);

    TiXmlDocument file(parameterFile.c_str());
    bool          loadOkay = file.LoadFile();

    if (!loadOkay)
    {
        std::stringstream err;
        err << "Failed to load file [" << parameterFile << "]";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
        throw std::runtime_error(err.str());
    }

    TiXmlHandle doc(&file);
    NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::INFO, "File [" + parameterFile + "] successfully loaded.");

    // Print the XML file's contents
    //cout << "Successfully loaded file: " << parameterFile << endl;
    //TiXmlPrinter printer;
    //printer.SetIndent("\t");
    //file.Accept(&printer);
    //cout << printer.Str() << endl;

    // Check for ApiMap
    TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));

    if (parametersElement.ToElement())
    {
        // Live coeffs names
//        IsCalibratedLiveCoeffName          = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "IsCalibratedLiveCoeff"));
        ControlModeLiveCoeffName           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ControlModeLiveCoeff"));
        CommandModeLiveCoeffName           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CommandModeLiveCoeff"));
        CalibrationModeLiveCoeffName       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CalibrationModeLiveCoeff"));
        CoeffStateLiveCoeffName            = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CoeffStateLiveCoeff"));
        ClearFaultStateLiveCoeffName       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ClearFaultStateLiveCoeff"));
    }
    else
    {
        std::stringstream err;
        err << "The file " << parameterFile << " has no element named [ApiMap]";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    // Set up expected values @bootup
    io.setLiveCoeff(ControlModeLiveCoeffName,     r2_msgs::JointControlMode::BOOTLOADER);
    io.setLiveCoeff(CommandModeLiveCoeffName,     r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
    io.setLiveCoeff(CalibrationModeLiveCoeffName, r2_msgs::JointControlCalibrationMode::UNCALIBRATED);
    io.setLiveCoeff(CoeffStateLiveCoeffName,      r2_msgs::JointControlCoeffState::LOADED);
    io.setLiveCoeff(ClearFaultStateLiveCoeffName, r2_msgs::JointControlClearFaultMode::DISABLE);
}

/***************************************************************************/
void JointControlActualFinger::getStates(r2_msgs::JointControlData& actualStates)
{
    updateCommandModeState(); // Must come before updateControlModeState()
    updateControlModeState();
    updateCalibrationModeState();
    updateClearFaultModeState();
    updateCoeffState();
    actualStates = states;
}
/***************************************************************************/
void JointControlActualFinger::updateControlModeState(void)
{
    // Verify existence of live coeffs
    if (not(io.hasLiveCoeff(ControlModeLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << ControlModeLiveCoeffName;
        return;
    }

    states.controlMode.state = io.getLiveCoeff(ControlModeLiveCoeffName);

    // Check for valid DRIVE ( DRIVE cannot occur unless calibration is DISABLE )
    if ( (states.controlMode.state == r2_msgs::JointControlMode::DRIVE) &&
            (io.getLiveCoeff(CalibrationModeLiveCoeffName) != r2_msgs::JointControlCalibrationMode::DISABLE))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Can not DRIVE until JointControlCalibrationMode is DISABLE";
        states.controlMode.state = r2_msgs::JointControlMode::PARK;
    }
}
/***************************************************************************/
void JointControlActualFinger::updateCommandModeState(void)
{
    // Verify existence of live coeffs
    if (not(io.hasLiveCoeff(CommandModeLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << CommandModeLiveCoeffName;
        return;
    }

    states.commandMode.state = io.getLiveCoeff(CommandModeLiveCoeffName);
}
/***************************************************************************/
void JointControlActualFinger::updateCalibrationModeState(void)
{
    if (not(io.hasLiveCoeff(CalibrationModeLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << CalibrationModeLiveCoeffName;
        return;
    }

    states.calibrationMode.state = io.getLiveCoeff(CalibrationModeLiveCoeffName);

//    // Check for DISABLE
//    if (io.getLiveCoeff(IsCalibratedLiveCoeffName) == 1)
//    {
//        if (states.calibrationMode.state != r2_msgs::JointControlCalibrationMode::DISABLE)
//        {
//            NasaCommonLogging::Logger::getCategory("gov.nasa.JointControlActualFsmFinger") << log4cpp::Priority::DEBUG << "Transition to JointControlCalibrationMode::DISABLE on joint: " << mechanism;
//            states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
//        }
//        return;
//    }

//    // If not DISABLE then ENABLE
//    if (states.calibrationMode.state != r2_msgs::JointControlCalibrationMode::ENABLE)
//    {
//        NasaCommonLogging::Logger::getCategory("gov.nasa.JointControlActualFsmFinger") << log4cpp::Priority::DEBUG << "Transition to JointControlCalibrationMode::ENABLE on joint: " << mechanism;
//        states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
//    }
}
/***************************************************************************/
void JointControlActualFinger::updateClearFaultModeState(void)
{
    if (not(io.hasLiveCoeff(ClearFaultStateLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << ClearFaultStateLiveCoeffName;
        return;
    }

    states.clearFaultMode.state = io.getLiveCoeff(ClearFaultStateLiveCoeffName);
}
/***************************************************************************/
void JointControlActualFinger::updateCoeffState(void)
{
    if (not(io.hasLiveCoeff(CoeffStateLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << CoeffStateLiveCoeffName;
        return;
    }

    states.coeffState.state = io.getLiveCoeff(CoeffStateLiveCoeffName);
}

void JointControlActualFinger::getFaults(diagnostic_msgs::DiagnosticStatus& faultStatus)
{
    faultStatus.name = mechanism;
    faultStatus.hardware_id = mechanism;
    faultStatus.level = diagnostic_msgs::DiagnosticStatus::OK;
    faultStatus.message = "OK";

    if (states.controlMode.state == r2_msgs::JointControlMode::FAULTED)
    {
        faultStatus.level = diagnostic_msgs::DiagnosticStatus::ERROR;
        faultStatus.message = "FAULT state detected on " + mechanism + ": FAULT";
    }

}