diff --git a/src/image_local_binarization_alg_node.cpp b/src/image_local_binarization_alg_node.cpp index 72607ac9264fdce4d1eec4790a59adc6d9e5e5e4..25451c7773819642beb10764df6050f99acd931c 100644 --- a/src/image_local_binarization_alg_node.cpp +++ b/src/image_local_binarization_alg_node.cpp @@ -47,10 +47,14 @@ void ImageLocalBinarizationAlgNode::mainNodeThread(void) /* [subscriber callbacks] */ void ImageLocalBinarizationAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info) { + sensor_msgs::ImagePtr my_img; + try{ 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_)); - this->image_out_publisher_.publish(this->image_->toImageMsg(),info); + my_img = this->image_->toImageMsg(); + my_img->header.stamp = msg->header.stamp; + this->image_out_publisher_.publish(my_img,info); }catch(cv_bridge::Exception& e){ ROS_ERROR("cv_bridge exception: %s", e.what()); }