Modified the nodelet class to use the node class instead re-implementing features.

CMakeLists.txt

@@ -88,7 +88,7 @@ include_directories(${iriutils_INCLUDE_DIR})
 ## Declare a cpp executable
 add_executable(${PROJECT_NAME} src/blob_detector_alg.cpp src/blob_detector_alg_node.cpp)
 
-add_library(${PROJECT_NAME}_nodelet src/blob_detector_alg.cpp src/blob_detector_alg_nodelet.cpp)
+add_library(${PROJECT_NAME}_nodelet src/blob_detector_alg.cpp src/blob_detector_alg_node.cpp src/blob_detector_alg_nodelet.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries

include/blob_detector_alg_node.h

@@ -94,7 +94,7 @@ class BlobDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<BlobDetector
     * This constructor initializes specific class attributes and all ROS
     * communications variables to enable message exchange.
     */
-    BlobDetectorAlgNode(void);
+    BlobDetectorAlgNode(const ros::NodeHandle &nh = ros::NodeHandle("~"));
 
    /**
     * \brief Destructor

include/blob_detector_alg_nodelet.h

@@ -26,116 +26,15 @@
 #define _blob_detector_alg_nodelet_h_
 
 #include "nodelet/nodelet.h"
+#include "blob_detector_alg_node.h"
 
-#include <iri_base_algorithm/iri_base_algorithm.h>
-#include "blob_detector_alg.h"
-#include "cv_bridge/cv_bridge.h"
-
-// [publisher subscriber headers]
-#include <sensor_msgs/Image.h>
-#include <image_transport/image_transport.h>
-
-// [service client headers]
-
-// [action server client headers]
-
-#include "eventserver.h"
 #include "threadserver.h"
 
-/**
- * \brief IRI ROS Specific Algorithm Class
- *
- */
-class BlobDetectorAlgNodelet : public algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>
-{
-  private:
-    // [publisher attributes]
-    image_transport::CameraPublisher image_out_publisher_;
-    sensor_msgs::ImagePtr image_out_Image_msg_;
-
-    // [subscriber attributes]
-    image_transport::ImageTransport it;
-    image_transport::CameraSubscriber image_in_subscriber_;
-    void image_in_callback(const sensor_msgs::Image::ConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& info);
-    pthread_mutex_t image_in_mutex_;
-    void image_in_mutex_enter(void);
-    void image_in_mutex_exit(void);
-
-    // [service attributes]
-
-    // [client attributes]
-
-    // [action server attributes]
-
-    // [action client attributes]
-
-    cv_bridge::CvImageConstPtr image_;
-    cv_bridge::CvImagePtr image_out_;
-
-  public:
-   /**
-    * \brief Constructor
-    * 
-    * This constructor initializes specific class attributes and all ROS
-    * communications variables to enable message exchange.
-    */
-    BlobDetectorAlgNodelet(ros::NodeHandle private_nh);
-
-   /**
-    * \brief Destructor
-    * 
-    * This destructor frees all necessary dynamic memory allocated within this
-    * this class.
-    */
-    ~BlobDetectorAlgNodelet(void);
-
-  protected:
-   /**
-    * \brief main node thread
-    *
-    * This is the main thread node function. Code written here will be executed
-    * in every node loop while the algorithm is on running state. Loop frequency 
-    * can be tuned by modifying loop_rate attribute.
-    *
-    * Here data related to the process loop or to ROS topics (mainly data structs
-    * related to the MSG and SRV files) must be updated. ROS publisher objects 
-    * must publish their data in this process. ROS client servers may also
-    * request data to the corresponding server topics.
-    */
-    void mainNodeThread(void);
-
-   /**
-    * \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.
-    */
-    void node_config_update(Config &config, uint32_t level);
-
-   /**
-    * \brief node add diagnostics
-    *
-    * In this abstract function additional ROS diagnostics applied to the 
-    * specific algorithms may be added.
-    */
-    void addNodeDiagnostics(void);
-
-    // [diagnostic functions]
-    
-    // [test functions]
-};
-
 class BlobDetectorNodelet : public nodelet::Nodelet
 {
   private:
     virtual void onInit();// initialization function
-    BlobDetectorAlgNodelet *dev;
+    BlobDetectorAlgNode *dev;
     // thread attributes
     CThreadServer *thread_server;
     std::string spin_thread_id;

src/blob_detector_alg_node.cpp

 #include "blob_detector_alg_node.h"
 
-BlobDetectorAlgNode::BlobDetectorAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(),it(this->public_node_handle_)
+BlobDetectorAlgNode::BlobDetectorAlgNode(const ros::NodeHandle &nh) :
+  algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(nh),it(nh)
 {
   //init class attributes if necessary
   this->loop_rate_ = 1;//in [Hz]

src/blob_detector_alg_nodelet.cpp

-#include "blob_detector_alg_nodelet.h"
-
-BlobDetectorAlgNodelet::BlobDetectorAlgNodelet(ros::NodeHandle private_nh) :
-  algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(),it(private_nh)
-{
-  //init class attributes if necessary
-  this->loop_rate_ = 1;//in [Hz]
-
-  // [init publishers]
-  this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
-  
-  // [init subscribers]
-  this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 1, &BlobDetectorAlgNodelet::image_in_callback,this);
-  pthread_mutex_init(&this->image_in_mutex_,NULL);
-
-  this->image_out_=cv_bridge::CvImagePtr(new cv_bridge::CvImage);
-  this->image_=cv_bridge::CvImagePtr(new cv_bridge::CvImage);
-  
-  // [init services]
-  
-  // [init clients]
-  
-  // [init action servers]
-  
-  // [init action clients]
-}
-
-BlobDetectorAlgNodelet::~BlobDetectorAlgNodelet(void)
-{
-  // [free dynamic memory]
-  pthread_mutex_destroy(&this->image_in_mutex_);
-}
-
-void BlobDetectorAlgNodelet::mainNodeThread(void)
-{
-  // [fill msg structures]
-  //this->image_out_Image_msg_.data = my_var;
-  
-  // [fill srv structure and make request to the server]
-  
-  // [fill action structure and make request to the action server]
-
-  // [publish messages]
-}
-
-/*  [subscriber callbacks] */
-void BlobDetectorAlgNodelet::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info) 
-{ 
-  try{
-    this->image_ = cv_bridge::toCvShare(msg, "rgb8");
-    this->image_out_->encoding=this->image_->encoding;
-    this->image_out_->header=this->image_->header;
-//    this->alg_.blob_detector(this->image_->image,this->image_out_->image);
-    this->image_out_publisher_.publish(this->image_out_->toImageMsg(),info);
-  }catch(cv_bridge::Exception& e){
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-  }
-
-  //use appropiate mutex to shared variables if necessary 
-  //this->alg_.lock(); 
-  //this->image_in_mutex_enter(); 
-
-  //std::cout << msg->data << std::endl; 
-
-  //unlock previously blocked shared variables 
-  //this->alg_.unlock(); 
-  //this->image_in_mutex_exit(); 
-} 
-
-void BlobDetectorAlgNodelet::image_in_mutex_enter(void) 
-{ 
-  pthread_mutex_lock(&this->image_in_mutex_); 
-} 
-
-void BlobDetectorAlgNodelet::image_in_mutex_exit(void) 
-{ 
-  pthread_mutex_unlock(&this->image_in_mutex_); 
-}
-
-/*  [service callbacks] */
-
-/*  [action callbacks] */
-
-/*  [action requests] */
-
-void BlobDetectorAlgNodelet::node_config_update(Config &config, uint32_t level)
-{
-}
-
-void BlobDetectorAlgNodelet::addNodeDiagnostics(void)
-{
-}
-
 #include "blob_detector_alg_nodelet.h"
 #include <pluginlib/class_list_macros.h>
 
@@ -101,10 +8,11 @@ BlobDetectorNodelet::BlobDetectorNodelet()
 
 void BlobDetectorNodelet::onInit()
 {
-  this->dev=new BlobDetectorAlgNodelet(getPrivateNodeHandle());
+  this->dev=new BlobDetectorAlgNode(this->getPrivateNodeHandle());
   // initialize the thread
+  std::cout << this->getName() << "," << this->getPrivateNodeHandle().getNamespace() << std::endl;
   this->thread_server=CThreadServer::instance();
-  this->spin_thread_id=getName() + "_image_local_bin_nodelet_spin";
+  this->spin_thread_id=getName() + "_blob_detector_nodelet_spin";
   std::cout << this->spin_thread_id << std::endl;
   this->thread_server->create_thread(this->spin_thread_id);
   this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);