Add separate image/info subscribers

include/image_local_binarization_alg_node.h

@@ -50,12 +50,29 @@ class ImageLocalBinarizationAlgNode : public algorithm_base::IriBaseAlgorithm<Im
     sensor_msgs::Image 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);
+    */
+
+    ros::Subscriber camera_info_subscriber_;
+    void camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg);
+    sensor_msgs::CameraInfo cam_info;
+    bool cam_info_received;
+
+    ros::Subscriber image_subscriber_;
+    void image_callback(const sensor_msgs::Image::ConstPtr& msg);
+    pthread_mutex_t image_mutex_;
+    void image_mutex_enter(void);
+    void image_mutex_exit(void);
+    //sensor_msgs::Image image_msg;
+
 
     // [service attributes]
 

src/image_local_binarization_alg_node.cpp

 #include "image_local_binarization_alg_node.h"
 
 ImageLocalBinarizationAlgNode::ImageLocalBinarizationAlgNode(void) :
-  algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm>(),it(this->public_node_handle_)
+  algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm>(),
+  it(this->public_node_handle_)
 {
   //init class attributes if necessary
   this->setRate(1);//in [Hz]
@@ -11,9 +12,15 @@ ImageLocalBinarizationAlgNode::ImageLocalBinarizationAlgNode(void) :
   
   int queue_size=10;
   this->private_node_handle_.getParam("queue_size", queue_size);
+  
   // [init subscribers]
-  this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", queue_size, &ImageLocalBinarizationAlgNode::image_in_callback,this);
-  pthread_mutex_init(&this->image_in_mutex_,NULL);
+  this->camera_info_subscriber_ = this->private_node_handle_.subscribe("image_in/camera_info", 1, &ImageLocalBinarizationAlgNode::camera_info_callback, this);
+
+  this->image_subscriber_ = this->private_node_handle_.subscribe("image_in/image_raw", 1, &ImageLocalBinarizationAlgNode::image_callback, this);
+  pthread_mutex_init(&this->image_mutex_,NULL);
+
+  //this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", queue_size, &ImageLocalBinarizationAlgNode::image_in_callback,this);
+  //pthread_mutex_init(&this->image_in_mutex_,NULL);
 
   
   // [init services]
@@ -26,12 +33,14 @@ ImageLocalBinarizationAlgNode::ImageLocalBinarizationAlgNode(void) :
 
   this->window_size_ = 3;
   this->k_ = 0.06;
+  this->cam_info_received=false;
 }
 
 ImageLocalBinarizationAlgNode::~ImageLocalBinarizationAlgNode(void)
 {
   // [free dynamic memory]
-  pthread_mutex_destroy(&this->image_in_mutex_);
+  //pthread_mutex_destroy(&this->image_in_mutex_);
+  pthread_mutex_destroy(&this->image_mutex_);
 }
 
 void ImageLocalBinarizationAlgNode::mainNodeThread(void)
@@ -47,6 +56,61 @@ void ImageLocalBinarizationAlgNode::mainNodeThread(void)
 }
 
 /*  [subscriber callbacks] */
+
+void ImageLocalBinarizationAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::ConstPtr& msg) 
+{
+  ROS_INFO("ImageLocalBinarizationAlgNode::camera_info_callback: message received. Unsubscribing");
+  this->alg_.lock();
+  this->cam_info = *msg;
+  this->cam_info_received=true;
+  this->camera_info_subscriber_.shutdown();
+  this->alg_.unlock();
+}
+
+void ImageLocalBinarizationAlgNode::image_callback(const sensor_msgs::Image::ConstPtr& msg) 
+{ 
+  ROS_INFO("ImageLocalBinarizationAlgNode::image_callback: message received.");
+  this->alg_.lock();
+  if(this->cam_info_received)
+  {
+    try
+    {
+      sensor_msgs::ImagePtr my_img;
+      //sensor_msgs::CameraInfoPtr my_info;
+      //*my_info= this->cam_info;
+
+      ROS_INFO("ImageLocalBinarizationAlgNode::image_callback: message received (2)");
+
+
+      this->image_ = cv_bridge::toCvCopy(msg, "mono8");
+      this->alg_.localThreshold(this->image_->image, this->image_->image, this->k_,(this->window_size_ % 2 == 0 ? this->window_size_ + 1: this->window_size_));
+      my_img = this->image_->toImageMsg();
+      my_img->header.stamp = msg->header.stamp;
+      this->image_out_publisher_.publish(*my_img,this->cam_info);
+    }
+    catch(cv_bridge::Exception& e)
+    {
+      ROS_ERROR("cv_bridge exception: %s", e.what());
+    }
+  }
+  else
+  {
+    ROS_INFO("ImageLocalBinarizationAlgNode::image_callback: waiting for camera_info msg");
+  }
+  this->alg_.unlock();
+} 
+
+void ImageLocalBinarizationAlgNode::image_mutex_enter(void) 
+{ 
+  pthread_mutex_lock(&this->image_mutex_); 
+} 
+
+void ImageLocalBinarizationAlgNode::image_mutex_exit(void) 
+{ 
+  pthread_mutex_unlock(&this->image_mutex_); 
+}
+
+/*
 void ImageLocalBinarizationAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info) 
 { 
   sensor_msgs::ImagePtr my_img;
@@ -70,7 +134,7 @@ void ImageLocalBinarizationAlgNode::image_in_callback(const sensor_msgs::Image::
   //unlock previously blocked shared variables 
   //this->alg_.unlock(); 
   //this->image_in_mutex_exit(); 
-} 
+}
 
 void ImageLocalBinarizationAlgNode::image_in_mutex_enter(void) 
 { 
@@ -81,6 +145,7 @@ void ImageLocalBinarizationAlgNode::image_in_mutex_exit(void)
 { 
   pthread_mutex_unlock(&this->image_in_mutex_); 
 }
+*/
 
 /*  [service callbacks] */