diff --git a/include/iri_action_server/iri_action_server.h b/include/iri_action_server/iri_action_server.h
index 6919b71f8c980bb910b5df2d3d31d7ac9b70c5a8..0423c5918b28b9c9446285362f5c2e3b5380effb 100755
--- a/include/iri_action_server/iri_action_server.h
+++ b/include/iri_action_server/iri_action_server.h
@@ -27,11 +27,8 @@
// ros action server
#include <actionlib/server/simple_action_server.h>
+#include <actionlib/action_definition.h>
-// include iri-utils thread and event servers and mutex object
-#include "threadserver.h"
-#include "eventserver.h"
-#include "mutex.h"
/**
* \brief IRI ROS Specific Driver Class
@@ -39,35 +36,24 @@
* This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>,
* http://www.ros.org/doc/api/actionlib/html/classactionlib_1_1SimpleActionServer.html
*/
-template <class MessageClass>
+template <class ActionSpec>
class IriActionServer
{
+ public:
+// //generates typedefs that we'll use to make our lives easier
+// ACTION_DEFINITION(ActionSpec);
+
private:
- /**
- * \brief Reference to the unique thread handler
- *
- * This reference to the unique thread handler is initialized when an object
- * of this class is first created. It is used to create and handle all the
- * IriBaseNodeDriver threads. The object pointed by this reference is shared
- * by all objects in any application.
- */
- CThreadServer *thread_server_;
- std::string monitor_thread_id_;
+// boost::function<void (const ActionGoalConstPtr&)> start_action_callback;
+// boost::function<bool ()> is_finished_callback;
+// boost::function<bool ()> is_success_callback;
+// boost::function<void (const ResultConstPtr&)> get_result_callback;
+// boost::function<void (const FeedbackConstPtr&)> publish_feedback_callback;
- CEventServer *event_server_;
- std::string active_action_event_id_;
-
- enum IRI_ASERVER_STATES { UNINIT, INIT, IDLE, ACTIVE, PREEMPTED };
- unsigned int current_state_;
- CMutex state_mutex_;
-
- bool block_new_actions_;
+// void executeCallback(const GoalConstPtr& goal);
- ros::NodeHandle nh_;
std::string action_name_;
- actionlib::SimpleActionServer<MessageClass> as_;
-
- enum IRI_ASERVER_RESULTS { ABORT, NOT_FINISHED, SUCCEED };
+// actionlib::SimpleActionServer<ActionSpec> as_;
public:
/**
@@ -87,7 +73,7 @@ class IriActionServer
*
* \param nh a reference to the node handle object to manage all ROS topics.
*/
- IriActionServer(const std::string & action_name, const bool & bna=false);
+ IriActionServer(const std::string & action_name, ros::NodeHandle & nh);
/**
* \brief Destructor
@@ -96,40 +82,6 @@ class IriActionServer
*
*/
~IriActionServer(void);
-
- protected:
- /**
- * \brief start
- *
- *
- */
- void start(void);
- /**
- * \brief start
- *
- *
- */
- void goalCallback(void);
- /**
- * \brief start
- *
- *
- */
- void preemptCallback(void);
- /**
- * \brief start
- *
- *
- */
- static void *monitorThread(void *param);
-
- void startActionCallback(void);
-
- int actionIsFinish(void);
-
- void fillUpFeedbackCallback(void);
-
- void fillUpResultCallback(void);
};
#endif
diff --git a/src/iri_action_server.cpp b/src/iri_action_server.cpp
index d288aad23315fadfa7e1359e3580349db431cfbf..a3d39c51f1d78df08d3412fdc5cd74e4b233ece7 100755
--- a/src/iri_action_server.cpp
+++ b/src/iri_action_server.cpp
@@ -1,197 +1,18 @@
#include "iri_action_server/iri_action_server.h"
-template <class MessageClass>
-IriActionServer<MessageClass>::IriActionServer(const std::string & aname, const bool & bna) :
- monitor_thread_id_("monitor_thread"),
- active_action_event_id_("active_action_event"),
- block_new_actions_(bna),
- nh_(),
- action_name_(aname),
- as_(nh_, action_name_, false),
- current_state_(UNINIT)
+template <class ActionSpec>
+IriActionServer<ActionSpec>::IriActionServer(const std::string & aname, ros::NodeHandle & nh) :
+ action_name_(aname)
+// as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), true)
{
- // create thread server instance
- thread_server_ = CThreadServer::instance();
-
- // create monitor thread and attach it to server
- thread_server_->create_thread(monitor_thread_id_);
- thread_server_->attach_thread(monitor_thread_id_, this->monitorThread, this);
-
- // create event server instance
- event_server_ = CEventServer::instance();
-
- // create active action event
- event_server_->create_event(active_action_event_id_);
-
- // register the goal and feeback callbacks
- as_.registerGoalCallback(boost::bind(&MessageClass::goalCB, this));
- as_.registerPreemptCallback(boost::bind(&MessageClass::preemptCB, this));
-
- // set current state to Initialized
- current_state_ = INIT;
-}
-
-template <class MessageClass>
-IriActionServer<MessageClass>::~IriActionServer(void)
-{
- thread_server_->kill_thread(monitor_thread_id_);
-}
-
-template <class MessageClass>
-void IriActionServer<MessageClass>::start(void)
-{
- // start action server to allow client requests
- as_.start();
-}
-
-template <class MessageClass>
-void IriActionServer<MessageClass>::goalCallback(void)
-{
- ROS_INFO("%s: Goal", action_name_.c_str());
- state_mutex_.enter();
- switch(current_state_)
- {
- case INIT:
- // start action monitor thread
- thread_server_->start_thread(monitor_thread_id_);
-
- //execute IDLE case
- case IDLE:
-//TODO:
-// // accept the new goal
-// GoalMessageClass msg = as_.acceptNewGoal()->samples;
-
- // set active action event to activate monitor thread
- event_server_->set_event(active_action_event_id_);
-
- // launch user's defined start action callback
- startActionCallback();
- break;
-
- case ACTIVE:
- // if new actions are ignored in favor to current action
- if(block_new_actions_)
- {
- // ignore new action request
- }
- // cancel current action in favor of new one
- else
- {
- // set the action state to preempted
- as_.setPreempted();
- current_state_ = PREEMPTED;
- }
- break;
- }
- state_mutex_.exit();
-}
-
-template <class MessageClass>
-void IriActionServer<MessageClass>::preemptCallback(void)
-{
- ROS_INFO("%s: Preempted", action_name_.c_str());
- state_mutex_.enter();
- // check if Preempt Callback was due to new action request
- if(as_.isNewGoalAvailable())
- {
- // if new actions are ignored in favor to current action
- if(block_new_actions_)
- {
- // ignore new action request
- }
- // cancel current action in favor of new one
- else
- {
- // set the action state to preempted
- as_.setPreempted();
- current_state_ = PREEMPTED;
- }
- }
- // callback is due to client preempt
- else
- {
- // set the action state to preempted
- as_.setPreempted();
- current_state_ = PREEMPTED;
- }
- state_mutex_.exit();
-}
-
-template <class MessageClass>
-void *IriActionServer<MessageClass>::monitorThread(void *param)
-{
- // retrieve base algorithm class
- IriActionServer<MessageClass> * iri_as = (IriActionServer<MessageClass> *)param;
-
- // define monitor loop iteration rate
- ros::Rate r(10);
-
- while(iri_as->event_server->event_is_set(iri_as->active_action_event_id_))
- {
- iri_as->state_mutex_.enter();
- switch(iri_as->current_state_)
- {
- case ACTIVE:
- iri_as->fillUpFeedbackCallback();
-
-//TODO:
-// // publish the feedback
-// iri_as->as_.publishFeedback(msg_feedback_);
-
- switch(iri_as->actionIsFinished())
- {
- case NOT_FINISHED:
- break;
-
- case SUCCEED:
- // launch user's defined fill up result callback
- iri_as->fillUpResultCallback();
-
-//TODO:
-// // set the action state to succeeded
-// iri_as->as_.setSucceeded(result_);
- break;
-
- case ABORT:
- default:
- // launch user's defined fill up result callback
- iri_as->fillUpResultCallback();
-
-//TODO:
-// // set the action state to aborted
-// iri_as->as_.setAborted(result_);
- break;
- }
- break;
-
- default:
- break;
- }
- iri_as->state_mutex_.exit();
-
- //force rate's frequency if convenient
- r.sleep();
- }
}
-template <class MessageClass>
-void IriActionServer<MessageClass>::startActionCallback(void)
+template <class ActionSpec>
+IriActionServer<ActionSpec>::~IriActionServer(void)
{
}
-template <class MessageClass>
-int IriActionServer<MessageClass>::actionIsFinish(void)
-{
- //ABORT, NOT_FINISHED, SUCCEED
- return NOT_FINISHED;
-}
-
-template <class MessageClass>
-void IriActionServer<MessageClass>::fillUpFeedbackCallback(void)
-{
-}
-
-template <class MessageClass>
-void IriActionServer<MessageClass>::fillUpResultCallback(void)
-{
-}
+// template <class ActionSpec>
+// void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
+// {
+// }