Added a function (start_operation()) to be called in the constructor of the...

Added a function (start_operation()) to be called in the constructor of the lowest class in the hierarchy.
Added an exception module.
Added a new class to implement the service call to dynamic reconfigure servers.

CMakeLists.txt

@@ -95,7 +95,7 @@ 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)
+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 include/${PROJECT_NAME}/module_exceptions.h include/${PROJECT_NAME}/module_dyn_reconf.h)
 set_target_properties(modules PROPERTIES LINKER_LANGUAGE CXX)
 target_link_libraries(modules ${PROJECT_NAME})
 

include/iri_ros_tools/module.h

@@ -4,6 +4,7 @@
 #include <iri_ros_tools/module_common.h>
 #include <iri_ros_tools/module_service.h>
 #include <iri_ros_tools/module_action.h>
+#include <iri_ros_tools/module_exceptions.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <dynamic_reconfigure/Reconfigure.h>
@@ -11,7 +12,6 @@
 #include "threadserver.h"
 #include "eventserver.h"
 #include "mutex.h"
-#include "exceptions.h"
 
 #define MODULE_DEFAULT_RATE 10
 
@@ -34,14 +34,16 @@ class CModule
     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);
+    virtual void state_machine(void)=0;
+    virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+    void start_operation(void);
     void lock(void);
     void unlock(void);
   public:
     CModule(const std::string &name);
     void set_rate(double rate_hz);
     double get_rate(void);
+    std::string get_name(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);
@@ -64,21 +66,22 @@ CModule<ModuleCfg>::CModule(const std::string &name) :
     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());
+    throw CModuleException(_HERE_,"Impossible to build module",this->name);
   }
 }
 
 template<class ModuleCfg>
 void CModule<ModuleCfg>::set_rate(double rate_hz)
 {
-  this->module_rate=rate_hz;
+  if(rate_hz<=0.0)
+    throw CModuleException(_HERE_,"Module rate must be positive",this->name);
+  else
+    this->module_rate=rate_hz;
 }
 
 template<class ModuleCfg>
@@ -87,11 +90,20 @@ double CModule<ModuleCfg>::get_rate(void)
   return this->module_rate;
 }
 
+template<class ModuleCfg>
+std::string CModule<ModuleCfg>::get_name(void)
+{
+  return this->name;
+}
+ 
 template<class ModuleCfg>
 void *CModule<ModuleCfg>::module_thread(void *param)
 {
   CModule *module=(CModule *)param;
   bool end =false;
+
+  /* initialize the dynamic reconfigure */
+  module->dyn_reconf.setCallback(boost::bind(&CModule<ModuleCfg>::reconfigure_callback,module,_1,_2));
   
   while(!end)
   {
@@ -110,15 +122,9 @@ void *CModule<ModuleCfg>::module_thread(void *param)
 }
 
 template<class ModuleCfg>
-void CModule<ModuleCfg>::state_machine(void)
+void CModule<ModuleCfg>::start_operation(void)
 {
-
-}
-
-template<class ModuleCfg>
-void CModule<ModuleCfg>::reconfigure_callback(ModuleCfg &config, uint32_t level)
-{
-
+  this->thread_server->start_thread(this->module_thread_id);
 }
 
 template<class ModuleCfg>

include/iri_ros_tools/module_action.h

@@ -2,6 +2,7 @@
 #define _MODULE_ACTION_H
 
 #include <iri_ros_tools/module_common.h>
+#include <iri_ros_tools/module_exceptions.h>
 #include <ros/ros.h>
 
 #include <iri_ros_tools/timeout.h>
@@ -180,6 +181,11 @@ 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;
+  else
+  {
+    this->action_access.exit();
+    throw CModuleException(_HERE_,"Invalid watchdog time value",this->name);
+  }
   this->action_access.exit();
 }
 
@@ -226,10 +232,18 @@ 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)
+  if(this->use_timeout)
   {
-    this->timeout_value=time_s;
-    this->action_timeout.start(ros::Duration(time_s));
+    if(time_s>0.0)
+    {
+      this->timeout_value=time_s;
+      this->action_timeout.start(ros::Duration(time_s));
+    }
+    else
+    {
+      this->action_access.exit();
+      throw CModuleException(_HERE_,"Invalid timeout value",this->name);
+    }
   }
   this->action_access.exit();
 }
@@ -238,10 +252,18 @@ 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)
+  if(this->use_timeout)
   {
-    this->action_timeout.start(ros::Duration(time_s));
-    this->timeout_value=time_s;
+    if(time_s>0.0)
+    {
+      this->action_timeout.start(ros::Duration(time_s));
+      this->timeout_value=time_s;
+    }
+    else
+    {
+      this->action_access.exit();
+      throw CModuleException(_HERE_,"Invalid timeout value",this->name);
+    }
   }
   this->action_access.exit();
 }

include/iri_ros_tools/module_dyn_reconf.h

+#ifndef _MODULE_DYN_RECONF_H
+#define _MODULE_DYN_RECONF_H
+
+#include <dynamic_reconfigure/Reconfigure.h>
+#include <iri_ros_tools/module_service.h>
+
+class CModuleDynReconf : protected CModuleService<dynamic_reconfigure::Reconfigure>
+{
+  private:
+    std::string name;
+    bool bool_value;
+    int int_value;
+    std::string string_value;
+    double double_value;
+  protected:
+    bool check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg);
+  public:
+    CModuleDynReconf(const std::string &name,const std::string &name_space=std::string(""));
+    bool set_parameter(const std::string &name,bool value);
+    bool set_parameter(const std::string &name,int value);
+    bool set_parameter(const std::string &name,std::string &value);
+    bool set_parameter(const std::string &name,double value);
+    ~CModuleDynReconf();
+};
+
+CModuleDynReconf::CModuleDynReconf(const std::string &name,const std::string &name_space) : CModuleService(name,name_space)
+{
+  this->set_max_num_retries(1);
+  this->set_call_check_function(boost::bind(&CModuleDynReconf::check_dyn_reconf,this,_1));
+}
+bool CModuleDynReconf::check_dyn_reconf(dynamic_reconfigure::Reconfigure &msg)
+{
+  unsigned int i=0;
+
+  for(i=0;i<msg.response.config.bools.size();i++)
+  {
+    if(msg.response.config.bools[i].name==this->name)
+    {
+      if(msg.response.config.bools[i].value!=this->bool_value)
+      {
+        return false;
+      }
+    }
+  }
+  for(i=0;i<msg.response.config.ints.size();i++)
+  {
+    if(msg.response.config.ints[i].name==this->name)
+    {
+      if(msg.response.config.ints[i].value!=this->int_value)
+      {
+        return false;
+      }
+    }
+  }
+  for(i=0;i<msg.response.config.strs.size();i++)
+  {
+    if(msg.response.config.strs[i].name==this->name)
+    {
+      if(msg.response.config.strs[i].value!=this->string_value)
+      {
+        return false;
+      }
+    }
+  }
+  for(i=0;i<msg.response.config.doubles.size();i++)
+  {
+    if(msg.response.config.doubles[i].name==this->name)
+    {
+      if(msg.response.config.doubles[i].value!=this->double_value)
+      {
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,bool value)
+{
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+  ROS_DEBUG_STREAM("CModuleDynReconf::set boolean parameter " << name << " to " << value);
+
+  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;
+  this->name=name;
+  this->bool_value=value;
+  switch(this->call(set_params_srv))
+  {
+    case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name()); 
+                          return true;
+                          break;
+    case ACT_SRV_PENDING:
+    case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+                       return false;
+                       break;
+  }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,int value)
+{
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+  ROS_DEBUG_STREAM("CModule::set integer parameter " << name << " to " << value);
+
+  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;
+  this->name=name;
+  this->int_value=value;
+  switch(this->call(set_params_srv))
+  {
+    case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name()); 
+                          return true;
+                          break;
+    case ACT_SRV_PENDING:
+    case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+                       return false;
+                       break;
+  }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,std::string &value)
+{
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModuleDynReconf::set_parameter: Sending new request on service " << this->get_name());
+  ROS_DEBUG_STREAM("CModuleDynReconf::set string parameter " << name << " to " << value);
+
+  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;
+  this->name=name;
+  this->string_value=value;
+  switch(this->call(set_params_srv))
+  {
+    case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name()); 
+                          return true;
+                          break;
+    case ACT_SRV_PENDING:
+    case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+                       return false;
+                       break;
+  }
+}
+
+bool CModuleDynReconf::set_parameter(const std::string &name,double value)
+{
+  dynamic_reconfigure::Reconfigure set_params_srv;
+
+  ROS_DEBUG_STREAM("CModule::set_parameter: Sending new request on service " << this->get_name());
+  ROS_DEBUG_STREAM("CModule::set double parameter " << name << " to " << value);
+
+  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;
+  this->name=name;
+  this->double_value=value;
+  switch(this->call(set_params_srv))
+  {
+    case ACT_SRV_SUCCESS: ROS_DEBUG_STREAM("CModule::set_parameters: Request successfull on server: " << this->get_name()); 
+                          return true;
+                          break;
+    case ACT_SRV_PENDING:
+    case ACT_SRV_FAIL: ROS_ERROR_STREAM("CModuleDynReconf::set_parameters: Request failed on server: " << this->get_name());
+                       return false;
+                       break;
+  }
+}
+
+CModuleDynReconf::~CModuleDynReconf()
+{
+
+}
+
+#endif

include/iri_ros_tools/module_exceptions.h

+// Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Author Sergi Hernandez  (shernand@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of iriutils
+// iriutils is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef MODULE_EXCEPTIONS_H
+#define MODULE_EXCEPTIONS_H
+
+#include "exceptions.h"
+#include <string>
+
+const std::string module_exception_msg="[CModule class] - ";
+
+class CModuleException : public CException
+{
+  public:
+    CModuleException(const std::string& where,const std::string& error_msg,const std::string& module_name) : CException(where,module_exception_msg)
+    {
+      this->error_msg+=error_msg;
+      this->error_msg+=" - ";
+      this->error_msg+=module_name;
+    }
+};
+
+#endif