RosMsgTreeUtilities.cpp

Go to the documentation of this file.

#include "robodyn_controllers/RosMsgTreeUtilities.h"

RosMsgTreeUtilities::RosMsgTreeUtilities()
{
    bases.clear();
    jointNames.clear();
}

//RosMsgTreeUtilities::~RosMsgTreeUtilities()
//{
//}

void RosMsgTreeUtilities::loadFromFile(const std::string& fileName)
{
    KdlTreeParser::loadFromFile(fileName);
    bases.clear();
    bases.push_back(getBaseName());
    jointNames.clear();
    getJointNames(jointNames);
}

bool RosMsgTreeUtilities::setBaseFrame(const r2_msgs::StringArray& baseMsg)
{
    int n = baseMsg.data.size();

    if (n > 0 && baseMsg.data != bases)
    {
        bases.clear();

        for (unsigned int i = 0; i < baseMsg.data.size(); ++i)
        {
            if (tree.getSegment(baseMsg.data[i]) == tree.getSegments().end())
            {
                std::stringstream err;
                err << "Unable to set base! Couldn't find segment " << baseMsg.data[i] << " in tree";
                NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.setBaseFrame", log4cpp::Priority::ERROR, err.str());
                return false;
            }

            bases.push_back(baseMsg.data[i]);
        }
    }
    else
    {
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.setBaseFrame", log4cpp::Priority::INFO, "call to setBaseFrame but bases are 0 or haven't changed");
    }

    return true;
}

bool RosMsgTreeUtilities::setBaseFrame(std::string& baseMsg)
{
    if (tree.getSegment(baseMsg) == tree.getSegments().end())
    {
        std::stringstream err;
        err << "Unable to set base! Couldn't find segment " << baseMsg << " in tree";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.setBaseFrame", log4cpp::Priority::ERROR, err.str());
        return false;
    }

    bases.clear();
    bases.push_back(baseMsg);
    return true;
}

void RosMsgTreeUtilities::getFrames(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::Frame>& frames)
{
    KDL::Frame  frame;
    std::string tipName;

    for (unsigned int i = 0; i < poseState.name.size(); ++i)
    {
        tipName = poseState.name[i];

        try
        {
            RosMsgConverter::PoseStateToFrame(poseState, base, tipName, frame );
            frames[tipName] = frame;
        }
        catch (std::exception)
        {
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.getFrames", log4cpp::Priority::WARN, "Squelched error.. continuing..");
        }
    }
}

void RosMsgTreeUtilities::getFrameVels(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::FrameVel>& framevels)
{
    NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgTreeUtilities") << log4cpp::Priority::DEBUG << "entered getFrameVels";
    KDL::FrameVel frameVel;
    std::string   tipName;

    if (framevels.size() != poseState.name.size())
    {
        NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgTreeUtilities") << log4cpp::Priority::DEBUG << "resizing getFrameVels";
        framevels.clear();

        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            framevels.insert(std::make_pair(poseState.name[i], KDL::FrameVel()));
        }
    }

    for (unsigned int i = 0; i < poseState.name.size(); ++i)
    {
        NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgTreeUtilities") << log4cpp::Priority::DEBUG << "entered getFrameVels";
        tipName = poseState.name[i];

        try
        {
            RosMsgConverter::PoseStateToFrameVel(poseState, base, tipName, frameVel );
            framevels[tipName] = frameVel;
        }
        catch (std::exception)
        {
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.getFrameVels", log4cpp::Priority::WARN, "Squelched error in getFrameVels.. continuing..");
        }
    }
}

void RosMsgTreeUtilities::getFrameAccs(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::FrameAcc>& frameaccs)
{
    KDL::FrameAcc frameAcc;
    std::string   tipName;

    for (unsigned int i = 0; i < poseState.name.size(); ++i)
    {
        tipName = poseState.name[i];

        try
        {
            RosMsgConverter::PoseStateToFrameAcc(poseState, base, tipName, frameAcc );
            frameaccs[tipName] = frameAcc;
        }
        catch (std::exception)
        {
            NasaCommonLogging::Logger::log("gov.nasa.controllers.RosMsgTreeUtilities.getFrameAccs", log4cpp::Priority::WARN, "Squelched error.. continuing..");
        }
    }
}