JointTrajectoryManager.cpp

Go to the documentation of this file.

#include <robodyn_controllers/JointTrajectoryManager.h>
#include "nasa_common_logging/Logger.h"
/***************************************************************************/
JointTrajectoryManager::JointTrajectoryManager()
{

}

JointTrajectoryManager::~JointTrajectoryManager()
{

}
/***************************************************************************/
void JointTrajectoryManager::addToJointGoalMap(const std::vector<std::string>& jointNames, const actionlib_msgs::GoalID goalId)
{
    // Note: This method assumes that no joints in trajectory are associated
    //     with any existing goalId's in the jointGoalMap.  This must be checked before this
    //     method is called!

    for (std::vector<std::string>::const_iterator nameIt = jointNames.begin();
            nameIt != jointNames.end(); ++nameIt)
    {
        jointGoalMap.insert(std::make_pair(*nameIt, goalId));
    }
}
/***************************************************************************/
void JointTrajectoryManager::removeFromJointGoalMap(const std::vector<std::string>& jointNames)
{
    for (std::vector<std::string>::const_iterator nameIt = jointNames.begin(); nameIt != jointNames.end(); ++nameIt)
    {
        // check each name for conflict and remove
        std::map<std::string, actionlib_msgs::GoalID>::iterator goalIt = jointGoalMap.find(*nameIt);

        if (goalIt != jointGoalMap.end())
        {
            if (isActive(goalIt->second))
            {
                // currently active
                std::stringstream err;
                err << "JointTrajectoryManager::removeFromJointGoalMap() - trying to remove an active goal";
                NasaCommonLogging::Logger::log("gov.nasa.controllers.JointTrajectoryManager", log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
                return;
            }
            else
            {
                jointGoalMap.erase(goalIt);
            }
        }
    }
}
/***************************************************************************/
void JointTrajectoryManager::removeFromJointGoalMap(const actionlib_msgs::GoalID& goal)
{
    if (isActive(goal))
    {
        // currently active
        std::stringstream err;
        err << "JointTrajectoryManager::removeFromJointGoalMap() - trying to remove an active goal";
        NasaCommonLogging::Logger::log("gov.nasa.controllers.JointTrajectoryManager", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }
    else
    {
        for (std::map<std::string, actionlib_msgs::GoalID>::iterator it = jointGoalMap.begin(); it != jointGoalMap.end();)
        {
            if (it->second.id == goal.id && it->second.stamp == goal.stamp)
            {
                jointGoalMap.erase(it++);
            }
            else
            {
                ++it;
            }
        }
    }
}

/***************************************************************************/
std::string JointTrajectoryManager::jointGoalMapToString(void) const
{
    std::stringstream msg;
    std::map<std::string, actionlib_msgs::GoalID>::const_iterator jointGoalIt;
    msg << std::endl << "jointGoalMap" << std::endl;
    msg << "------------" << std::endl;

    for (jointGoalIt = jointGoalMap.begin(); jointGoalIt != jointGoalMap.end(); ++jointGoalIt)
    {
        msg << jointGoalIt->first << ": " << jointGoalIt->second.stamp << ", " << jointGoalIt->second.id << std::endl;
    }

    return msg.str();
}