Skip to content
Snippets Groups Projects
Commit 075056cd authored by Joan Perez Ibarz's avatar Joan Perez Ibarz
Browse files

Documentation for IRI Action Server

parent f1682d00
No related branches found
Tags v0.4.0
No related merge requests found
...@@ -32,52 +32,168 @@ ...@@ -32,52 +32,168 @@
#include "exceptions.h" #include "exceptions.h"
/** /**
* \brief IRI ROS Specific Driver Class * \brief IRI ROS Action Server
* *
* This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>, * This class is a wrapper for the ROS Simple Action Server, thus their policies
* http://www.ros.org/doc/api/actionlib/html/classactionlib_1_1SimpleActionServer.html * are adquired. The action server logic for receiving new goals, controlling
* client preemption and dealing with goal status has been implemented on top of
* the simple action server. Instead of working straight with the provided execute
* callback, this class defines additional callbacks to the user with a more
* reduced scope. By using this class, the final user just needs to worry
* about executing and cancelling the action and filling up feedback and result
* messages.
*
* This is a template class, where ActionSpec must be a class generated from an
* action message. Goal, Result and Feedback objects are assumed to exist.
*/ */
template <class ActionSpec> template <class ActionSpec>
class IriActionServer class IriActionServer
{ {
public: public:
//generates typedefs that we'll use to make our lives easier /**
* \brief Action definitions macro
*
* This ROS defined macro includes convenient type definitions for Goal,
* Result and Feedback classes to ease developement.
*/
ACTION_DEFINITION(ActionSpec); ACTION_DEFINITION(ActionSpec);
/**
* \brief Result Pointer type definition
*
* Additional definition for non-constant result pointers
*/
typedef boost::shared_ptr<Result> ResultPtr; typedef boost::shared_ptr<Result> ResultPtr;
/**
* \brief Result Pointer type definition
*
* Additional definition for non-constant feedback pointers
*/
typedef boost::shared_ptr<Feedback> FeedbackPtr; typedef boost::shared_ptr<Feedback> FeedbackPtr;
private: private:
/**
* \brief Start Callback object
*
* Boost function object to bind user callback. The start callback is called
* once at the beginning of the simple action server execute callback. User
* may use the Goal object to start the action within this callback.
*
* The user defined function must receive a GoalConstPtr and return void.
*/
boost::function<void (const GoalConstPtr&)> start_action_callback_; boost::function<void (const GoalConstPtr&)> start_action_callback_;
/**
* \brief Stop Callback object
*
* Boost function object to bind user callback. The stop callback is called
* whenever the client requests a preempt or the action fails for some other
* reason. User may stop the action and get ready to receieve another goal.
*
* The user defined function takes no parameters and returns void.
*/
boost::function<void ()> stop_action_callback_; boost::function<void ()> stop_action_callback_;
/**
* \brief Is Finished Callback object
*
* Boost function object to bind user callback. The is finished callback is
* called from the simple action server execute callback to check whether the
* action has been finished or not. An action can finish either because it
* accomplished its goal or because it has been aborted.
*
* The user defined function takes no parameters and returns a boolean
* indicating if the action is finished or not.
*/
boost::function<bool ()> is_finished_callback_; boost::function<bool ()> is_finished_callback_;
/**
* \brief Has Succeed Callback object
*
* Boost function object to bind user callback. The has succeeded callback is
* called from the simple action server execute callback when the action has
* finished.
*
* The user defined function takes no parameters and returns a boolean
* indicating if the action successfully accomplished its goal (true) or if
* it was aborted (false).
*/
boost::function<bool ()> has_succeed_callback_; boost::function<bool ()> has_succeed_callback_;
/**
* \brief Get Result Callback object
*
* Boost function object to bind user callback. The get result callback is
* called from the simple action server execute callback when the action has
* finished. User must fill up the result object with the convenient data
* depending on wether the action accomplished its goal or was aborted.
*
* The user defined function must receive a ResultPtr and return void.
*/
boost::function<void (ResultPtr&)> get_result_callback_; boost::function<void (ResultPtr&)> get_result_callback_;
/**
* \brief Get Feedback Callback object
*
* Boost function object to bind user callback. The get result callback is
* called on each iteration of the simple action server execute callback while
* the action is active. User must fill up the feedback object to leave it
* ready to be published.
*
* The user defined function must receive a FeedbackPtr and return void.
*/
boost::function<void (FeedbackPtr&)> get_feedback_callback_; boost::function<void (FeedbackPtr&)> get_feedback_callback_;
/**
* \brief Execute Callback
*
* This is the ROS simple action server execute callback which is called in a
* separate thread whenever a new goal is received. This function implements
* the IRI action server logic on top of the ROS simple action server policy.
*
* This function controls the life-cycle of a single goal, since it is received
* until achieves a terminal state. User callbacks are triggered to monitor
* the goal state during the action execution.
*
* \param GoalConstPtr a const pointer to the received action goal
*/
void executeCallback(const GoalConstPtr& goal); void executeCallback(const GoalConstPtr& goal);
/**
* \brief Action Name
*
* This string stores the action name used to request actions.
*/
std::string action_name_; std::string action_name_;
actionlib::SimpleActionServer<ActionSpec> as_;
/**
* \brief Simple Action Server
*
* The ROS simple action server is used to handle action requests. For
* complete information on its behaviour please refer to:
* http://www.ros.org/wiki/actionlib/DetailedDescription
*/
actionlib::SimpleActionServer<ActionSpec> as_;
/**
* \brief Loop Rate
*
* This object controls the frequency of the execution callback loop. The
* value can be updated by calling the member method setLoopRate.
*/
ros::Rate loop_rate_; ros::Rate loop_rate_;
public: public:
/** /**
* \brief constructor * \brief Constructor
*
* This constructor mainly creates and initializes the IriActionServer topics
* through the given public_node_handle object. IriBaseNodeDriver attributes
* may be also modified to suit node specifications.
*
* All kind of ROS topics (publishers, subscribers, servers or clients) can
* be easyly generated with the scripts in the iri_ros_scripts package. Refer
* to ROS and IRI Wiki pages for more details:
* *
* http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++) * This constructor mainly initializes the ROS simple action server object,
* http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++) * as well as the action name and the execute loop rate frequency.
* http://wikiri.upc.es/index.php/Robotics_Lab
* *
* \param nh a reference to the node handle object to manage all ROS topics. * \param nh a reference to the node handle object to manage all ROS topics.
* \param action_name name of the action
*/ */
IriActionServer(ros::NodeHandle & nh, const std::string & action_name); IriActionServer(ros::NodeHandle & nh, const std::string & action_name);
...@@ -89,14 +205,74 @@ class IriActionServer ...@@ -89,14 +205,74 @@ class IriActionServer
*/ */
~IriActionServer(void); ~IriActionServer(void);
/**
* \brief Start Action
*
* This function checks whether all user callbacks have been registered and
* then starts the simple action server.
*/
void start(void); void start(void);
/**
* \brief Set Loop Rate
*
* This function sets the loop rate for the execute callback
*
*/
void setLoopRate(ros::Rate r); void setLoopRate(ros::Rate r);
/**
* \brief Register Start Callback
*
* This method let's the user register the start callback method
*
* \param cb user callback function with input parameter GoalConstPtr
*/
void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb); void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb);
/**
* \brief Register Stop Callback
*
* This method let's the user register the stop callback method
*
* \param cb user callback function with no input parameters
*/
void registerStopCallback(boost::function<void ()> cb); void registerStopCallback(boost::function<void ()> cb);
/**
* \brief Register Is Finished Callback
*
* This method let's the user register the is finished callback method
*
* \param cb user callback function with no input parameters, returns boolean
*/
void registerIsFinishedCallback( boost::function<bool ()> cb); void registerIsFinishedCallback( boost::function<bool ()> cb);
/**
* \brief Register Has Succeed Callback
*
* This method let's the user register the has succeed callback method
*
* \param cb user callback function with no input parameters, returns boolean
*/
void registerHasSucceedCallback( boost::function<bool ()> cb); void registerHasSucceedCallback( boost::function<bool ()> cb);
/**
* \brief Register Get Result Callback
*
* This method let's the user register the get result callback method
*
* \param cb user callback function with input parameter ResultPtr
*/
void registerGetResultCallback(boost::function<void (ResultPtr&)> cb); void registerGetResultCallback(boost::function<void (ResultPtr&)> cb);
/**
* \brief Register Get Feedback Callback
*
* This method let's the user register the get feedback callback method
*
* \param cb user callback function with input parameter FeedbackPtr
*/
void registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb); void registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb);
}; };
...@@ -164,46 +340,46 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal) ...@@ -164,46 +340,46 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
// and if ROS connection is OK // and if ROS connection is OK
if( as_.isPreemptRequested() || !ros::ok() ) if( as_.isPreemptRequested() || !ros::ok() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!"); ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
is_active = false; is_active = false;
// stop action // stop action
stop_action_callback_(); stop_action_callback_();
as_.setPreempted(); as_.setPreempted();
} }
// check if action has finished // check if action has finished
else if( is_finished_callback_() ) else if( is_finished_callback_() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::FINISH!"); ROS_DEBUG("IriActionServer::executeCallback::FINISH!");
is_active = false; is_active = false;
// get action result // get action result
ResultPtr result(new Result); ResultPtr result(new Result);
get_result_callback_(result); get_result_callback_(result);
// action has been successfully accomplished // action has been successfully accomplished
if( has_succeed_callback_() ) if( has_succeed_callback_() )
{ {
ROS_DEBUG("IriActionServer::executeCallback::SUCCEED!"); ROS_DEBUG("IriActionServer::executeCallback::SUCCEED!");
// set action as succeeded // set action as succeeded
as_.setSucceeded(*result); as_.setSucceeded(*result);
} }
// action needs to be aborted // action needs to be aborted
else else
{ {
ROS_DEBUG("IriActionServer::executeCallback::ABORTED!"); ROS_DEBUG("IriActionServer::executeCallback::ABORTED!");
// set action as aborted // set action as aborted
as_.setAborted(*result); as_.setAborted(*result);
} }
} }
// action has not finished yet // action has not finished yet
else else
{ {
ROS_DEBUG("IriActionServer::executeCallback::feedback"); ROS_DEBUG("IriActionServer::executeCallback::feedback");
// get action feedback and publish it // get action feedback and publish it
FeedbackPtr feedback(new Feedback); FeedbackPtr feedback(new Feedback);
get_feedback_callback_(feedback); get_feedback_callback_(feedback);
as_.publishFeedback(*feedback); as_.publishFeedback(*feedback);
} }
// force loop rate's frequency if convenient // force loop rate's frequency if convenient
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment