rtt_simple_action_server.h

Go to the documentation of this file.

#ifndef  RTT_SIMPLE_ACTION_SERVER_HPP
#define  RTT_SIMPLE_ACTION_SERVER_HPP

#include <stdexcept>
#include <boost/bind.hpp>
#include <boost/function.hpp>

#include <actionlib/server/action_server.h>
#include <actionlib/action_definition.h>
#include <rtt_actionlib/rtt_action_server.h>

namespace rtt_actionlib {

template <class ActionSpec> class RTTSimpleActionServer 
{
        protected:
                // GoalHandle
                typedef actionlib::ServerGoalHandle<ActionSpec> GoalHandle;
                // Goal, Feedback, Result typedefs
                ACTION_DEFINITION(ActionSpec);  

        protected:
                RTTActionServer<ActionSpec> action_server;
                GoalHandle goal_active;
                GoalHandle goal_pending;

                // callbacks
                boost::function<void(const Goal&)> newGoalHook;
                boost::function<void()> cancelGoalHook;

        protected:

                void goalCallback(GoalHandle gh);
                void cancelCallback(GoalHandle gh);

        public:

                RTTSimpleActionServer(boost::shared_ptr<RTT::Service> owner_service) 
                {       
                        if (!owner_service) throw std::invalid_argument("RTTSimpleActionServer: owner pointer must be valid.");
                        action_server.addPorts(owner_service);
                        action_server.registerGoalCallback(boost::bind(&RTTSimpleActionServer<ActionSpec>::goalCallback, this, _1));
                        action_server.registerCancelCallback(boost::bind(&RTTSimpleActionServer<ActionSpec>::cancelCallback, this, _1));
                }

                bool ready() { 
                        return action_server.ready(); 
                }


                bool start(bool publish_feedback = false);

                void shutdown() {
                        action_server.shutdown();
                }

                bool isActive() const {
                        return goal_active.isValid() && goal_active.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE;
                }

                bool isPreempting() const {
                        return goal_active.isValid() && goal_active.getGoalStatus().status == actionlib_msgs::GoalStatus::PREEMPTING;
                }

                bool isPending() const {
                        return goal_pending.isValid() && goal_pending.getGoalStatus().status == actionlib_msgs::GoalStatus::PENDING;
                }

                boost::shared_ptr<const Goal> getActiveGoal() const {
                        if (isActive()) return goal_active.getGoal();
                        else return boost::shared_ptr<const Goal>(); // return NULL
                        // else nullptr;
                }

                boost::shared_ptr<const Goal> getPendingGoal() const {
                        if (isPending()) return goal_pending.getGoal();
                        else return boost::shared_ptr<const Goal>(); // return NULL
                        // else nullptr;
                }

                bool acceptPending(const Result& result, const std::string& msg = "");

                bool rejectPending(const Result& result, const std::string& msg = "");

                bool publishFeedback(const Feedback& feedpack);

                bool abortActive(const Result& result, const std::string& msg = "");

                bool cancelActive(const Result& result, const std::string& msg = "");

                bool succeedActive(const Result& result, const std::string& msg = "");

                void setGoalHook(boost::function< void(const Goal&)> _newGoalHook) {
                        newGoalHook = _newGoalHook;
                }

                void setCancelHook(boost::function< void()> _cancelGoalHook) {
                        cancelGoalHook = _cancelGoalHook;
                }
};

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::start(bool publish_feedback) 
{
        if (ready()) {
                action_server.start();
                if (publish_feedback) action_server.initialize();
                return true;
        }
        else return false;
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::acceptPending(const Result& result, const std::string& msg)
{
        if (isPending()) {
                if (goal_active.isValid()) {
                        switch (goal_active.getGoalStatus().status) {
                                case actionlib_msgs::GoalStatus::ACTIVE:
                                case actionlib_msgs::GoalStatus::PREEMPTING:
                                        goal_active.setAborted(result, msg);
                                        break;
                        }
                }
                goal_pending.setAccepted();
                goal_active = goal_pending;
                return isActive();
        }
        else {
                // if pending is in RECALLING cancelHook perform necessary actions
                return false;
        }
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::rejectPending(const Result& result, const std::string& msg) 
{
        if (!goal_pending.isValid())  return false;
        switch (goal_pending.getGoalStatus().status) {
                case actionlib_msgs::GoalStatus::PENDING:
                case actionlib_msgs::GoalStatus::RECALLING:
                        goal_pending.setRejected(result, msg);
                        return true;
        }
        return false;
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::publishFeedback(const Feedback& feedpack)
{
        if (!isActive()) return false;
        goal_active.publishFeedback(feedpack);
        return true;
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::abortActive(const Result& result, const std::string& msg)
{
        if (!goal_active.isValid())  return false;
        switch (goal_active.getGoalStatus().status) {
                case actionlib_msgs::GoalStatus::ACTIVE:
                case actionlib_msgs::GoalStatus::PREEMPTING:
                        goal_active.setAborted(result, msg);
                        return true;
        }
        return false;
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::cancelActive(const Result& result, const std::string& msg)
{
        if (!goal_active.isValid()) return false;
        switch (goal_active.getGoalStatus().status) {
                case actionlib_msgs::GoalStatus::ACTIVE:
                case actionlib_msgs::GoalStatus::PREEMPTING:
                        goal_active.setCanceled(result, msg);
                        return true;
        }
        return true;
}

template <class ActionSpec> bool RTTSimpleActionServer<ActionSpec>::succeedActive(const Result& result, const std::string& msg)
{
        if (!goal_active.isValid()) return false;
        switch (goal_active.getGoalStatus().status) {
                case actionlib_msgs::GoalStatus::ACTIVE:
                case actionlib_msgs::GoalStatus::PREEMPTING:
                        goal_active.setSucceeded(result, msg);
                        return true;
        }
        return false;
}

template <class ActionSpec> void RTTSimpleActionServer<ActionSpec>::goalCallback(GoalHandle gh) 
{
        if (goal_pending.isValid()) {
                switch (goal_pending.getGoalStatus().status) {
                        case actionlib_msgs::GoalStatus::PENDING:
                        case actionlib_msgs::GoalStatus::RECALLING:
                                goal_pending.setRejected(Result(), "Pending goal is replaced by new received goal.");
                                break;
                }
        }
        goal_pending = gh;
        if (isPending() && newGoalHook) {
                newGoalHook(*goal_pending.getGoal());
        }
}

template <class ActionSpec> void RTTSimpleActionServer<ActionSpec>::cancelCallback(GoalHandle gh) {
        if (!gh.isValid()) return;
        switch (gh.getGoalStatus().status) {
                case actionlib_msgs::GoalStatus::ACTIVE:
                case actionlib_msgs::GoalStatus::PREEMPTING:
                case actionlib_msgs::GoalStatus::PENDING:
                case actionlib_msgs::GoalStatus::RECALLING:
                        if (goal_active == gh) {
                                if (cancelGoalHook) cancelGoalHook();
                                // cancelActive(Result(), "Active goal is canceled by client request.");
                        }
                        else if (goal_pending == gh) {
                                gh.setCanceled(Result(), "Pending goal is canceled by client request.");
                        }
                        else {
                                gh.setCanceled(Result(), "Unknown goal.");
                        }
                        break;
        }
}

} // namespace rtt_actionlib

#endif  /*RTT_SIMPLE_ACTION_SERVER_HPP*/