diff --git a/include/iri_action_server/iri_action_server.h b/include/iri_action_server/iri_action_server.h
index 0423c5918b28b9c9446285362f5c2e3b5380effb..65cb665aa6c69690cc6b567c542675389bff194c 100755
--- a/include/iri_action_server/iri_action_server.h
+++ b/include/iri_action_server/iri_action_server.h
@@ -40,20 +40,22 @@ template <class ActionSpec>
 class IriActionServer
 {
   public:
-//     //generates typedefs that we'll use to make our lives easier
-//     ACTION_DEFINITION(ActionSpec);
+    //generates typedefs that we'll use to make our lives easier
+    ACTION_DEFINITION(ActionSpec);
     
   private:
-//     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;
+    boost::function<void (const GoalConstPtr&)> start_action_callback_;
+    boost::function<bool ()> is_finished_callback_;
+    boost::function<bool ()> has_succeed_callback_;
+    boost::function<void (ResultConstPtr&)> get_result_callback_;
+    boost::function<void (FeedbackConstPtr&)> publish_feedback_callback_;
     
-//     void executeCallback(const GoalConstPtr& goal);
+    void executeCallback(const GoalConstPtr& goal);
 
     std::string action_name_;
-//     actionlib::SimpleActionServer<ActionSpec> as_;
+    actionlib::SimpleActionServer<ActionSpec> as_;
+    
+    ros::Rate loop_rate_;
 
   public:
    /**
@@ -82,6 +84,129 @@ class IriActionServer
     *
     */
     ~IriActionServer(void);
+    
+    void setLoopRate(ros::Rate r);
+
+    void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb);
+    void registerIsFinishedCallback( boost::function<bool ()> cb);
+    void registerHasSucceedCallback( boost::function<bool ()> cb);
+    void registerGetResultCallback(boost::function<void (ResultConstPtr&)> cb);
+    void registerPublishFeedbackCallback(boost::function<void (FeedbackConstPtr&)> cb);
+    
 };
 
+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),
+  loop_rate_(10)
+{
+}
+
+template <class ActionSpec>
+IriActionServer<ActionSpec>::~IriActionServer(void)
+{
+}
+
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::setLoopRate(ros::Rate r)
+{
+  loop_rate_ = r;
+}
+
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
+{
+  // check if all callbacks have been registered
+  if( !start_action_callback_ || 
+      !is_finished_callback_  || 
+      !has_succeed_callback_  || 
+      !get_result_callback_   || 
+      !publish_feedback_callback_ )
+  {
+    ROS_ERROR("Some Callbacks have not been registered yet!");
+    return;
+  }
+
+  // init action 
+  start_action_callback_(goal);
+
+  // boolean to control action status
+  bool is_active = true;
+
+  // while action is active
+  while( is_active )
+  {
+    // check if preempt has been requested
+    // and if ROS connection is OK
+    if( as_.isPreemptRequested() || !ros::ok() )
+    {
+      is_active = false;
+    }
+    // check if action has finished
+    else if( is_finished_callback_() )
+    {
+      is_active = false;
+
+      // get action result
+      ResultConstPtr result;
+      get_result_callback_(result);
+      
+      // action has been successfully accomplished
+      if( has_succeed_callback_() )
+      {
+        // set action as succeeded
+        as_.setSucceeded(*result); 
+      }
+      // action needs to be aborted
+      else
+      {
+        // set action as aborted
+        as_.setAborted(*result); 
+      }
+    }
+    // action has not finished yet
+    else
+    {
+      // get action feedback and publish it
+      FeedbackConstPtr feedback;
+      publish_feedback_callback_(feedback);
+      as_.publishFeedback(*feedback); 
+    }
+    
+    // force loop rate's frequency if convenient 
+    loop_rate_.sleep();
+  }
+}
+
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::registerStartCallback(boost::function<void (const GoalConstPtr&)> cb)
+{
+  start_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 (ResultConstPtr&)> cb)
+{
+  get_result_callback_ = cb;
+}
+
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::registerPublishFeedbackCallback(boost::function<void (FeedbackConstPtr&)> cb)
+{
+  publish_feedback_callback_ = cb;
+}
+
 #endif
diff --git a/src/iri_action_server.cpp b/src/iri_action_server.cpp
deleted file mode 100755
index a3d39c51f1d78df08d3412fdc5cd74e4b233ece7..0000000000000000000000000000000000000000
--- a/src/iri_action_server.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "iri_action_server/iri_action_server.h"
-
-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)
-{
-}
-
-template <class ActionSpec>
-IriActionServer<ActionSpec>::~IriActionServer(void)
-{
-}
-
-// template <class ActionSpec>
-// void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
-// {
-// }