BaseFrameDetector.cpp

Go to the documentation of this file.

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

BaseFrameDetector::BaseFrameDetector()
{
}

BaseFrameDetector::~BaseFrameDetector()
{
}

int BaseFrameDetector::PopulateEEFStateInfo(const r2_msgs::GripperPositionState& eefState)
{
    unsigned int neef = eefState.name.size();

    if (neef != eefState.locked.size() || neef != eefState.loaded.size())
    {
        return -1;
    }

    // Iterate through all end effectors in message
    for (unsigned int i = 0; i < neef; i++)
    {
        endEffectorMap[eefState.name[i]].locked = eefState.locked[i];
        endEffectorMap[eefState.name[i]].loaded = eefState.loaded[i];
    }

    return 0;

}

int BaseFrameDetector::PopulateBaseVerifInfo(const r2_msgs::GripperPositionState& baseVerif)
{
    unsigned int neef = baseVerif.name.size();

    if (neef != baseVerif.verified.size())
    {
        return -1;
    }

    // Iterate through all end effectors in message
    for (unsigned int i = 0; i < neef; i++)
    {
        endEffectorMap[baseVerif.name[i]].verified = baseVerif.verified[i];
    }

    return 0;

}

void BaseFrameDetector::PopulateBaseFrameMsg(const std::string& baseFrameCmd, r2_msgs::StringArray& baseList)
{
    // time stamp message
    baseList.header.stamp = ros::Time::now();
    bfList.clear();

    // iterate through map, find potential base frames
    std::map<std::string, EndEffectorData>::const_iterator it = endEffectorMap.begin();

    while (it != endEffectorMap.end())
    {
        // get proper base frame name
        std::string bfName = GripperToBaseFrame(it->first);

        // check and find- don't need loaded!!
        if (it->second.locked == 1 && it->second.verified == 1 && bfName != it->first)
        {
            bfList.push_back(bfName);
        }

        it++;
    }

    // Allows r2/robot_world to be base frame
    if (!baseFrameCmd.empty())
    {
        if (bfList.empty() && endEffectorMap.find(BaseFrameToGripper(baseFrameCmd)) == endEffectorMap.end())
        {
            // signifies that default is not being used
            if (baseList.data.empty())
            {
                baseList.data.push_back(baseFrameCmd);
            }
        }
    }

    // create message, starting with id (cmd) base frame
    std::vector<std::string>::iterator itv = std::find(bfList.begin(), bfList.end(), baseFrameCmd);

    if (itv != bfList.end())
    {
        baseList.data.push_back(baseFrameCmd);
        bfList.erase(itv);
    }


    // put the rest of frames in list
    for (unsigned int i = 0; i < bfList.size(); i++)
    {
        baseList.data.push_back(bfList[i]);
    }

    return;
}