modifying iri_base_algorithm to provide diagnostics apart from dynamic_reconfigure

include/iri_base_algorithm/iri_base_algorithm.h

@@ -2,27 +2,62 @@
 #define _IRI_BASE_ALGORITHM_H
 
 #include <ros/ros.h>
-// #include <boost/bind.hpp>
-#include <dynamic_reconfigure/server.h>
+#include <signal.h>
 
 // iri-utils thread server
 #include "threadserver.h"
 #include "exceptions.h"
 
 //check me out
+// #define INTERNAL_SPIN
+// #define DYNAMIC_CFG
+#define DIAGNOSTICS
+
+#ifdef INTERNAL_SPIN
+  #include <boost/thread.hpp>
+  #include <boost/bind.hpp>
+#endif
+
+#ifdef DYNAMIC_CFG
+#include <dynamic_reconfigure/server.h>
+#endif
+
+#ifdef DIAGNOSTICS
+#include <diagnostic_updater/diagnostic_updater.h>
+#endif
+
+// //check me out
 // namespace algorithm_base
 // {
 
+//check me out
+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_++;
+  }
+};
+
 /**
  * \brief IRI ROS Base Node Class
  *
  */
 template <class Algorithm>
-class IriBaseAlgorithm
+class IriBaseAlgorithm : public AbstractAlgorithmNode
 {
+#ifdef DYNAMIC_CFG
   public:
 	//check me out
     typedef typename Algorithm::Config Config;
+#endif
 
   protected:
     Algorithm alg_;
@@ -33,8 +68,14 @@ class IriBaseAlgorithm
     ros::NodeHandle private_node_handle_;
 
 	//check me out
+	#ifdef DYNAMIC_CFG
     dynamic_reconfigure::Server<Config> dsrv_;
 // 	dynamic_reconfigure::Server<Config>::CallbackType cb;
+    #endif
+
+    #ifdef DIAGNOSTICS
+    diagnostic_updater::Updater diagnostic_;
+    #endif
 
     ros::Rate loop_rate_;
     static const unsigned int DEFAULT_RATE = 10; //[Hz]
@@ -43,74 +84,155 @@ class IriBaseAlgorithm
     IriBaseAlgorithm();
     ~IriBaseAlgorithm();
 
+#ifdef INTERNAL_SPIN
+	int spin(void);
+
+  private:
+	boost::shared_ptr<boost::thread> ros_thread_;
+#endif
+
   protected:
+#ifdef DYNAMIC_CFG
     void reconfigureCallback(Config &config, uint32_t level);
+#endif
+#ifdef DIAGNOSTICS
     void addDiagnostics(void);
+	virtual void addNodeDiagnostics(void) = 0;
+#endif
 //     void addOpenedTests(void);
 //     void addStoppedTests(void);
 //     void addRunningTests(void);
-    virtual void addNodeDiagnostics(void) = 0;
 //     virtual void addNodeOpenedTests(void) = 0;
 //     virtual void addNodeStoppedTests(void) = 0;
 //     virtual void addNodeRunningTests(void) = 0;
     static void *mainThread(void *param);
     virtual void mainNodeThread(void) = 0;
-
 };
 
 //check me out
 template <class Algorithm>
-IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), dsrv_(ros::NodeHandle("~")), loop_rate_(DEFAULT_RATE)
+IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : 
+  private_node_handle_("~"), 
+#ifdef DYNAMIC_CFG
+  dsrv_(ros::NodeHandle("~")),
+#endif
+#ifdef DIAGNOSTICS
+  diagnostic_(),
+#endif
+  loop_rate_(DEFAULT_RATE)
 {
   // create the status thread
   this->thread_server_=CThreadServer::instance();
   this->main_thread_id_="main_thread";
 
+  //check me out
   this->thread_server_->create_thread(this->main_thread_id_);
   this->thread_server_->attach_thread(this->main_thread_id_, this->mainThread, this);
+#ifndef INTERNAL_SPIN
   this->thread_server_->start_thread( this->main_thread_id_);
+#endif
   
   //check me out
+#ifdef DYNAMIC_CFG
 //   dynamic_reconfigure::Server<Config>::CallbackType cb;
   // = boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2);
   dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
-
+#endif
   // initialize class attributes
 }
 
 template <class Algorithm>
 IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
 {
+  std::cout << "I'm leaving for good" << std::endl << std::endl;
   this->thread_server_->kill_thread(this->main_thread_id_);
 }
 
+#ifdef DYNAMIC_CFG
 template <class Algorithm>
 void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
 {
   //check me out
-  alg_.config_ = config;
+  this->alg_.config_ = config;
 }
+#endif
 
+#ifdef DIAGNOSTICS
 template <class Algorithm>
 void IriBaseAlgorithm<Algorithm>::addDiagnostics(void)
 {
   addNodeDiagnostics();
 }
+#endif
 
 template <class Algorithm>
 void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
 {
   IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param;
 
-  while(ros::ok())
+  //check me out
+#ifndef INTERNAL_SPIN
+
+  #ifdef DIAGNOSTICS
+  //initialize diagnostics
+  iriNode->diagnostic_.setHardwareID("none");
+  iriNode->addDiagnostics();
+  #endif
+
+#endif
+
+  while(ros::ok() && !ctrl_c_hit_count_)
   {
+	  //run node stuff
       iriNode->mainNodeThread();
+
+	  #ifdef DIAGNOSTICS
+	  iriNode->diagnostic_.update();
+	  #endif
+	
+	  //sleep remainder time
       iriNode->loop_rate_.sleep();
   }
 
   pthread_exit(NULL);
 }
 
+#ifdef INTERNAL_SPIN
+template <class Algorithm>
+int IriBaseAlgorithm<Algorithm>::spin(void)
+{
+  #ifdef DIAGNOSTICS
+  //initialize diagnostics
+  this->diagnostic_.setHardwareID("none");
+  this->addDiagnostics();
+  #endif
+  
+  //launch ros spin
+  this->ros_thread_.reset( new boost::thread(boost::bind(&ros::spin)) );
+  assert(ros_thread_);
+  
+  //launch node thread
+  this->thread_server_->start_thread( this->main_thread_id_);
+  
+  while(ros::ok()) && !ctrl_c_hit_count_)
+  {
+	#ifdef DIAGNOSTICS
+	this->diagnostic_.update();
+	#endif
+  }
+
+  //stop ros
+  ros::shutdown();
+
+  //kill ros thread
+  this->ros_thread_.reset();
+
+  return 0;
+}
+#endif
+
+int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
+
 namespace algorithm_base
 {
 
@@ -118,12 +240,17 @@ template <class AlgImplTempl>
 int main(int argc, char **argv, std::string name)
 {
   ros::init(argc, argv, name);
-
+  
   //check me out
-  AlgImplTempl algImpl;
+  signal(SIGHUP, &AbstractAlgorithmNode::hupCalled);
 
   //check me out
+  AlgImplTempl algImpl;
+#ifdef INTERNAL_SPIN
+  algImpl.spin();
+#else
   ros::spin();
+#endif
 
   return 0;
 }

manifest.xml

@@ -11,6 +11,7 @@
 
   <depend package="roscpp"/>
   <depend package="dynamic_reconfigure"/>
+  <depend package="diagnostic_updater"/>
 
   <export>
     <cpp cflags="-I${prefix}/include"/>