ModeArbiter.cpp

Go to the documentation of this file.

#include "robodyn_controllers/ModeArbiter.h"
#include <iostream>

ModeArbiter::ModeArbiter()
{
}

ModeArbiter::~ModeArbiter()
{
}

void ModeArbiter::InitializeMaps(std::vector<std::string> mechanisms, std::vector<std::string> exJoints)
{
    // Initialize zeros in the right ways
    std::string user = "user";
    std::string mode = "IK";
    std::string jntName;

    // Grab tree segment map and iterator
    KDL::SegmentMap                 segments = tree.getSegments();
    KDL::SegmentMap::const_iterator it       = segments.begin();

    // Iterate through map
    while (it != segments.end())
    {

        if (it->second.segment.getJoint().getType() != KDL::Joint::None)
        {
            jntName                = it->second.segment.getJoint().getName();
            supervisorMap[jntName] = user;
            incumbentMap[jntName]  = user;
            modeMap[jntName]       = mode;
        }

        // Next element, please
        it++;
    }

    // add mechanisms
    for (unsigned int i = 0; i < mechanisms.size(); i++)
    {
        supervisorMap[mechanisms[i]] = user;
        incumbentMap[mechanisms[i]]  = user;
        modeMap[mechanisms[i]]       = mode;
    }

    // add the joints in the property file
    for (unsigned int i = 0; i < exJoints.size(); i++)
    {
        supervisorMap[exJoints[i]] = user;
        incumbentMap[exJoints[i]]  = user;
        modeMap[exJoints[i]]       = mode;
    }

    grippers = exJoints;

    return;

}

void ModeArbiter::ClearMaps()
{
    supervisorMap.clear();
    incumbentMap.clear();
    modeMap.clear();
}

bool ModeArbiter::GetMapElement(const std::string& map, const std::string& element, std::string& value)
{
    bool ret = false;
    std::map<std::string, std::string>::const_iterator it;

    if (map == "supervisor")
    {
        if ((it = supervisorMap.find(element)) != supervisorMap.end())
        {
            ret   = true;
            value = it->second;
        }
    }
    else if (map == "incumbent")
    {
        if ((it = incumbentMap.find(element)) != incumbentMap.end())
        {
            ret   = true;
            value = it->second;
        }
    }
    else
    {
        if ((it = modeMap.find(element)) != modeMap.end())
        {
            ret   = true;
            value = it->second;
        }
    }

    return ret;
}

void ModeArbiter::AssignMaster(const std::string& name)
{
    master = name;
}

void ModeArbiter::GetMaster(std::string& name)
{
    name = master;
}

bool ModeArbiter::isValidHijack(const std::string& name)
{
    std::map<std::string, std::string>::iterator it;

    for (it = supervisorMap.begin(); it != supervisorMap.end(); ++it)
    {
        if (it->second == name)
        {
            return true;
        }
    }

    return false;
}

int ModeArbiter::AssignArbitrationProperties(const r2_msgs::Modes& modeMsg)
{
    int ret = 0;

    if (isValidCommand(modeMsg))
    {
        if (modeMsg.supervisor.size() > 0)
        {
            ret += AssignSupervisors(modeMsg);
        }

        if (modeMsg.incumbent.size() > 0)
        {
            ret += AssignIncumbents(modeMsg);
        }

        if (modeMsg.mode.size() > 0)
        {
            ret += AssignModes(modeMsg);
        }

        if (!modeMsg.master.empty())
        {
            AssignMaster(modeMsg.master);
        }
    }

    return ret;
}

int ModeArbiter::AssignSupervisors(const r2_msgs::Modes& modeMsg)
{
    int ret = 1;
    std::map<std::string, std::string>::iterator it;

    if (modeMsg.joint_names.size() == 0)
    {
        // change all joints to new super
        for (it = supervisorMap.begin(); it != supervisorMap.end(); it++)
        {
            it->second = modeMsg.supervisor[0];
        }
    }
    else
    {
        for (unsigned int i = 0; i < modeMsg.joint_names.size(); i++)
        {
            if ((it = supervisorMap.find(modeMsg.joint_names[i])) != supervisorMap.end())
            {
                it->second = modeMsg.supervisor[i];
            }
        }
    }

    return ret;
}

int ModeArbiter::AssignIncumbents(const r2_msgs::Modes& modeMsg)
{
    int ret = 1;
    std::map<std::string, std::string>::iterator it;

    if (modeMsg.joint_names.size() == 0)
    {
        // change all joints to new super
        for (it = incumbentMap.begin(); it != incumbentMap.end(); it++ )
        {
            it->second = modeMsg.incumbent[0];

        }
    }
    else
    {
        for (unsigned int i = 0; i < modeMsg.joint_names.size(); i++)
        {
            if ((it = incumbentMap.find(modeMsg.joint_names[i])) != incumbentMap.end())
            {
                it->second = modeMsg.incumbent[i];
            }
        }
    }

    return ret;
}

int ModeArbiter::AssignModes(const r2_msgs::Modes& modeMsg)
{
    int ret = 1;
    std::map<std::string, std::string>::iterator it;

    if (modeMsg.joint_names.size() == 0)
    {
        // change all joints to new super
        for (it = modeMap.begin(); it != modeMap.end(); it++)
        {
            it->second = modeMsg.mode[0];
        }
    }
    else
    {
        for (unsigned int i = 0; i < modeMsg.joint_names.size(); i++)
        {
            if ((it = modeMap.find(modeMsg.joint_names[i])) != modeMap.end())
            {
                it->second = modeMsg.mode[i];
            }
        }
    }

    return ret;
}

bool ModeArbiter::isValidCommand(const std::string& originator, const std::vector<std::string>& jointNames)
{
    bool ret = true;
    std::map<std::string, std::string>::const_iterator it;

    if (jointNames.size() == 0)
    {
        // search over entire map
        it = supervisorMap.begin();

        while (it != supervisorMap.end())
        {
            if (!(it->second == originator || originator == master))
            {
                ret = false;
                break;
            }

            it++;
        }
    }
    else
    {
        // search over joints in the message only
        for (unsigned int i = 0; i < jointNames.size(); i++)
        {
            if ((it = supervisorMap.find(jointNames[i])) != supervisorMap.end())
            {
                if (!(it->second == originator || originator == master))
                {
                    ret = false;
                    break;
                }
            }
        }
    }

    return ret;
}

bool ModeArbiter::isValidCommand(const r2_msgs::Modes& modeMsg)
{
    return isValidCommand(modeMsg.originator, modeMsg.joint_names);
}

// May want to take master out of individual commands
bool ModeArbiter::isValidCommand(const r2_msgs::LabeledGains& gains)
{
    return isValidCommand(gains.originator, gains.joint_names);
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledJointState& js)
{
    return isValidCommand(js.originator, js.name);
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledPoseTrajectory& pt)
{
    bool ret = true;

    std::map<std::string, std::string>::const_iterator it = supervisorMap.begin();

    while (it != supervisorMap.end())
    {
        if (!(it->second == pt.originator || pt.originator == master))
        {
            ret = false;
            break;
        }

        it++;

    }

    return ret;
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledControllerPoseSettings& ps)
{
    bool ret = true;

    std::map<std::string, std::string>::const_iterator it = supervisorMap.begin();

    while (it != supervisorMap.end())
    {
        if (!(it->second == ps.originator || ps.originator == master))
        {
            ret = false;
            break;
        }

        it++;

    }

    return ret;
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledGripperPositionCommand& gp)
{
    bool ret = true;
    std::map<std::string, std::string>::const_iterator it;

    for (unsigned int i = 0; i < gp.name.size(); i++)
    {
        if ((it = supervisorMap.find(gp.name[i])) != supervisorMap.end())
        {
            if (!(it->second == gp.originator || gp.originator == master))
            {
                ret = false;
                break;
            }
        }
        else
        {
            ret = false;
            break;
        }

    }

    return ret;
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledJointTrajectory& jt)
{
    return isValidCommand(jt.originator, jt.joint_names);
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledControllerJointSettings& js)
{
    return isValidCommand(js.originator, js.joint_names);
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledTrajectoryMonitorFactors& tmf)
{
    return isValidCommand(tmf.originator, tmf.joint_names);
}

bool ModeArbiter::isValidCommand(const r2_msgs::LabeledJointControlDataArray& jc)
{
    bool ret = true;
    std::map<std::string, std::string>::const_iterator it;

    for (unsigned int i = 0; i < jc.joint.size(); i++)
    {
        if ((it = supervisorMap.find(jc.joint[i])) != supervisorMap.end())
        {
            if (!(it->second  == jc.originator || jc.originator == master))
            {
                ret = false;
                break;
            }
        }
        else
        {
            ret = false;
            break;
        }

    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledGains& gainsIn, r2_msgs::Gains& gainsOut)
{
    bool ret = false;

    if (isValidCommand(gainsIn))
    {
        ret                   = true;
        gainsOut.joint_names  = gainsIn.joint_names;
        gainsOut.naturalFreq  = gainsIn.naturalFreq;
        gainsOut.dampingRatio = gainsIn.dampingRatio;
        gainsOut.K            = gainsIn.K;
        gainsOut.D            = gainsIn.D;
        gainsOut.I            = gainsIn.I;
        gainsOut.windupLimit  = gainsIn.windupLimit;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledJointState& jsIn, sensor_msgs::JointState& jsOut)
{
    bool ret = false;

    if (isValidCommand(jsIn))
    {
        ret            = true;
        jsOut.header   = jsIn.header;
        jsOut.name     = jsIn.name;
        jsOut.position = jsIn.position;
        jsOut.velocity = jsIn.velocity;
        jsOut.effort   = jsIn.effort;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledPoseTrajectory& ptIn, r2_msgs::PoseTrajectory& ptOut)
{
    bool ret = false;

    if (isValidCommand(ptIn))
    {
        ret                   = true;
        ptOut.header          = ptIn.header;
        ptOut.nodes           = ptIn.nodes;
        ptOut.node_priorities = ptIn.node_priorities;
        ptOut.refFrames       = ptIn.refFrames;
        ptOut.points          = ptIn.points;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledControllerPoseSettings& psIn, r2_msgs::ControllerPoseSettings& psOut)
{
    bool ret = false;

    if (isValidCommand(psIn))
    {
        ret                             = true;
        psOut.maxLinearVelocity         = psIn.maxLinearVelocity;
        psOut.maxRotationalVelocity     = psIn.maxRotationalVelocity;
        psOut.maxLinearAcceleration     = psIn.maxLinearAcceleration;
        psOut.maxRotationalAcceleration = psIn.maxRotationalAcceleration;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledJointTrajectory& jtIn, trajectory_msgs::JointTrajectory& jtOut)
{
    bool ret = false;

    if (isValidCommand(jtIn))
    {
        ret               = true;
        jtOut.header      = jtIn.header;
        jtOut.joint_names = jtIn.joint_names;
        jtOut.points      = jtIn.points;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledControllerJointSettings& jsIn, r2_msgs::ControllerJointSettings& jsOut)
{
    bool ret = false;

    if (isValidCommand(jsIn))
    {
        ret                           = true;
        jsOut.joint_names             = jsIn.joint_names;
        jsOut.jointVelocityLimits     = jsIn.jointVelocityLimits;
        jsOut.jointAccelerationLimits = jsIn.jointAccelerationLimits;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledGripperPositionCommand& gpIn, r2_msgs::GripperPositionCommand& gpOut)
{
    bool ret = false;

    if (isValidCommand(gpIn))
    {
        ret             = true;
        gpOut.name      = gpIn.name;
        gpOut.setpoint  = gpIn.setpoint;
        gpOut.command   = gpIn.command;
        gpOut.dutyCycle = gpIn.dutyCycle;
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledJointControlDataArray& jcIn, r2_msgs::JointControlDataArray& jcOut)
{
    bool ret = false;
    jcOut.joint.clear();
    jcOut.data.clear();

    if (isValidCommand(jcIn))
    {
        ret = true;

        for (unsigned int i = 0; i < jcIn.joint.size(); i++)
        {
            // expecting a small grippers vector (2)- otherwise, should flip the search around
            if (std::find(grippers.begin(), grippers.end(), jcIn.joint[i]) == grippers.end())
            {
                jcOut.joint.push_back(jcIn.joint[i]);
                jcOut.data.push_back(jcIn.data[i]);
            }
            else
            {
                r2_msgs::JointControlData data = jcIn.data[i];
                data.controlMode.state = r2_msgs::JointControlMode::IGNORE;
                data.commandMode.state = r2_msgs::JointControlCommandMode::IGNORE;
                data.calibrationMode.state = r2_msgs::JointControlCalibrationMode::IGNORE;
                jcOut.joint.push_back(jcIn.joint[i]);
                jcOut.data.push_back(data);
            }
        }
    }

    return ret;
}

bool ModeArbiter::CreateCommandMessage(const r2_msgs::LabeledTrajectoryMonitorFactors& tmfIn, r2_msgs::TrajectoryMonitorFactors& tmfOut)
{
    bool ret = false;

    if (isValidCommand(tmfIn))
    {
        ret = true;
        tmfOut.joint_names = tmfIn.joint_names;
        tmfOut.distanceFactors = tmfIn.distanceFactors;
        tmfOut.torqueLimitFactors = tmfIn.torqueLimitFactors;
    }

    return ret;
}

void ModeArbiter::CreateTriggerMessages(std::vector<r2_msgs::Modes>& triggerMsgs)
{
    r2_msgs::Modes tempMsg;
    std::list<std::string>     supervisors;

    triggerMsgs.clear();

    std::map<std::string, std::string>::const_iterator it = supervisorMap.begin();

    while (it != supervisorMap.end())
    {
        supervisors.push_back(it->second);
        it++;
    }

    supervisors.unique();

    std::list<std::string>::iterator itl = supervisors.begin();

    for (; itl != supervisors.end(); ++itl)
    {
        tempMsg.header.stamp = ros::Time::now();
        tempMsg.joint_names.clear();
        tempMsg.supervisor.clear();
        tempMsg.incumbent.clear();

        std::string super = *itl;
        tempMsg.supervisor.push_back(super);

        std::map<std::string, std::string>::const_iterator its = supervisorMap.begin();

        while (its != supervisorMap.end())
        {
            if (its->second == super)
            {
                tempMsg.joint_names.push_back(its->first);
            }

            its++;
        }

        // hack for only one incumbent per super
        if (tempMsg.joint_names.size() > 0)
        {
            tempMsg.incumbent.push_back(incumbentMap[tempMsg.joint_names[0]]);
        }

        triggerMsgs.push_back(tempMsg);
    }
}