JointControlActualWrist.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/JointControlActualWrist.h"
/***************************************************************************/
JointControlActualWrist::JointControlActualWrist(const std::string& mechanism, IoFunctions ioFunctions)
    : JointControlActualInterface(mechanism, ioFunctions), logCategory("gov.nasa.JointControlActualWrist")
{
    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();
}

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

void JointControlActualWrist::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.");

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

    if (parametersElement.ToElement())
    {
        // Live coeffs names
        PitchLimitLiveCoeffName            = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "PitchLimitLiveCoeff"));
        YawLimitLiveCoeffName              = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "YawLimitLiveCoeff"));
        LittlesideSliderLimitLiveCoeffName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "LittlesideSliderLimitLiveCoeff"));
        ThumbsideSliderLimitLiveCoeffName  = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ThumbsideSliderLimitLiveCoeff"));
        SensorErrorLiveCoeffName           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "SensorErrorLiveCoeff"));
        SliderDiffErrorLiveCoeffName       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "SliderDiffErrorLiveCoeff"));
        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);

    // Set up default values @bootup (these will get overwritten in JointCommandApi)
    io.setLiveCoeff(PitchLimitLiveCoeffName,            0);
    io.setLiveCoeff(YawLimitLiveCoeffName,              0);
    io.setLiveCoeff(LittlesideSliderLimitLiveCoeffName, 0);
    io.setLiveCoeff(ThumbsideSliderLimitLiveCoeffName,  0);
    io.setLiveCoeff(SensorErrorLiveCoeffName,           0);
    io.setLiveCoeff(SliderDiffErrorLiveCoeffName,       0);
}

/***************************************************************************/
void JointControlActualWrist::getStates(r2_msgs::JointControlData& actualStates)
{
    updateCommandModeState(); // Must come before updateControlModeState()
    updateControlModeState();
    updateCalibrationModeState();
    updateClearFaultModeState();
    updateCoeffState();
    actualStates = states;
}
/***************************************************************************/
void JointControlActualWrist::updateControlModeState(void)
{

    states.controlMode.state = r2_msgs::JointControlMode::INVALID;

    // Verify existence of live coeffs
    if (not(io.hasLiveCoeff(ControlModeLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << ControlModeLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(PitchLimitLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << PitchLimitLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(YawLimitLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << YawLimitLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(LittlesideSliderLimitLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << LittlesideSliderLimitLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(ThumbsideSliderLimitLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << ThumbsideSliderLimitLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(SensorErrorLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << SensorErrorLiveCoeffName;
        return;
    }

    if (not(io.hasLiveCoeff(SliderDiffErrorLiveCoeffName)))
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << SliderDiffErrorLiveCoeffName;
        return;
    }

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

    // Check for FAULTED state
    if ((io.getLiveCoeff(PitchLimitLiveCoeffName)            == 1) ||
            (io.getLiveCoeff(YawLimitLiveCoeffName)              == 1) ||
            (io.getLiveCoeff(LittlesideSliderLimitLiveCoeffName) == 1) ||
            (io.getLiveCoeff(ThumbsideSliderLimitLiveCoeffName)  == 1) ||
            (io.getLiveCoeff(SensorErrorLiveCoeffName)           == 1) ||
            (io.getLiveCoeff(SliderDiffErrorLiveCoeffName)       == 1))
    {
        if (states.controlMode.state != r2_msgs::JointControlMode::FAULTED)
        {
            NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Transition to JointControlMode::FAULTED on joint: " << mechanism;
            states.controlMode.state = r2_msgs::JointControlMode::FAULTED;
        }

        return;
    }

    // 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 JointControlActualWrist::updateCommandModeState(void)
{
    if (not(io.hasLiveCoeff(CommandModeLiveCoeffName)))
    {
        states.commandMode.state = r2_msgs::JointControlCommandMode::INVALID;
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << CommandModeLiveCoeffName;
        return;
    }

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

    states.calibrationMode.state = io.getLiveCoeff(CalibrationModeLiveCoeffName);
}
/***************************************************************************/
void JointControlActualWrist::updateClearFaultModeState(void)
{
    if (not(io.hasLiveCoeff(ClearFaultStateLiveCoeffName)))
    {
        states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::ERROR << mechanism << ": Missing live coeff: " << ClearFaultStateLiveCoeffName;
        return;
    }

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

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

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

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

        if (io.getLiveCoeff(PitchLimitLiveCoeffName)            == 1)
        {
            message << ", PitchLimit";
        }

        if (io.getLiveCoeff(YawLimitLiveCoeffName)              == 1)
        {
            message << ", YawLimit";
        }

        if (io.getLiveCoeff(LittlesideSliderLimitLiveCoeffName) == 1)
        {
            message << ", LittlesideSliderLimit";
        }

        if (io.getLiveCoeff(ThumbsideSliderLimitLiveCoeffName)  == 1)
        {
            message << ", ThumbsideSliderLimit";
        }

        if (io.getLiveCoeff(SensorErrorLiveCoeffName)           == 1)
        {
            message << ", SensorError";
        }

        if (io.getLiveCoeff(SliderDiffErrorLiveCoeffName)       == 1)
        {
            message << ", SliderDiffError";
        }

        faultStatus.message = message.str().c_str();
    }
}