iri_action_server.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author Joan Perez & Sergi Hernandez
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _iri_action_server_h_
#define _iri_action_server_h_

// ros action server
#include <actionlib/server/simple_action_server.h>
#include <actionlib/action_definition.h>

template <class ActionSpec>
class IriActionServer
{
  public:
    ACTION_DEFINITION(ActionSpec);

    typedef boost::shared_ptr<Result> ResultPtr;

    typedef boost::shared_ptr<Feedback> FeedbackPtr;
    
  private:
  
    boost::function<void (const GoalConstPtr&)> start_action_callback_;

    boost::function<void ()> stop_action_callback_;

    boost::function<bool ()> is_finished_callback_;

    boost::function<bool ()> has_succeed_callback_;
    
    boost::function<void (ResultPtr&)> get_result_callback_;
    
    boost::function<void (FeedbackPtr&)> get_feedback_callback_;
    
    void executeCallback(const GoalConstPtr& goal);

    std::string action_name_;
    
    actionlib::SimpleActionServer<ActionSpec> as_;

    ros::Rate loop_rate_;

    bool is_started;

  public:
    IriActionServer(ros::NodeHandle & nh, const std::string & action_name);

    ~IriActionServer(void);
    
    void start(void);

    bool isStarted(void) const;
   
    void shutdown(void);

    void setAborted(const Result& result = Result(), const std::string& text = std::string(""));

    void setPreempted(const Result& result = Result(), const std::string& text = std::string(""));

    bool isActive(void);

    void setLoopRate(ros::Rate r);

    void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb);
    
    void registerStopCallback(boost::function<void ()> cb);
    
    void registerIsFinishedCallback( boost::function<bool ()> cb);
    
    void registerHasSucceedCallback( boost::function<bool ()> cb);
    
    void registerGetResultCallback(boost::function<void (ResultPtr&)> cb);
    
    void registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb);
    
};

template <class ActionSpec>
IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::string & aname) :
  action_name_(aname),
  as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), false),
  loop_rate_(10),
  is_started(false)
{
  ROS_DEBUG("IriActionServer::Constructor");
}

template <class ActionSpec>
IriActionServer<ActionSpec>::~IriActionServer(void)
{
//   std::cout << "IriActionServer::Destructor:: active=" << as_.isActive() << std::endl;
//   if( as_.isActive() )
//     as_.setAborted();
//   as_.shutdown();
  ROS_DEBUG("IriActionServer::Destructor");
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::setLoopRate(ros::Rate r)
{
  loop_rate_ = r;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::start(void)
{
  ROS_DEBUG("IriActionServer::Start");

  // check if all callbacks have been registered
  if( start_action_callback_ && 
      stop_action_callback_  &&
      is_finished_callback_  && 
      has_succeed_callback_  && 
      get_result_callback_   && 
      get_feedback_callback_ )
  {
    // launch action server
    as_.start();
    is_started = true;
  }
  else
    ROS_FATAL("Some Callbacks have not been registered yet!");
}

template <class ActionSpec>
bool IriActionServer<ActionSpec>::isStarted(void) const
{
  return is_started;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::shutdown(void)
{
  as_.shutdown();
  is_started = false;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::setAborted(const Result& result, const std::string& text)
{
  as_.setAborted(result, text);
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::setPreempted(const Result& result, const std::string& text)
{
  as_.setPreempted(result, text);
}

template <class ActionSpec>
bool IriActionServer<ActionSpec>::isActive(void)
{
  return as_.isActive();
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
{
  ROS_DEBUG("IriActionServer::executeCallback::start");
  
  try
  {
    // init action
    start_action_callback_(goal);

    // boolean to control action status
    //bool is_active = true;

    // while action is active
    while(this->as_.isActive() )
    {
      ROS_DEBUG("IriActionServer::executeCallback::Active!");
      
      // check if preempt has been requested
      // and if ROS connection is OK
      if( as_.isPreemptRequested() || !ros::ok() )
      {
        ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
        //is_active = false;

        // stop action
        if(!as_.isNewGoalAvailable())
          stop_action_callback_();
        as_.setPreempted();
      }
      // check if action has finished
      else if( is_finished_callback_() )
      {
        ROS_DEBUG("IriActionServer::executeCallback::FINISH!");
        //is_active = false;

        // get action result
        ResultPtr result(new Result);
        get_result_callback_(result);

        // action has been successfully accomplished
        if( has_succeed_callback_() )
        {
          ROS_DEBUG("IriActionServer::executeCallback::SUCCEED!");
          // set action as succeeded
          as_.setSucceeded(*result); 
        }
        // action needs to be aborted
        else
        {
          ROS_DEBUG("IriActionServer::executeCallback::ABORTED!");
          // set action as aborted
          as_.setAborted(*result); 
        }
      }
      // action has not finished yet
      else
      {
        ROS_DEBUG("IriActionServer::executeCallback::feedback");
        // get action feedback and publish it
        FeedbackPtr feedback(new Feedback);
        get_feedback_callback_(feedback);
        as_.publishFeedback(*feedback); 
      }
      
      // force loop rate's frequency if convenient 
      loop_rate_.sleep();
    }
  
  }
  catch(std::exception &e)
  {
    std::cout << e.what() << std::endl;
    as_.setAborted(Result(), "something went wrong!");
  }
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerStartCallback(boost::function<void (const GoalConstPtr&)> cb)
{
  start_action_callback_ = cb;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerStopCallback(boost::function<void ()> cb)
{
  stop_action_callback_ = cb;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerIsFinishedCallback( boost::function<bool ()> cb)
{
  is_finished_callback_ = cb;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerHasSucceedCallback( boost::function<bool ()> cb)
{
  has_succeed_callback_ = cb;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerGetResultCallback(boost::function<void (ResultPtr&)> cb)
{
  get_result_callback_ = cb;
}

template <class ActionSpec>
void IriActionServer<ActionSpec>::registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb)
{
  get_feedback_callback_ = cb;
}

#endif