GripperStatusChecker.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/GripperStatusChecker.h"

GripperStatusChecker::GripperStatusChecker()
{
    timer = ros::Duration(0);
    requestTimeOut = ros::Duration(0);
    grantTimeOut = ros::Duration(0);
}

GripperStatusChecker::~GripperStatusChecker()
{
}

void GripperStatusChecker::initializeMap(const std::vector<std::string>& grippers)
{
    // Initialize zeros in the right ways
    GripperStatusCheckInfo initData;

    initData.pilotMode = r2_msgs::JointControlMode::BOOTLOADER;
    initData.captainMode = r2_msgs::JointControlMode::BOOTLOADER;
    initData.pilotRequest = false;
    initData.captainRequest = false;
    initData.engineerCheck = false;
    initData.state = IDLE;

    for (unsigned int i = 0; i < grippers.size(); ++i)
    {
        gripperInfoMap[grippers[i]] = initData;
    }

}

void GripperStatusChecker::setTimingParameters(const double& rTimeOut, const double& gTimeOut)
{
    requestTimeOut = ros::Duration(rTimeOut);
    grantTimeOut = ros::Duration(gTimeOut);
}

bool GripperStatusChecker::getMapEntry(const std::string& name, GripperStatusCheckInfo& info)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    if ((gripIt = gripperInfoMap.find(name)) != gripperInfoMap.end())
    {
        info = gripIt->second;
        return true;
    }

    return false;
}

int GripperStatusChecker::getState(const std::string& name)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    if ((gripIt = gripperInfoMap.find(name)) != gripperInfoMap.end())
    {
        return gripIt->second.state;
    }

    return -1;
}

void GripperStatusChecker::populateJointControlPilotInfo(const r2_msgs::JointControlDataArray& jointControlMsg)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    if (jointControlMsg.joint.size() == jointControlMsg.data.size())
    {
        for (unsigned int i = 0; i < jointControlMsg.joint.size(); ++i)
        {
            if ((gripIt = gripperInfoMap.find(jointControlMsg.joint[i])) != gripperInfoMap.end())
            {
                gripIt->second.pilotMode = jointControlMsg.data[i].controlMode.state;
            }
        }
    }

    return;
}

void GripperStatusChecker::populateJointControlCaptainInfo(const r2_msgs::JointControlDataArray& jointControlMsg)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    if (jointControlMsg.joint.size() == jointControlMsg.data.size())
    {
        for (unsigned int i = 0; i < jointControlMsg.joint.size(); ++i)
        {
            if ((gripIt = gripperInfoMap.find(jointControlMsg.joint[i])) != gripperInfoMap.end())
            {
                gripIt->second.captainMode = jointControlMsg.data[i].controlMode.state;
            }
        }
    }

    return;
}

void GripperStatusChecker::populatePilotRequestInfo(const r2_msgs::StringArray& pilotRequest)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    for (unsigned int i = 0; i < pilotRequest.data.size(); ++i)
    {
        if ((gripIt = gripperInfoMap.find(pilotRequest.data[i])) != gripperInfoMap.end())
        {
            gripIt->second.pilotRequest = true;
        }
    }

    return;
}

void GripperStatusChecker::populateCaptainRequestInfo(const r2_msgs::StringArray& captainRequest)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    for (unsigned int i = 0; i < captainRequest.data.size(); ++i)
    {
        if ((gripIt = gripperInfoMap.find(captainRequest.data[i])) != gripperInfoMap.end())
        {
            gripIt->second.captainRequest = true;
        }
    }

    return;
}

void GripperStatusChecker::createEngineerCheckMsg(r2_msgs::StringArray& engineerCheckMsg)
{
    engineerCheckMsg.header.stamp = ros::Time::now();
    engineerCheckMsg.data.clear();

    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    for (gripIt = gripperInfoMap.begin(); gripIt != gripperInfoMap.end(); ++gripIt)
    {
        if (gripIt->second.engineerCheck)
        {
            engineerCheckMsg.data.push_back(gripIt->first);
        }
    }

    return;
}

void GripperStatusChecker::createSweStopMessage(diagnostic_msgs::DiagnosticArray& sweStopMsg)
{
    sweStopMsg.header.stamp = ros::Time::now();
    sweStopMsg.status.clear();

    sweStopMsg.status = sweStopCollection.status;
    sweStopCollection.status.clear();

    return;
}

void GripperStatusChecker::update()
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    for (gripIt = gripperInfoMap.begin(); gripIt != gripperInfoMap.end(); ++gripIt)
    {
        if ((gripIt->second.pilotMode == r2_msgs::JointControlMode::DRIVE
                || gripIt->second.captainMode == r2_msgs::JointControlMode::DRIVE)
                && !gripIt->second.engineerCheck)
        {
            diagnostic_msgs::DiagnosticStatus diagnosticStatusTemp;
            diagnosticStatusTemp.name    = "GripperStatusChecker";
            diagnosticStatusTemp.level   = diagnostic_msgs::DiagnosticStatus::ERROR;
            diagnosticStatusTemp.hardware_id = gripIt->first;
            diagnosticStatusTemp.message = gripIt->first + " reported in DRIVE without engineer's okay!";
            sweStopCollection.status.push_back(diagnosticStatusTemp);

            clearRequestsAndChecks(gripIt->first);
            gripIt->second.state = IDLE;
        }

        if (gripIt->second.state == IDLE)
        {
            if (gripIt->second.pilotRequest && gripIt->second.captainRequest)
            {
                gripIt->second.engineerCheck = true;
                gripIt->second.state = GRANT;
                startTime = ros::Time::now();
            }
            else if (gripIt->second.pilotRequest)
            {
                gripIt->second.state = PILOT_REQUEST;
                startTime = ros::Time::now();
            }
            else if (gripIt->second.captainRequest)
            {
                gripIt->second.state = CAPTAIN_REQUEST;
                startTime = ros::Time::now();
            }

        }
        else if (gripIt->second.state == PILOT_REQUEST)
        {
            if (!gripIt->second.pilotRequest)
            {
                gripIt->second.state = IDLE;
            }
            else if (gripIt->second.captainRequest)
            {
                gripIt->second.engineerCheck = true;
                gripIt->second.state = GRANT;
                startTime = ros::Time::now();
            }
            else
            {
                timer = ros::Time::now() - startTime;

                if (timer > requestTimeOut)
                {
                    clearRequestsAndChecks(gripIt->first);
                    gripIt->second.state = IDLE;
                }
            }
        }
        else if (gripIt->second.state == CAPTAIN_REQUEST)
        {
            if (!gripIt->second.captainRequest)
            {
                gripIt->second.state = IDLE;
            }
            else if (gripIt->second.pilotRequest)
            {
                gripIt->second.engineerCheck = true;
                gripIt->second.state = GRANT;
                startTime = ros::Time::now();
            }
            else
            {
                timer = ros::Time::now() - startTime;

                if (timer > requestTimeOut)
                {
                    clearRequestsAndChecks(gripIt->first);
                    gripIt->second.state = IDLE;
                }
            }
        }
        else if (gripIt->second.state == GRANT)
        {
            timer = ros::Time::now() - startTime;

            if (timer > grantTimeOut && (gripIt->second.pilotMode != r2_msgs::JointControlMode::DRIVE
                                         || gripIt->second.captainMode != r2_msgs::JointControlMode::DRIVE))
            {
                clearRequestsAndChecks(gripIt->first);
                gripIt->second.state = IDLE;
            }
            else if (gripIt->second.pilotMode == r2_msgs::JointControlMode::DRIVE
                     && gripIt->second.captainMode == r2_msgs::JointControlMode::DRIVE)
            {
                gripIt->second.state = DRIVING;
                startTime = ros::Time::now();
            }
        }
        else if (gripIt->second.state == DRIVING)
        {
            timer = ros::Time::now() - startTime;

            if (gripIt->second.pilotMode == r2_msgs::JointControlMode::DRIVE
                    && gripIt->second.captainMode == r2_msgs::JointControlMode::DRIVE)
            {
                startTime = ros::Time::now();
            }
            else if (timer > requestTimeOut)
            {
                clearRequestsAndChecks(gripIt->first);
                gripIt->second.state = IDLE;
            }
        }
    }
}

void GripperStatusChecker::clearRequestsAndChecks(const std::string& gripper)
{
    std::map<std::string, GripperStatusCheckInfo>::iterator gripIt;

    if ((gripIt = gripperInfoMap.find(gripper)) != gripperInfoMap.end())
    {
        gripIt->second.pilotRequest = false;
        gripIt->second.captainRequest = false;
        gripIt->second.engineerCheck = false;
    }

    return;
}