GoalStatusSender.cpp

Go to the documentation of this file.

#include "robodyn_utilities/GoalStatusSender.h"
/***************************************************************************/
GoalStatusSender::GoalStatusSender(const std::string& name)
{
    goalStatusMsg.header.frame_id = name;
}

GoalStatusSender::~GoalStatusSender()
{
}
/***************************************************************************/
void GoalStatusSender::sendStatusMessage(bool clear)
{
    if (!goalStatusMsg.status_list.empty())
    {
        goalStatusMsg.header.stamp = ros::Time::now();
        writeStatus(goalStatusMsg);

        if (clear)
        {
            clearStatusMessage();
        }
    }
}
/***************************************************************************/
void GoalStatusSender::sendStatusMessage(const actionlib_msgs::GoalStatus& status, bool clear)
{
    addStatus(status);
    sendStatusMessage(clear);
}
/***************************************************************************/
void GoalStatusSender::sendStatusMessage(const ros::Time& stamp, const std::string& id,
        const actionlib_msgs::GoalStatus::_status_type& status,
        const std::string& text, bool clear)
{
    addStatus(stamp, id, status, text);
    sendStatusMessage(clear);
}
/***************************************************************************/
void GoalStatusSender::sendStatusMessage(const actionlib_msgs::GoalStatus::_status_type& status,
        const std::string& text, bool clear)
{
    addStatus(ros::Time::now(), goalStatusMsg.header.frame_id, status, text);
    sendStatusMessage(clear);
}
/***************************************************************************/
void GoalStatusSender::clearStatusMessage()
{
    goalStatusMsg.status_list.clear();
}
/***************************************************************************/
unsigned int GoalStatusSender::addStatus(const actionlib_msgs::GoalStatus& status)
{
    goalStatusMsg.status_list.push_back(status);
    return goalStatusMsg.status_list.size() - 1;
}
/***************************************************************************/
unsigned int GoalStatusSender::addStatus(const ros::Time& stamp, const std::string& id,
        const actionlib_msgs::GoalStatus::_status_type& status,
        const std::string& text)
{
    actionlib_msgs::GoalStatus gStatus;
    gStatus.goal_id.stamp = stamp;
    gStatus.goal_id.id    = id;
    gStatus.status        = status;
    gStatus.text          = text;
    return addStatus(gStatus);
}
/***************************************************************************/
actionlib_msgs::GoalStatus& GoalStatusSender::status(unsigned int index)
{
    if (index < goalStatusMsg.status_list.size())
    {
        return goalStatusMsg.status_list[index];
    }
    else
    {
        std::stringstream ss;
        ss << "Trying to access status " << index << " that is out of range " << goalStatusMsg.status_list.size();
        throw std::out_of_range(ss.str());
    }
}
/***************************************************************************/
const actionlib_msgs::GoalStatus& GoalStatusSender::status(unsigned int index) const
{
    if (index < goalStatusMsg.status_list.size())
    {
        return goalStatusMsg.status_list[index];
    }
    else
    {
        std::stringstream ss;
        ss << "Trying to access status " << index << " that is out of range " << goalStatusMsg.status_list.size();
        throw std::out_of_range(ss.str());
    }
}
/***************************************************************************/
void GoalStatusSender::removeStatus(unsigned int index)
{
    if (index < goalStatusMsg.status_list.size())
    {
        goalStatusMsg.status_list.erase(goalStatusMsg.status_list.begin() + index);
    }
    else
    {
        std::stringstream ss;
        ss << "Trying to remove status " << index << " that is out of range " << goalStatusMsg.status_list.size();
        throw std::out_of_range(ss.str());
    }
}