BaseGripperConverter.cpp

Go to the documentation of this file.

#include "robodyn_utilities/BaseGripperConverter.h"

BaseGripperConverter::BaseGripperConverter()
{
}

BaseGripperConverter::~BaseGripperConverter()
{
}

std::string BaseGripperConverter::BaseFrameToGripper(const std::string& bfName)
{
    std::string name = bfName;
    std::size_t pos  = name.find("tip", name.size() - 3);

    if (pos != std::string::npos)
    {
        name.erase(pos, std::string::npos);
        name.append("joint0");
    }

    return name;
}

std::string BaseGripperConverter::GripperToBaseFrame(const std::string& gripper)
{
    std::string name = gripper;
    std::size_t pos  = name.find("joint0", name.size() - 6);

    if (pos != std::string::npos)
    {
        name.erase(pos, std::string::npos);
        name.append("tip");
    }

    return name;
}

std::string BaseGripperConverter::FindOpposite(const std::string& name)
{
    std::string opposite = name;
    std::string side     = "left";
    std::size_t pos      = opposite.find(side);

    if (pos != std::string::npos)
    {
        opposite.replace(pos, side.size(), "right");
    }
    else
    {
        side = "right";
        pos  = opposite.find(side);

        if (pos != std::string::npos)
        {
            opposite.replace(pos, side.size(), "left");
        }
        else
        {
            // if bad input, send back an empty string
            opposite.clear();
        }
    }

    return opposite;
}

std::string BaseGripperConverter::FindSide(const std::string& input)
{
    std::size_t posLower, posUpper;
    posLower = input.find("left");
    posUpper = input.find("Left");

    if (posLower != std::string::npos || posUpper != std::string::npos)
    {
        return "left";
    }
    else
    {
        posLower = input.find("right");
        posUpper = input.find("Right");

        if (posLower != std::string::npos || posUpper != std::string::npos)
        {
            return "right";
        }
        else
        {
            return input;
        }
    }
}