GripperSupervisor.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/GripperSupervisor.h"
#include <iostream>

GripperSupervisor::GripperSupervisor(): setpoint("null"), motcomLimit(30), pendingTimer(0)
{
}

GripperSupervisor::~GripperSupervisor()
{
}

void GripperSupervisor::initializeMaps(const std::vector<std::string>& grippers)
{
    // Initialize zeros in the right ways
    EndEffectorData zeroData;
    zeroData.locked   = false;
    zeroData.loaded   = false;
    zeroData.verified = false;

    GripperControlModes initMode;
    initMode.state = r2_msgs::JointControlMode::IGNORE;
    initMode.coeffs = r2_msgs::JointControlCoeffState::NOTLOADED;

    for (unsigned int i = 0; i < grippers.size(); i++)
    {
        endEffectorMap[grippers[i]]      = zeroData;
        errorStates[grippers[i]]         = false;
        stateMap[grippers[i]]            = IDLE;
        gripperJointModeMap[grippers[i]] = initMode;
        gripperDriveApprovedMap[grippers[i]] = false;
    }

    return;

}

void GripperSupervisor::populateGripperStateInfo(const r2_msgs::GripperPositionState& eefState)
{
    // Iterate through all end effectors in message
    if (eefState.name.size() == eefState.locked.size() && eefState.name.size() == eefState.loaded.size())
    {
        for (unsigned int i = 0; i < eefState.name.size(); i++)
        {
            if ((eefIt = endEffectorMap.find(eefState.name[i])) != endEffectorMap.end())
            {
                eefIt->second.locked = eefState.locked[i];
                eefIt->second.loaded = eefState.loaded[i];
            }
        }
    }

    return;

}

void GripperSupervisor::populateBaseFrameInfo(const r2_msgs::StringArray& baseFrames)
{
    baseFrameList = baseFrames.data;

    return;
}

void GripperSupervisor::populateGoalStatus(const actionlib_msgs::GoalStatusArray& goalStats)
{
    for (unsigned int i = 0; i < goalStats.status_list.size(); i++)
    {
        // clear this vector after using it in transitions
        if (goalStats.status_list[i].status == actionlib_msgs::GoalStatus::ABORTED ||
                goalStats.status_list[i].status == actionlib_msgs::GoalStatus::SUCCEEDED)
        {
            goalStatuses.status_list.push_back(goalStats.status_list[i]);
        }
    }

    return;
}

void GripperSupervisor::populateGripperCommandsIn(const r2_msgs::GripperPositionCommand& gripCmd)
{
    // clear this vector after using it in transitions
    gripperCmds = gripCmd;

    actionlib_msgs::GoalStatus gStat;
    gStat.goal_id.stamp = gripperCmds.header.stamp;
    gStat.status        = actionlib_msgs::GoalStatus::PENDING;
    gStat.text          = "Command pending";

    for (unsigned int i = 0; i < gripperCmds.name.size(); i++)
    {
        gStat.goal_id.id = gripperCmds.name[i];
        goalStatusesOut.status_list.push_back(gStat);
    }

    return;
}

void GripperSupervisor::populateJointControlMode(const r2_msgs::JointControlDataArray& jntMode)
{
    if (jntMode.joint.size() == jntMode.data.size())
    {
        for (unsigned int i = 0; i < jntMode.joint.size(); i++)
        {
            if ((intIt = gripperJointModeMap.find(jntMode.joint[i])) != gripperJointModeMap.end())
            {
                intIt->second.state = jntMode.data[i].controlMode.state;
                intIt->second.coeffs = jntMode.data[i].coeffState.state;
            }
        }
    }

    return;
}

void GripperSupervisor::populateDriveRequestMsg(r2_msgs::StringArray& requestOut)
{
    requestOut = driveRequest;
    requestOut.header.stamp = ros::Time::now();

    driveRequest.data.clear();
    return;
}

void GripperSupervisor::populateJointControlCommandMsg(r2_msgs::JointControlDataArray& modeCmdOut)
{
    modeCmdOut = jointModeCommands;
    modeCmdOut.header.stamp = ros::Time::now();

    jointModeCommands.joint.clear();
    jointModeCommands.data.clear();
    return;
}

void GripperSupervisor::populateGoalStatusMsg(actionlib_msgs::GoalStatusArray& statusOut)
{
    statusOut = goalStatusesOut;
    goalStatusesOut.status_list.clear();
    return;
}

void GripperSupervisor::populateGripperCommandMsg(r2_msgs::GripperPositionCommand& cmdOut)
{
    // put all commands into one message
    cmdOut              = gripperCmdOut;
    cmdOut.header.stamp = ros::Time::now();

    gripperCmdOut.name.clear();
    gripperCmdOut.setpoint.clear();
    gripperCmdOut.command.clear();
    gripperCmdOut.dutyCycle.clear();

    return;
}

void GripperSupervisor::populateCheckedGrippers(const bool override)
{
    for (reqIt = gripperDriveApprovedMap.begin(); reqIt != gripperDriveApprovedMap.end(); ++reqIt)
    {
        reqIt->second = override;
    }
}

void GripperSupervisor::populateCheckedGrippers(const r2_msgs::StringArray& grippers)
{
    for (reqIt = gripperDriveApprovedMap.begin(); reqIt != gripperDriveApprovedMap.end(); ++reqIt)
    {
        reqIt->second = false;
    }

    for (unsigned int i = 0; i < grippers.data.size(); ++i)
    {
        reqIt = gripperDriveApprovedMap.find(grippers.data[i]);

        if (reqIt != gripperDriveApprovedMap.end())
        {
            reqIt->second = true;
        }
    }
}

void GripperSupervisor::checkTransitions(int baseOverride)
{
    for (errIt = errorStates.begin(); errIt != errorStates.end(); ++errIt)
    {
        if ((intIt = gripperJointModeMap.find(errIt->first)) != gripperJointModeMap.end())
        {
            if (intIt->second.coeffs == r2_msgs::JointControlCoeffState::LOADED)
            {
                int change = detectChangeInErrorState(errIt->first);

                if (change > 0 && !errIt->second)
                {
                    updateErrorState(errIt->first);
                    transToFault(errIt->first);
                }
                else if (change > 0 && errIt->second)
                {
                    if (transToIdle(errIt->first))
                    {
                        updateErrorState(errIt->first);
                    }
                }
            }
        }
    }

    for (unsigned int i = 0; i < goalStatuses.status_list.size(); i++)
    {
        std::string name = goalStatuses.status_list[i].goal_id.id;

        if (stateMap.find(name) != stateMap.end() && (intIt = gripperJointModeMap.find(name)) != gripperJointModeMap.end())
        {
            if (intIt->second.state != r2_msgs::JointControlMode::FAULTED)
            {
                if (goalStatuses.status_list[i].status == actionlib_msgs::GoalStatus::ABORTED)
                {
                    transToFailure(name);

                    // send warning via status message
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::ABORTED;
                    gStat.text          = "Transition to failure";
                    goalStatusesOut.status_list.push_back(gStat);
                }
                else
                {
                    // will send multiple park commands after time out in state machine if this doesn't work
                    transToIdle(name);

                    // send warning via status message
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id = name;
                    gStat.status = actionlib_msgs::GoalStatus::SUCCEEDED;
                    gStat.text = "Command succeeded";
                    goalStatusesOut.status_list.push_back(gStat);
                }
            }
        }

        runStateMachine(name);
    }

    goalStatuses.status_list.clear();

    for (unsigned int i = 0; i < gripperCmds.name.size(); i++)
    {
        std::string name = gripperCmds.name[i];

        if ((stateIt = stateMap.find(name)) != stateMap.end() && (intIt = gripperJointModeMap.find(name)) != gripperJointModeMap.end())
        {
            if (intIt->second.coeffs == r2_msgs::JointControlCoeffState::LOADED)
            {
                if (gripperCmds.command[i] == "cancel")
                {
                    // add command to cancel this gripper
                    gripperCmdOut.name.push_back(gripperCmds.name[i]);
                    gripperCmdOut.setpoint.push_back(gripperCmds.setpoint[i]);
                    gripperCmdOut.command.push_back("c_cancel");
                    gripperCmdOut.dutyCycle.push_back(hiDutyCycleLimit);

                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = gripperCmds.name[i];
                    gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                    gStat.text          = "Sending cancel";
                    goalStatusesOut.status_list.push_back(gStat);

                    if (!(stateIt->second == FAULT or stateIt->second == FAILURE))
                    {
                        transToIdle(name);
                    }
                }
                else if (gripperCmds.command[i] == "reset")
                {
                    if (stateIt->second == FAILURE)
                    {
                        transToIdle(name);
                    }
                }
                else if (gripperCmds.command[i] == "lock")
                {
                    if (!(stateIt->second == FAULT or stateIt->second == FAILURE or isLocked(name)))
                    {
                        setpoint    = gripperCmds.setpoint[i];
                        motcomLimit = hiDutyCycleLimit;
                        transToLock(name);
                    }
                    else
                    {
                        // send warning via status message
                        actionlib_msgs::GoalStatus gStat;
                        gStat.goal_id.stamp = gripperCmds.header.stamp;
                        gStat.goal_id.id    = name;
                        gStat.status        = actionlib_msgs::GoalStatus::REJECTED;
                        gStat.text          = "Lock not valid";
                        goalStatusesOut.status_list.push_back(gStat);
                    }
                }
                else if (gripperCmds.command[i] == "release")
                {
                    if (!(stateIt->second == FAULT or stateIt->second == FAILURE) && (isReleaseValid(name) || baseOverride == 7))
                    {
                        setpoint    = gripperCmds.setpoint[i];
                        motcomLimit = hiDutyCycleLimit;
                        transToRelease(name);
                    }
                    else
                    {
                        // send warning via status message
                        actionlib_msgs::GoalStatus gStat;
                        gStat.goal_id.stamp = gripperCmds.header.stamp;
                        gStat.goal_id.id    = name;
                        gStat.status        = actionlib_msgs::GoalStatus::REJECTED;
                        gStat.text          = "Release not valid";
                        goalStatusesOut.status_list.push_back(gStat);
                    }
                }
                else if (gripperCmds.command[i] == "set")
                {
                    // including "backdoor" to allow gripper to close regardless of other base frames being present
                    if (!(stateIt->second == FAULT or stateIt->second == FAILURE) && (isReleaseValid(name) || baseOverride == 7))
                    {
                        setpoint = gripperCmds.setpoint[i];
                        motcomLimit = hiDutyCycleLimit;
                        transToSet(name);
                    }
                    else
                    {
                        // send warning via status message
                        actionlib_msgs::GoalStatus gStat;
                        gStat.goal_id.stamp = gripperCmds.header.stamp;
                        gStat.goal_id.id    = name;
                        gStat.status        = actionlib_msgs::GoalStatus::REJECTED;
                        gStat.text          = "Set not valid";
                        goalStatusesOut.status_list.push_back(gStat);
                    }
                }
                else
                {
                    // send warning via status message
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::REJECTED;
                    gStat.text          = "Not a valid command";
                    goalStatusesOut.status_list.push_back(gStat);
                }
            }
        }

        runStateMachine(name);
    }

    gripperCmds.name.clear();
    gripperCmds.setpoint.clear();
    gripperCmds.command.clear();
    gripperCmds.dutyCycle.clear();

    for (errIt = errorStates.begin(); errIt != errorStates.end(); ++errIt)
    {
        runStateMachine(errIt->first);
    }

    return;
}

int GripperSupervisor::detectChangeInErrorState(const std::string& name)
{

    if ((errIt = errorStates.find(name)) == errorStates.end() || (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end())
    {
        return -1;
    }

    if ((intIt->second.state == r2_msgs::JointControlMode::FAULTED and !errIt->second)
            || (intIt->second.state != r2_msgs::JointControlMode::FAULTED and errIt->second))
    {
        return 1; // true, there is a change!
    }

    return 0;  // no change
}

int GripperSupervisor::updateErrorState(const std::string& name)
{
    if ((errIt = errorStates.find(name)) == errorStates.end() || (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end())
    {
        return -1;
    }

    if (intIt->second.state == r2_msgs::JointControlMode::FAULTED)
    {
        errIt->second = true;
    }
    else
    {
        errIt->second = false;
    }

    return 0;
}

bool GripperSupervisor::isReleaseValid(const std::string& name)
{
    std::string oppositeGripper = FindOpposite(name);
    std::string oppositeBase = FindOpposite(GripperToBaseFrame(name));
    unsigned int             numBaseFrames = baseFrameList.size();
    bool matches = false;

    if (oppositeBase.empty() || oppositeGripper.empty())
    {
        return false; // not valid
    }

    for (unsigned int i = 0; i < numBaseFrames; i++)
    {
        if (oppositeBase == baseFrameList[i])
        {
            matches = true;
            break;
        }
    }

    if (!matches)
    {
        return false; // other gripper isn't a base frame
    }

    stateIt = stateMap.find(oppositeGripper);

    if (stateIt != stateMap.end())
    {
        if (stateIt->second != IDLE)
        {
            return false;
        }
    }
    else
    {
        return false;
    }

    std::map<std::string, GripperControlModes>::const_iterator itm = gripperJointModeMap.find(oppositeGripper);

    if (itm != gripperJointModeMap.end())
    {
        if (itm->second.state != r2_msgs::JointControlMode::FAULTED && itm->second.state != r2_msgs::JointControlMode::DRIVE)
        {
            return true; // valid
        }
    }

    return false;  // not valid
}

bool GripperSupervisor::isLocked(const std::string& name)
{
    if ((eefIt = endEffectorMap.find(name)) != endEffectorMap.end())
    {
        if (eefIt->second.locked == 1)
        {
            return true;
        }
    }

    return false;
}

bool GripperSupervisor::transToFault(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() || (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end())
    {
        return false;
    }

    // add a command to park gripper if in drive
    if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
    {
        r2_msgs::JointControlData jcd;
        jcd.controlMode.state = r2_msgs::JointControlMode::PARK;
        jointModeCommands.joint.push_back(name);
        jointModeCommands.data.push_back(jcd);
        actionlib_msgs::GoalStatus gStat;
        gStat.goal_id.stamp = gripperCmds.header.stamp;
        gStat.goal_id.id    = name;
        gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
        gStat.text          = "Trans to fault- parking joint";
        goalStatusesOut.status_list.push_back(gStat);
    }

    // add command to cancel this gripper
    gripperCmdOut.name.push_back(name);
    gripperCmdOut.setpoint.push_back(setpoint);
    gripperCmdOut.command.push_back("c_cancel");
    gripperCmdOut.dutyCycle.push_back(0);

    actionlib_msgs::GoalStatus gStat;
    gStat.goal_id.stamp = gripperCmds.header.stamp;
    gStat.goal_id.id    = name;
    gStat.status        = actionlib_msgs::GoalStatus::ABORTED;
    gStat.text          = "Sending cancel due to gripper fault";
    goalStatusesOut.status_list.push_back(gStat);

    stateMap[name] = FAULT;

    return true;
}

bool GripperSupervisor::transToIdle(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() || (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end())
    {
        return false;
    }

    // add a command to park gripper
    if (stateIt->second != SET_PENDING)
    {
        if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
        {
            r2_msgs::JointControlData jcd;
            jcd.controlMode.state = r2_msgs::JointControlMode::PARK;
            jointModeCommands.joint.push_back(name);
            jointModeCommands.data.push_back(jcd);
            actionlib_msgs::GoalStatus gStat;
            gStat.goal_id.stamp = gripperCmds.header.stamp;
            gStat.goal_id.id    = name;
            gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
            gStat.text          = "Trans to idle- parking joint";
            goalStatusesOut.status_list.push_back(gStat);

            return false;
        }
        else
        {
            stateIt->second = IDLE;
        }
    }
    else
    {
        stateIt->second = IDLE;
    }

    return true;
}

bool GripperSupervisor::transToFailure(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() || (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end())
    {
        return false;
    }

    // add a command to park gripper
    if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
    {
        r2_msgs::JointControlData jcd;
        jcd.controlMode.state = r2_msgs::JointControlMode::PARK;
        jointModeCommands.joint.push_back(name);
        jointModeCommands.data.push_back(jcd);
        actionlib_msgs::GoalStatus gStat;
        gStat.goal_id.stamp = gripperCmds.header.stamp;
        gStat.goal_id.id    = name;
        gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
        gStat.text          = "Trans to failure- parking joint";
        goalStatusesOut.status_list.push_back(gStat);
    }

    stateIt->second = FAILURE;

    return true;
}

bool GripperSupervisor::transToLock(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() ||
            (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end() ||
            (reqIt = gripperDriveApprovedMap.find(name)) == gripperDriveApprovedMap.end())
    {
        return false;
    }

    // add a command to servo gripper
    if (intIt->second.state == r2_msgs::JointControlMode::FAULTED)
    {
        transToFault(name);
    }
    else if (intIt->second.state != r2_msgs::JointControlMode::DRIVE)
    {
        if (reqIt->second)
        {
            r2_msgs::JointControlData jcd;
            jcd.controlMode.state = r2_msgs::JointControlMode::DRIVE;
            jointModeCommands.joint.push_back(name);
            jointModeCommands.data.push_back(jcd);
            stateIt->second = LOCK;
        }
        else
        {
            // send request to engineer
            driveRequest.data.push_back(name);
            nextState = LOCK;
            stateIt->second = REQUEST_PENDING;
            pendingTimer = 0;
        }
    }
    else
    {
        stateIt->second = LOCK;
    }

    return true;
}

bool GripperSupervisor::transToRelease(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() ||
            (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end() ||
            (reqIt = gripperDriveApprovedMap.find(name)) == gripperDriveApprovedMap.end())
    {
        return false;
    }

    // add a command to servo gripper
    if (intIt->second.state == r2_msgs::JointControlMode::FAULTED)
    {
        transToFault(name);
    }
    else if (intIt->second.state != r2_msgs::JointControlMode::DRIVE)
    {
        if (reqIt->second)
        {
            r2_msgs::JointControlData jcd;
            jcd.controlMode.state = r2_msgs::JointControlMode::DRIVE;
            jointModeCommands.joint.push_back(name);
            jointModeCommands.data.push_back(jcd);
            stateIt->second = RELEASE;
        }
        else
        {
            // send request to engineer
            driveRequest.data.push_back(name);
            nextState = RELEASE;
            stateIt->second = REQUEST_PENDING;
            pendingTimer = 0;
        }
    }
    else
    {
        stateIt->second = RELEASE;
    }

    return true;
}

bool GripperSupervisor::transToSet(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() ||
            (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end() ||
            (reqIt = gripperDriveApprovedMap.find(name)) == gripperDriveApprovedMap.end())
    {
        return false;
    }

    // add a command to servo gripper
    if (intIt->second.state == r2_msgs::JointControlMode::FAULTED)
    {
        transToFault(name);
    }
    else if (intIt->second.state != r2_msgs::JointControlMode::DRIVE)
    {
        if (reqIt->second)
        {
            r2_msgs::JointControlData jcd;
            jcd.controlMode.state = r2_msgs::JointControlMode::DRIVE;
            jointModeCommands.joint.push_back(name);
            jointModeCommands.data.push_back(jcd);
            stateIt->second = SET;
        }
        else
        {
            // send request to engineer
            driveRequest.data.push_back(name);
            nextState = SET;
            stateIt->second = REQUEST_PENDING;
            pendingTimer = 0;
        }
    }
    else
    {
        stateIt->second = SET;
    }

    return true;
}

int GripperSupervisor::runStateMachine(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end() ||
            (intIt = gripperJointModeMap.find(name)) == gripperJointModeMap.end() ||
            (reqIt = gripperDriveApprovedMap.find(name)) == gripperDriveApprovedMap.end())
    {
        return -1;
    }

    switch (stateIt->second)
    {
        case IDLE:
        case FAULT:
        case FAILURE:
            break;

        case REQUEST_PENDING:
            pendingTimer++;

            if (reqIt->second)
            {
                r2_msgs::JointControlData jcd;
                jcd.controlMode.state = r2_msgs::JointControlMode::DRIVE;
                jointModeCommands.joint.push_back(name);
                jointModeCommands.data.push_back(jcd);
                stateIt->second = nextState;
            }
            else if (pendingTimer > timeLimit)
            {
                transToIdle(name);
                actionlib_msgs::GoalStatus gStat;
                gStat.goal_id.stamp = gripperCmds.header.stamp;
                gStat.goal_id.id    = name;
                gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                gStat.text          = "Request timed out- trans to idle";
                goalStatusesOut.status_list.push_back(gStat);
                pendingTimer = 0;
            }
            else
            {
                if (std::find(driveRequest.data.begin(), driveRequest.data.end(), name) == driveRequest.data.end())
                {
                    driveRequest.data.push_back(name);
                }
            }

            break;

        case RELEASE_PENDING:
        case SET_PENDING:
            pendingTimer++;

            if (pendingTimer > timeLimit)
            {
                if (!isLocked(name))
                {
                    transToIdle(name);
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                    gStat.text          = "Command timed out- trans to idle";
                    goalStatusesOut.status_list.push_back(gStat);
                }
                else
                {
                    transToFailure(name);
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::ABORTED;
                    gStat.text          = "Command timed out- trans to failure";
                    goalStatusesOut.status_list.push_back(gStat);
                }

                pendingTimer = 0;
            }

            if (intIt->second.state == r2_msgs::JointControlMode::PARK)
            {
                stateIt->second = IDLE;
            }

            break;

        case LOCK_PENDING:
            pendingTimer++;

            if (pendingTimer > timeLimit)
            {
                if (isLocked(name))
                {
                    transToIdle(name);
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::SUCCEEDED;
                    gStat.text          = "Lock timed out- trans to idle";
                    goalStatusesOut.status_list.push_back(gStat);
                }
                else
                {
                    transToFailure(name);
                    actionlib_msgs::GoalStatus gStat;
                    gStat.goal_id.stamp = gripperCmds.header.stamp;
                    gStat.goal_id.id    = name;
                    gStat.status        = actionlib_msgs::GoalStatus::ABORTED;
                    gStat.text          = "Lock timed out- trans to failure";
                    goalStatusesOut.status_list.push_back(gStat);
                }

                pendingTimer = 0;
            }

            if (intIt->second.state == r2_msgs::JointControlMode::PARK)
            {
                stateIt->second = IDLE;
            }

            break;

        case LOCK:
            if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
            {
                // send the lock command
                gripperCmdOut.name.push_back(name);
                gripperCmdOut.setpoint.push_back(setpoint);
                gripperCmdOut.command.push_back("c_lock");
                gripperCmdOut.dutyCycle.push_back(motcomLimit);

                actionlib_msgs::GoalStatus gStat;
                gStat.goal_id.stamp = gripperCmds.header.stamp;
                gStat.goal_id.id    = name;
                gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                gStat.text          = "Sending lock command";
                goalStatusesOut.status_list.push_back(gStat);

                stateIt->second = LOCK_PENDING;
                pendingTimer = 0;
            }

            break;

        case RELEASE:
            if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
            {
                // send the release command
                gripperCmdOut.name.push_back(name);
                gripperCmdOut.setpoint.push_back(setpoint);
                gripperCmdOut.command.push_back("c_release");
                gripperCmdOut.dutyCycle.push_back(motcomLimit);

                actionlib_msgs::GoalStatus gStat;
                gStat.goal_id.stamp = gripperCmds.header.stamp;
                gStat.goal_id.id    = name;
                gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                gStat.text          = "Sending release command";
                goalStatusesOut.status_list.push_back(gStat);

                stateIt->second = RELEASE_PENDING;
                pendingTimer = 0;
            }

            break;

        case SET:
            if (intIt->second.state == r2_msgs::JointControlMode::DRIVE)
            {
                // send the set command
                gripperCmdOut.name.push_back(name);
                gripperCmdOut.setpoint.push_back(setpoint);
                gripperCmdOut.command.push_back("c_set");
                gripperCmdOut.dutyCycle.push_back(motcomLimit);

                actionlib_msgs::GoalStatus gStat;
                gStat.goal_id.stamp = gripperCmds.header.stamp;
                gStat.goal_id.id    = name;
                gStat.status        = actionlib_msgs::GoalStatus::ACTIVE;
                gStat.text          = "Sending set command";
                goalStatusesOut.status_list.push_back(gStat);

                stateIt->second = SET_PENDING;
                pendingTimer = 0;
            }

            break;

        default:
            // unknown
            break;
    }

    return 0;
}

int GripperSupervisor::getState(const std::string& name)
{
    if ((stateIt = stateMap.find(name)) == stateMap.end())
    {
        return -1;
    }

    return stateIt->second;
}

void GripperSupervisor::setLimits(const int& highDuty, const int& time)
{
    hiDutyCycleLimit = highDuty;
    timeLimit = time;
}