updating iri_base_algorithm with dynamic_reconfigure

CMakeLists.txt

 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
+set(PROJECT_NAME iri_base_algorithm)
+
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 #  Debug          : w/ debug symbols, w/o optimization
@@ -16,15 +18,20 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
+# edit the following line to add all the source code files of the library
+# SET(sources ./src/iri_base_driver.cpp)
+# edit the following line to add all the header files of the library
+# SET(headers ./include/iri_base_algorithm/iri_base_algorithm.h)
+
+# FIND_PACKAGE(iriutils REQUIRED)
+
+# INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${headers})
 
 #common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+# rosbuild_add_library(${PROJECT_NAME} SHARED ${sources})
 #target_link_libraries(${PROJECT_NAME} another_library)
 #rosbuild_add_boost_directories()
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+# target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+

include/iri_base_algorithm/iri_base_algorithm.h

@@ -2,11 +2,14 @@
 #define _IRI_BASE_ALGORITHM_H
 
 #include <ros/ros.h>
+// #include <boost/bind.hpp>
+#include <dynamic_reconfigure/server.h>
 
 // iri-utils thread server
 #include "threadserver.h"
 #include "exceptions.h"
 
+//check me out
 // namespace algorithm_base
 // {
 
@@ -17,15 +20,23 @@
 template <class Algorithm>
 class IriBaseAlgorithm
 {
+  public:
+	//check me out
+    typedef typename Algorithm::Config Config;
+
   protected:
-    Algorithm alg;
-    CThreadServer *thread_server;
-    std::string main_thread_id;
+    Algorithm alg_;
+    CThreadServer *thread_server_;
+    std::string main_thread_id_;
 
     ros::NodeHandle node_handle_;
     ros::NodeHandle private_node_handle_;
 
-    ros::Rate loop_rate;
+	//check me out
+    dynamic_reconfigure::Server<Config> dsrv_;
+// 	dynamic_reconfigure::Server<Config>::CallbackType cb;
+
+    ros::Rate loop_rate_;
     static const unsigned int DEFAULT_RATE = 10; //[Hz]
 
   public:
@@ -33,11 +44,12 @@ class IriBaseAlgorithm
     ~IriBaseAlgorithm();
 
   protected:
-//     void addDiagnostics(void);
+    void reconfigureCallback(Config &config, uint32_t level);
+    void addDiagnostics(void);
 //     void addOpenedTests(void);
 //     void addStoppedTests(void);
 //     void addRunningTests(void);
-//     virtual void addNodeDiagnostics(void) = 0;
+    virtual void addNodeDiagnostics(void) = 0;
 //     virtual void addNodeOpenedTests(void) = 0;
 //     virtual void addNodeStoppedTests(void) = 0;
 //     virtual void addNodeRunningTests(void) = 0;
@@ -46,20 +58,45 @@ class IriBaseAlgorithm
 
 };
 
+//check me out
 template <class Algorithm>
-IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), loop_rate(DEFAULT_RATE)
+IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), dsrv_(ros::NodeHandle("~")), loop_rate_(DEFAULT_RATE)
 {
   // create the status thread
-  this->thread_server=CThreadServer::instance();
-  this->main_thread_id="main_thread";
+  this->thread_server_=CThreadServer::instance();
+  this->main_thread_id_="main_thread";
 
-  this->thread_server->create_thread(this->main_thread_id);
-  this->thread_server->attach_thread(this->main_thread_id, this->mainThread, this);
-  this->thread_server->start_thread( this->main_thread_id);
+  this->thread_server_->create_thread(this->main_thread_id_);
+  this->thread_server_->attach_thread(this->main_thread_id_, this->mainThread, this);
+  this->thread_server_->start_thread( this->main_thread_id_);
   
+  //check me out
+//   dynamic_reconfigure::Server<Config>::CallbackType cb;
+  // = boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2);
+  dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
   // initialize class attributes
 }
 
+template <class Algorithm>
+IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
+{
+  this->thread_server_->kill_thread(this->main_thread_id_);
+}
+
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
+{
+  //check me out
+  alg_.config_ = config;
+}
+
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::addDiagnostics(void)
+{
+  addNodeDiagnostics();
+}
+
 template <class Algorithm>
 void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
 {
@@ -68,27 +105,28 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
   while(ros::ok())
   {
       iriNode->mainNodeThread();
-      iriNode->loop_rate.sleep();
+      iriNode->loop_rate_.sleep();
   }
 
   pthread_exit(NULL);
 }
 
-template <class Algorithm>
-IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
+namespace algorithm_base
 {
-  this->thread_server->kill_thread(this->main_thread_id);
-}
 
 template <class AlgImplTempl>
 int main(int argc, char **argv, std::string name)
 {
   ros::init(argc, argv, name);
 
+  //check me out
   AlgImplTempl algImpl;
 
-  return ros::spin();
+  //check me out
+  ros::spin();
+
+  return 0;
 }
 
-// }
+}
 #endif

manifest.xml

@@ -13,8 +13,7 @@
   <depend package="dynamic_reconfigure"/>
 
   <export>
-    <cpp cflags="-I${prefix}/include"
-         lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_base_algorithm"/>
+    <cpp cflags="-I${prefix}/include"/>
   </export>
 
 </package>