Merged branch used to develop the modules basic structures.

CMakeLists.txt

@@ -95,6 +95,11 @@ include_directories(include)
 # )
 add_library(${PROJECT_NAME} src/timeout.cpp src/watchdog.cpp)
 
+add_library(modules include/${PROJECT_NAME}/module_common.h include/${PROJECT_NAME}/module.h include/${PROJECT_NAME}/module_service.h include/${PROJECT_NAME}/module_action.h)
+set_target_properties(modules PROPERTIES LINKER_LANGUAGE CXX)
+target_link_libraries(modules ${PROJECT_NAME})
+
+
 ## Declare a cpp executable
 # add_executable(iri_ros_tool_node src/iri_ros_tool_node.cpp)
 

include/iri_ros_tools/module.h

+#ifndef _IRI_MODULE_H
+#define _IRI_MODULE_H
+
+#include <iri_ros_tools/module_common.h>
+#include <iri_ros_tools/module_service.h>
+#include <iri_ros_tools/module_action.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <dynamic_reconfigure/Reconfigure.h>
+
+#include "threadserver.h"
+#include "eventserver.h"
+#include "mutex.h"
+#include "exceptions.h"
+
+#define MODULE_DEFAULT_RATE 10
+
+template<class ModuleCfg>
+class CModule
+{
+  private:
+    std::string name;
+    ros::Rate module_rate;
+    CMutex module_access;
+    /* internal node handle */
+    ros::NodeHandle module_nh;
+    /* reconfiguer attributes */
+    dynamic_reconfigure::Server<ModuleCfg> dyn_reconf;
+    /* thread attributes */
+    CThreadServer *thread_server;
+    std::string module_thread_id;
+    /* event attributes */
+    CEventServer *event_server;
+    std::string finish_thread_event_id;
+  protected:
+    static void *module_thread(void *param);
+    virtual void state_machine(void);
+    virtual void reconfigure_callback(ModuleCfg &config, uint32_t level);
+    void lock(void);
+    void unlock(void);
+  public:
+    CModule(const std::string &name);
+    void set_rate(double rate_hz);
+    double get_rate(void);
+    bool set_parameter(ros::ServiceClient &service,const std::string &name,bool value);
+    bool set_parameter(ros::ServiceClient &service,const std::string &name,int value);
+    bool set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value);
+    bool set_parameter(ros::ServiceClient &service,const std::string &name,double value);
+    virtual ~CModule();
+};
+
+template<class ModuleCfg>
+CModule<ModuleCfg>::CModule(const std::string &name) : 
+  module_rate(MODULE_DEFAULT_RATE), 
+  dyn_reconf(module_nh),
+  module_nh(ros::this_node::getName(),name)
+{
+  try
+  {
+    this->event_server=CEventServer::instance();
+    this->finish_thread_event_id=name+"_finish_thread_event";
+    this->event_server->create_event(this->finish_thread_event_id);
+    this->thread_server=CThreadServer::instance();
+    this->module_thread_id=name+"_module_thread";
+    this->thread_server->create_thread(this->module_thread_id);
+    this->thread_server->attach_thread(this->module_thread_id,this->module_thread,this);
+    this->thread_server->start_thread(this->module_thread_id);
+    this->name=name;
+    /* initialize the dynamic reconfigure */
+    this->dyn_reconf.setCallback(boost::bind(&CModule<ModuleCfg>::reconfigure_callback,this,_1,_2));
+  }
+  catch (CException &ex)
+  {
+    ROS_ERROR_STREAM("CModule::constructor: exception caught: " << ex.what());
+  }
+}
+
+template<class ModuleCfg>
+void CModule<ModuleCfg>::set_rate(double rate_hz)
+{
+  this->module_rate=rate_hz;
+}
+
+template<class ModuleCfg>
+double CModule<ModuleCfg>::get_rate(void)
+{
+  return this->module_rate;
+}
+
+template<class ModuleCfg>
+void *CModule<ModuleCfg>::module_thread(void *param)
+{
+  CModule *module=(CModule *)param;
+  bool end =false;
+  
+  while(!end)
+  {
+    if(module->event_server->event_is_set(module->finish_thread_event_id))
+      end=true;
+    else
+    {
+      module->module_access.enter();
+      module->state_machine();
+      module->module_access.exit();
+      module->module_rate.sleep();
+    }
+  }
+  
+  pthread_exit(NULL);
+}
+
+template<class ModuleCfg>
+void CModule<ModuleCfg>::state_machine(void)
+{
+
+}
+
+template<class ModuleCfg>
+void CModule<ModuleCfg>::reconfigure_callback(ModuleCfg &config, uint32_t level)
+{
+
+}
+
+template<class ModuleCfg>
+void CModule<ModuleCfg>::lock(void)
+{
+  this->module_access.enter();
+}
+
+template<class ModuleCfg>
+void CModule<ModuleCfg>::unlock(void)
+{
+  this->module_access.exit();
+}
+
+template<class ModuleCfg>
+bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,bool value)
+{
+  unsigned int i=0;
+  bool changed=true,call_status;
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
+  ROS_DEBUG_STREAM("CModule::set boolean parameter " << name << " to " << value);
+
+  this->module_access.enter();
+  set_params_srv.request.config.bools.resize(1);
+  set_params_srv.request.config.bools[0].name=name;
+  set_params_srv.request.config.bools[0].value=value;
+  call_status=service.call(set_params_srv);
+  if(call_status)
+  {
+    for(i=0;i<set_params_srv.response.config.bools.size();i++)
+    {
+      if(set_params_srv.response.config.bools[i].name==name)
+      {
+        if(set_params_srv.response.config.bools[i].value!=value)
+        {
+          changed=false;
+          break;
+        }
+      }
+    }
+    if(changed)
+    {
+      ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
+      this->module_access.exit();
+      return true;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+      this->module_access.exit();
+      return false;
+    }
+  }
+  else
+  {
+    ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+    this->module_access.exit();
+    return false;
+  }
+}
+
+template<class ModuleCfg>
+bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,int value)
+{
+  unsigned int i=0;
+  bool changed=true,call_status;
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
+  ROS_DEBUG_STREAM("CModule::set integer parameter " << name << " to " << value);
+
+  this->module_access.enter();
+  set_params_srv.request.config.ints.resize(1);
+  set_params_srv.request.config.ints[0].name=name;
+  set_params_srv.request.config.ints[0].value=value;
+  call_status=service.call(set_params_srv);
+  if(call_status)
+  {
+    for(i=0;i<set_params_srv.response.config.ints.size();i++)
+    {
+      if(set_params_srv.response.config.ints[i].name==name)
+      {
+        if(set_params_srv.response.config.ints[i].value!=value)
+        {
+          changed=false;
+          break;
+        }
+      }
+    }
+    if(changed)
+    {
+      ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
+      this->module_access.exit();
+      return true;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+      this->module_access.exit();
+      return false;
+    }
+  }
+  else
+  {
+    ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+    this->module_access.exit();
+    return false;
+  }
+}
+
+template<class ModuleCfg>
+bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,std::string &value)
+{
+  unsigned int i=0;
+  bool changed=true,call_status;
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
+  ROS_DEBUG_STREAM("CModule::set string parameter " << name << " to " << value);
+
+  this->module_access.enter();
+  set_params_srv.request.config.strs.resize(1);
+  set_params_srv.request.config.strs[0].name=name;
+  set_params_srv.request.config.strs[0].value=value;
+  call_status=service.call(set_params_srv);
+  if(call_status)
+  {
+    for(i=0;i<set_params_srv.response.config.strs.size();i++)
+    {
+      if(set_params_srv.response.config.strs[i].name==name)
+      {
+        if(set_params_srv.response.config.strs[i].value!=value)
+        {
+          changed=false;
+          break;
+        }
+      }
+    }
+    if(changed)
+    {
+      ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
+      this->module_access.exit();
+      return true;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+      this->module_access.exit();
+      return false;
+    }
+  }
+  else
+  {
+    ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+    this->module_access.exit();
+    return false;
+  }
+}
+
+template<class ModuleCfg>
+bool CModule<ModuleCfg>::set_parameter(ros::ServiceClient &service,const std::string &name,double value)
+{
+  unsigned int i=0;
+  bool changed=true,call_status;
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << service.getService());
+  ROS_DEBUG_STREAM("CModule::set double parameter " << name << " to " << value);
+
+  this->module_access.enter();
+  set_params_srv.request.config.doubles.resize(1);
+  set_params_srv.request.config.doubles[0].name=name;
+  set_params_srv.request.config.doubles[0].value=value;
+  call_status=service.call(set_params_srv);
+  if(call_status)
+  {
+    for(i=0;i<set_params_srv.response.config.doubles.size();i++)
+    {
+      if(set_params_srv.response.config.doubles[i].name==name)
+      {
+        if(set_params_srv.response.config.doubles[i].value!=value)
+        {
+          changed=false;
+          break;
+        }
+      }
+    }
+    if(changed)
+    {
+      ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << service.getService());
+      this->module_access.exit();
+      return true;
+    }
+    else
+    {
+      ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+      this->module_access.exit();
+      return false;
+    }
+  }
+  else
+  {
+    ROS_ERROR_STREAM("CModule::set_parameters: Request failed on server: " << service.getService());
+    this->module_access.exit();
+    return false;
+  }
+}
+
+template<class ModuleCfg>
+CModule<ModuleCfg>::~CModule()
+{
+  if(this->thread_server->get_thread_state(this->module_thread_id)==starting || this->thread_server->get_thread_state(this->module_thread_id)==active)
+  {
+    this->event_server->set_event(this->finish_thread_event_id);
+    this->thread_server->end_thread(this->module_thread_id);
+  }
+  this->thread_server->detach_thread(this->module_thread_id);
+  this->thread_server->delete_thread(this->module_thread_id);
+  this->module_thread_id="";
+  this->event_server->delete_event(this->finish_thread_event_id);
+  this->finish_thread_event_id="";
+}
+
+#endif

include/iri_ros_tools/module_action.h

+#ifndef _MODULE_ACTION_H
+#define _MODULE_ACTION_H
+
+#include <iri_ros_tools/module_common.h>
+#include <ros/ros.h>
+
+#include <iri_ros_tools/timeout.h>
+#include <iri_ros_tools/watchdog.h>
+
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/action_definition.h>
+
+#include "mutex.h"
+
+#define     DEFAULT_ACTION_MAX_RETRIES     5
+#define     DEFAULT_ACTION_TIMEOUT         10
+#define     DEFAULT_WATCHDOG_TIME          2
+
+typedef enum {ACTION_IDLE,ACTION_RUNNING,ACTION_SUCCESS,ACTION_TIMEOUT,ACTION_FB_WATCHDOG,ACTION_ABORTED,ACTION_PREEMPTED} action_status;
+
+template<class action_ros>
+class CModuleAction
+{
+  private:
+    CMutex action_access;
+    std::string name;
+    action_status status;
+    // number of retries attributes
+    unsigned int current_num_retries;
+    unsigned int max_num_retries;
+    // ROS node handle
+    ros::NodeHandle nh;
+    // timeout attributes
+    bool use_timeout;
+    double timeout_value;
+    CROSTimeout action_timeout;
+    // watchdog attributes
+    double watchdog_time;
+    CROSWatchdog feedback_watchdog;
+    // action attributes
+    ACTION_DEFINITION(action_ros);
+    actionlib::SimpleActionClient<action_ros> *action_client;
+    Result action_result_msg;
+    Feedback action_feedback_msg;
+    void action_done(const actionlib::SimpleClientGoalState& state,const ResultConstPtr& result)
+    {
+      this->action_access.enter();
+      if(state==actionlib::SimpleClientGoalState::ABORTED)
+      {
+        ROS_ERROR_STREAM("CModuleAction::action_done: goal on server " << this->name << " aborted");
+        this->status=ACTION_ABORTED;
+      }
+      else if(state==actionlib::SimpleClientGoalState::PREEMPTED)
+      {
+        ROS_WARN_STREAM("CModuleAction::action_done: goal on server " << this->name << " preempted");
+        this->status=ACTION_PREEMPTED;
+      }
+      else if(state==actionlib::SimpleClientGoalState::SUCCEEDED)
+      {
+        ROS_DEBUG_STREAM("CModuleAction::action_done: goal on server " << this->name << " successfull");
+        this->status=ACTION_SUCCESS;
+      } 
+      this->action_result_msg=*result;
+      this->action_access.exit();
+    }
+    void action_active(void);
+    void action_feedback(const FeedbackConstPtr& feedback)
+    {
+      ROS_DEBUG_STREAM("CModuleAction::action_feedback: Got Feedback from server " << this->name << "!");
+      this->action_access.enter();
+      this->action_feedback_msg=*feedback;
+      this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
+      this->action_access.exit();
+    }
+  public:
+    CModuleAction(const std::string &name,const std::string &name_space=std::string(""));
+    void set_max_num_retries(unsigned int max_retries);
+    unsigned int get_max_num_retries(void);
+    void set_feedback_watchdog_time(double time_s);
+    double get_feedback_watchdog_time(void);
+    bool is_watchdog_active(void);
+    void enable_timeout(void);
+    void disable_timeout(void); 
+    void start_timeout(double time_s);
+    void update_timeout(double time_s);
+    void stop_timeout(void);
+    bool is_timeout_enabled(void);
+    bool is_timeout_active(void);
+    std::string get_name(void);
+    act_srv_status make_request(Goal &msg)
+    {
+      if(this->action_client->isServerConnected())
+      {
+        ROS_DEBUG_STREAM("CModuleAction::make_request: Server " << this->name << " is Available!");
+        this->current_num_retries=0;
+        this->action_client->sendGoal(msg,
+                    boost::bind(&CModuleAction<action_ros>::action_done,     this, _1, _2),
+                    boost::bind(&CModuleAction<action_ros>::action_active,   this),
+                    boost::bind(&CModuleAction<action_ros>::action_feedback, this, _1));
+        this->feedback_watchdog.reset(ros::Duration(this->watchdog_time));
+        this->status=ACTION_RUNNING;
+        ROS_DEBUG_STREAM("CModuleAction::make_request: Goal Sent to server " << this->name << ". Wait for Result!");
+        return ACT_SRV_SUCCESS;
+      }
+      else
+      {
+        this->current_num_retries++;
+        if(this->current_num_retries>this->max_num_retries)
+        {
+          this->current_num_retries=0;
+          ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " failed!");
+          return ACT_SRV_FAIL;
+        }
+        else
+        {
+          ROS_DEBUG_STREAM("CModuleAction::make_request: Action start on server " << this->name << " pending!");
+          return ACT_SRV_PENDING;
+        }
+      }
+    }
+
+    void cancel(void);
+    bool is_finished(void);
+    action_status get_state(void);
+    Result get_result(void)
+    {
+      return this->action_result_msg;
+    }
+    Feedback get_feedback(void)
+    {
+      return this->action_feedback_msg;
+    }
+    ~CModuleAction();
+};
+
+template<class action_ros>
+CModuleAction<action_ros>::CModuleAction(const std::string &name,const std::string &name_space):nh(ros::this_node::getName()+"/"+name_space)
+{
+  std::size_t position;
+
+  this->name=name;
+  this->status=ACTION_IDLE;
+  // retry control
+  this->current_num_retries=0;
+  this->max_num_retries=DEFAULT_ACTION_MAX_RETRIES;
+  // assign the full service name
+  // timeouts
+  this->use_timeout=false;
+  this->timeout_value=DEFAULT_ACTION_TIMEOUT;
+  // waychdog
+  this->watchdog_time=DEFAULT_WATCHDOG_TIME;
+  // internal action
+  this->action_client=NULL;
+  this->action_client=new actionlib::SimpleActionClient<action_ros>(this->nh,name,true);
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::action_active(void)
+{
+  ROS_DEBUG_STREAM("CModuleAction::action_active: Goal on server " << this->name << " just went active!");
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::set_max_num_retries(unsigned int max_retries)
+{
+  this->action_access.enter();
+  this->max_num_retries=max_retries;
+  this->action_access.exit();
+}
+
+template<class action_ros>
+unsigned int CModuleAction<action_ros>::get_max_num_retries(void)
+{
+  return this->max_num_retries;
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::set_feedback_watchdog_time(double time_s)
+{
+  this->action_access.enter();
+  if(time_s>0.0)
+    this->watchdog_time=time_s;
+  this->action_access.exit();
+}
+
+template<class action_ros>
+double CModuleAction<action_ros>::get_feedback_watchdog_time(void)
+{
+  return this->watchdog_time;
+}
+
+template<class action_ros>
+bool CModuleAction<action_ros>::is_watchdog_active(void)
+{
+  this->action_access.enter();
+  if(this->feedback_watchdog.is_active())
+  {
+    this->status=ACTION_FB_WATCHDOG;
+    this->action_access.exit();
+    return true;
+  }
+  else
+  {
+    this->action_access.exit();
+    return false;
+  }
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::enable_timeout(void)
+{
+  this->action_access.enter();
+  this->use_timeout=true;
+  this->action_access.exit();
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::disable_timeout(void)
+{
+  this->action_access.enter();
+  this->use_timeout=false;
+  this->action_access.exit();
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::start_timeout(double time_s)
+{
+  this->action_access.enter();
+  if(this->use_timeout && time_s>0.0)
+  {
+    this->timeout_value=time_s;
+    this->action_timeout.start(ros::Duration(time_s));
+  }
+  this->action_access.exit();
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::update_timeout(double time_s)
+{
+  this->action_access.enter();
+  if(this->use_timeout && time_s>0.0)
+  {
+    this->action_timeout.start(ros::Duration(time_s));
+    this->timeout_value=time_s;
+  }
+  this->action_access.exit();
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::stop_timeout(void)
+{
+  this->action_access.enter();
+  this->action_timeout.stop();
+  this->action_access.exit();
+}
+
+template<class action_ros>
+bool CModuleAction<action_ros>::is_timeout_enabled(void)
+{
+  return this->use_timeout;
+}
+
+template<class action_ros>
+bool CModuleAction<action_ros>::is_timeout_active(void)
+{
+  this->action_access.enter();
+  if(this->action_timeout.timed_out())
+  {
+    this->status=ACTION_TIMEOUT;
+    this->action_access.exit();
+    return true;
+  }
+  else
+  {
+    this->action_access.exit();
+    return false;
+  }
+}
+
+template<class action_ros>
+std::string CModuleAction<action_ros>::get_name(void)
+{
+  return this->name;
+}
+
+template<class action_ros>
+void CModuleAction<action_ros>::cancel(void)
+{
+  this->action_client->cancelGoal();
+}
+
+template<class action_ros>
+bool CModuleAction<action_ros>::is_finished(void)
+{
+  actionlib::SimpleClientGoalState action_state(actionlib::SimpleClientGoalState::PENDING);
+
+  action_state=action_client->getState();
+  if(action_state==actionlib::SimpleClientGoalState::ACTIVE)
+    return false;
+  else
+    return true;
+}
+
+template<class action_ros>
+action_status CModuleAction<action_ros>::get_state(void)
+{
+  if(this->status==ACTION_RUNNING)
+  {
+    if(this->use_timeout && this->is_timeout_active())
+      this->status=ACTION_TIMEOUT;
+    if(this->is_watchdog_active())
+      this->status=ACTION_FB_WATCHDOG;
+  }
+
+  return this->status;
+}
+
+template<class action_ros>
+CModuleAction<action_ros>::~CModuleAction()
+{
+  if(this->status==ACTION_RUNNING)
+  {
+    this->action_client->cancelGoal();
+    while(!this->is_finished());
+  }
+  delete this->action_client;
+}
+
+#endif

include/iri_ros_tools/module_common.h

+#ifndef _MODULE_COMMON_H
+#define _MODULE_COMMON_H
+
+typedef enum {ACT_SRV_SUCCESS,ACT_SRV_PENDING,ACT_SRV_FAIL} act_srv_status;
+
+#endif

include/iri_ros_tools/module_service.h

+#ifndef _MODULE_SERVICE_H
+#define _MODULE_SERVICE_H
+
+#include <ros/ros.h>
+#include <iri_ros_tools/module_common.h>
+
+#define     DEFAULT_SERVICE_MAX_RETRIES     5
+
+template<class service_msg>
+class CModuleService
+{
+  private:
+    std::string name;
+    unsigned int current_num_retries;
+    unsigned int max_num_retries;
+    ros::NodeHandle nh;
+    ros::ServiceClient service_client;
+    boost::function< bool(service_msg &msg) > call_successfull;
+  protected:
+    bool default_call_check(service_msg &msg);
+  public:
+    CModuleService(const std::string &name,const std::string &name_space=std::string(""));
+    void set_max_num_retries(unsigned int max_retries);
+    unsigned int get_max_num_retries(void);
+    std::string get_name(void);
+    void set_call_check_function(const boost::function< bool(service_msg &msg) > &function);
+    act_srv_status call(service_msg &msg);
+    ~CModuleService();
+};
+
+template<class service_msg>
+CModuleService<service_msg>::CModuleService(const std::string &name,const std::string &name_space):nh(ros::this_node::getName()+"/"+name_space)
+{
+  std::size_t position;
+
+  this->current_num_retries=0;
+  this->max_num_retries=DEFAULT_SERVICE_MAX_RETRIES;
+  this->service_client=this->nh.template serviceClient<service_msg>(name); 
+  // assign the full service name
+  this->name=name;
+  // assign the default check function
+  this->call_successfull=boost::bind(&CModuleService::default_call_check, this, _1);
+}
+
+template<class service_msg>
+bool CModuleService<service_msg>::default_call_check(service_msg &msg)
+{
+  return true;
+}
+
+template<class service_msg>
+void CModuleService<service_msg>::set_max_num_retries(unsigned int max_retries)
+{
+  this->max_num_retries=max_retries;
+}
+
+template<class service_msg>
+unsigned int CModuleService<service_msg>::get_max_num_retries(void)
+{
+  return this->max_num_retries;
+}
+
+template<class service_msg>
+std::string CModuleService<service_msg>::get_name(void)
+{
+  return this->name;
+}
+
+template<class service_msg>
+act_srv_status CModuleService<service_msg>::call(service_msg &msg)
+{
+  ROS_DEBUG_STREAM("CModuleService::call: Sending New Request to service " << this->service_client.getService());
+
+  if(this->service_client.call(msg))
+  {
+    if(this->call_successfull(msg))
+    {
+      ROS_DEBUG_STREAM("CModuleService::call: Request successfull! (server: " << this->service_client.getService() << ")");
+      this->current_num_retries=0;
+      return ACT_SRV_SUCCESS;
+    }
+    else
+    {
+      this->current_num_retries++;
+      if(this->current_num_retries>this->max_num_retries)
+      {
+        ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")");
+        this->current_num_retries=0;
+        return ACT_SRV_FAIL;
+      }
+      else
+      {
+        ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")");
+        return ACT_SRV_PENDING;
+      }
+    }
+  }
+  else
+  {
+    this->current_num_retries++;
+    if(this->current_num_retries>this->max_num_retries)
+    {
+      ROS_ERROR_STREAM("CModuleService::call: Request failed! (server: " << this->service_client.getService() << ")");
+      this->current_num_retries=0;
+      return ACT_SRV_FAIL;
+    }
+    else
+    {
+      ROS_WARN_STREAM("CModuleService::call: Request pending! (server: " << this->service_client.getService() << ")");
+      return ACT_SRV_PENDING;
+    }
+  }
+}
+
+template<class service_msg>
+void CModuleService<service_msg>::set_call_check_function(const boost::function< bool(service_msg &msg) > &function)
+{
+  this->call_successfull=function;
+}
+
+template<class service_msg>
+CModuleService<service_msg>::~CModuleService()
+{
+
+}
+
+#endif

include/iri_ros_tools/watchdog.h

@@ -11,7 +11,7 @@ class CROSWatchdog
     pthread_mutex_t access_;
   public:
     CROSWatchdog();
-    void reset(ros::Duration time);
+    void reset(const ros::Duration &time);
     bool is_active(void);
     ~CROSWatchdog();
 };

src/watchdog.cpp

@@ -6,22 +6,20 @@ CROSWatchdog::CROSWatchdog()
   this->max_time=ros::Duration(0);
 }
 
-void CROSWatchdog::reset(ros::Duration time)
+void CROSWatchdog::reset(const ros::Duration &time)
 {
   pthread_mutex_lock(&this->access_);
   this->max_time=time;
+  this->start_time=ros::Time::now();
   pthread_mutex_unlock(&this->access_);
 }
 
 bool CROSWatchdog::is_active(void)
 {
-  this->start_time=ros::Time::now();
   ros::Time current_time=ros::Time::now();
 
   pthread_mutex_lock(&this->access_);
-  this->max_time-=(current_time-this->start_time);
-  this->start_time=current_time;
-  if(this->max_time.toSec()<=0.0)
+  if((current_time-this->start_time)>this->max_time)
   {
     pthread_mutex_unlock(&this->access_);
     return true;