adding exceptions to iri action server

CMakeLists.txt

@@ -23,9 +23,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 # edit the following line to add all the header files of the library
 # SET(headers ./include/iri_action_server.h)
 
-# FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(iriutils REQUIRED)
 
-# INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${headers})
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${headers})
 
 #common commands for building c++ executables and libraries
 # rosbuild_add_library(${PROJECT_NAME} SHARED ${sources})

include/iri_action_server/iri_action_server.h

@@ -29,6 +29,7 @@
 #include <actionlib/server/simple_action_server.h>
 #include <actionlib/action_definition.h>
 
+#include "exceptions.h"
 
 /**
  * \brief IRI ROS Specific Driver Class
@@ -146,65 +147,74 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
 {
   ROS_DEBUG("IriActionServer::executeCallback::start");
   
-  // init action 
-  start_action_callback_(goal);
+  try
+  {
+    // init action
+    start_action_callback_(goal);
 
-  // boolean to control action status
-  bool is_active = true;
+    // boolean to control action status
+    bool is_active = true;
 
-  // while action is active
-  while( is_active )
-  {
-    ROS_DEBUG("IriActionServer::executeCallback::Active!");
-    
-    // check if preempt has been requested
-    // and if ROS connection is OK
-    if( as_.isPreemptRequested() || !ros::ok() )
+    // while action is active
+    while( is_active )
     {
-      ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
-      is_active = false;
+      ROS_DEBUG("IriActionServer::executeCallback::Active!");
       
-      // stop action
-      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_() )
+      // check if preempt has been requested
+      // and if ROS connection is OK
+      if( as_.isPreemptRequested() || !ros::ok() )
       {
-	ROS_DEBUG("IriActionServer::executeCallback::SUCCEED!");
-        // set action as succeeded
-        as_.setSucceeded(*result); 
+	ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
+	is_active = false;
+	
+	// stop action
+	stop_action_callback_();
+	as_.setPreempted();
       }
-      // action needs to be aborted
+      // 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::ABORTED!");
-        // set action as aborted
-        as_.setAborted(*result); 
+	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();
     }
-    // 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(CException e)
+  {
+    std::cout << e.what() << std::endl;
+    as_.setAborted(Result(), "something went wrong!");
   }
 }