diff --git a/include/image_local_binarization_alg_node.h b/include/image_local_binarization_alg_node.h index 8deaa8c93ee1784af6dc06d95de81a8b1e4497fa..62d48bb63bc0df2495f0d5193ecae8a9297483ca 100644 --- a/include/image_local_binarization_alg_node.h +++ b/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] diff --git a/src/image_local_binarization_alg_node.cpp b/src/image_local_binarization_alg_node.cpp index 56c22bfb3c2c11196b86b5b40896da204ff95c1c..af729c23791669d96ac507f070fcdb4f5eab50b8 100644 --- a/src/image_local_binarization_alg_node.cpp +++ b/src/image_local_binarization_alg_node.cpp @@ -1,7 +1,8 @@ #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] */