merge from iri_base_algorithm5

CMakeLists.txt

@@ -19,9 +19,9 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
 # edit the following line to add all the source code files of the library
-# SET(sources ./src/iri_base_driver.cpp)
+# SET(sources ./src/iri_base_algorithm.cpp)
 # edit the following line to add all the header files of the library
-# SET(headers ./include/iri_base_algorithm/iri_base_algorithm.h)
+# SET(headers ./include/iri_base_algorithm.h)
 
 # FIND_PACKAGE(iriutils REQUIRED)
 
@@ -33,5 +33,5 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #rosbuild_add_boost_directories()
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)
-# target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+#target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
 

include/iri_base_algorithm/iri_base_algorithm.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author Joan Perez
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg 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 _IRI_BASE_ALGORITHM_H
 #define _IRI_BASE_ALGORITHM_H
 
@@ -7,6 +25,7 @@
 // iri-utils thread server
 #include "threadserver.h"
 #include "exceptions.h"
+#include "mutex.h"
 
 // boost thread includes for ROS::spin thread
 #include <boost/thread.hpp>
@@ -27,17 +46,18 @@ namespace algorithm_base
  */
 class AbstractAlgorithmNode
 {
-  static int ctrl_c_hit_count_;
-  
-  static void hupCalled(int sig)
-  {
-    ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
-  }
-  
-  static void sigCalled(int sig)
-  {
-    ctrl_c_hit_count_++;
-  }
+  protected:
+    static int ctrl_c_hit_count_;
+    
+    static void hupCalled(int sig)
+    {
+      ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
+    }
+    
+    static void sigCalled(int sig)
+    {
+      ctrl_c_hit_count_++;
+    }
 };
 
 /**
@@ -206,8 +226,22 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
     * \param level  integer referring the level in which the configuration
     *               has been changed.
     */
-    virtual void reconfigureCallback(Config &config, uint32_t level) = 0;
+    void reconfigureCallback(Config &config, uint32_t level);
 
+   /**
+    * \brief dynamic reconfigure server callback
+    * 
+    * This method is called whenever a new configuration is received through
+    * the dynamic reconfigure. The derivated generic algorithm class must 
+    * implement it.
+    *
+    * \param config an object with new configuration from all algorithm 
+    *               parameters defined in the config file.
+    * \param level  integer referring the level in which the configuration
+    *               has been changed.
+    */
+    virtual void node_config_update(Config &config, uint32_t level) = 0;
+   
    /**
     * \brief add diagnostics
     * 
@@ -261,45 +295,53 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() :
   public_node_handle_(ros::this_node::getName()),
   private_node_handle_("~"), 
   loop_rate_(DEFAULT_RATE),
-  dsrv_(private_node_handle_),
-  diagnostic_()
+  diagnostic_(),
+  dsrv_(private_node_handle_)
 {
+  ROS_DEBUG("IriBaseAlgorithm::Constructor");
   // create thread server instance
   this->thread_server_  = CThreadServer::instance();
-  
+
   // identify main thread
   this->main_thread_id_ = "main_thread";
 
   // create main thread and attach to server
   this->thread_server_->create_thread(this->main_thread_id_);
   this->thread_server_->attach_thread(this->main_thread_id_, this->mainThread, this);
-  
-  // assign callback to dynamic reconfigure server
-  dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
+//   // assign callback to dynamic reconfigure server
+//   dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
 }
 
 template <class Algorithm>
 IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
 {
-  std::cout << "I'm leaving for good" << std::endl << std::endl;
+  ROS_DEBUG("IriBaseAlgorithm::Destructor");
+
   this->thread_server_->kill_thread(this->main_thread_id_);
 }
 
-// template <class Algorithm>
-// void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
-// {
-//   this->alg_.config_ = config;
-// }
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
+{
+  ROS_DEBUG("IriBaseAlgorithm::reconfigureCallback");
+  this->node_config_update(config, level);
+  this->alg_.config_update(config, level);
+}
 
 template <class Algorithm>
 void IriBaseAlgorithm<Algorithm>::addDiagnostics(void)
 {
+  ROS_DEBUG("IriBaseAlgorithm::addDiagnostics");
   addNodeDiagnostics();
 }
 
 template <class Algorithm>
 void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
 {
+  ROS_DEBUG("IriBaseAlgorithm::mainThread");
+
   // retrieve base algorithm class
   IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param;
 
@@ -319,6 +361,8 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
 template <class Algorithm>
 int IriBaseAlgorithm<Algorithm>::spin(void)
 {
+  ROS_DEBUG("IriBaseAlgorithm::spin");
+
   // initialize diagnostics
   this->diagnostic_.setHardwareID("none");
   this->addDiagnostics();
@@ -326,7 +370,10 @@ int IriBaseAlgorithm<Algorithm>::spin(void)
   // launch ros spin in different thread
   this->ros_thread_.reset( new boost::thread(boost::bind(&ros::spin)) );
   assert(ros_thread_);
-  
+
+  // assign callback to dynamic reconfigure server
+  this->dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
   // launch node thread
   this->thread_server_->start_thread( this->main_thread_id_);
   
@@ -334,6 +381,8 @@ int IriBaseAlgorithm<Algorithm>::spin(void)
   {
     // update diagnostics
     this->diagnostic_.update();
+    
+    ros::WallDuration(0.1).sleep();
   }
 
   // stop ros
@@ -352,7 +401,7 @@ int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
  * \brief base main
  *
  * This main is common for all the algorithm nodes. The AlgImplTempl class
- * refers to an specific implementation of a generic algorithm.
+ * refers to an specific implementation derived from a generic algorithm.
  * 
  * First, ros::init is called providing the node name. Ctrl+C control is 
  * activated for sercure and safe exit. Finally, an AlgImplTempl object is
@@ -365,9 +414,11 @@ int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
 template <class AlgImplTempl>
 int main(int argc, char **argv, std::string node_name)
 {
+  ROS_DEBUG("IriBaseAlgorithm::%s Launched", node_name.c_str());
+
   // ROS initialization
   ros::init(argc, argv, node_name);
-  
+
   // allow Ctrl+C management
   signal(SIGHUP, &AbstractAlgorithmNode::hupCalled);