Skip to content
Snippets Groups Projects
Commit 0d92abca authored by Fernando Herrero's avatar Fernando Herrero
Browse files

iri_image_local_binarization: publish image with input image header/stamp

parent 4c777907
No related branches found
No related tags found
No related merge requests found
...@@ -47,10 +47,14 @@ void ImageLocalBinarizationAlgNode::mainNodeThread(void) ...@@ -47,10 +47,14 @@ void ImageLocalBinarizationAlgNode::mainNodeThread(void)
/* [subscriber callbacks] */ /* [subscriber callbacks] */
void ImageLocalBinarizationAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info) void ImageLocalBinarizationAlgNode::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info)
{ {
sensor_msgs::ImagePtr my_img;
try{ try{
this->image_ = cv_bridge::toCvCopy(msg, "mono8"); 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->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){ }catch(cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what()); ROS_ERROR("cv_bridge exception: %s", e.what());
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment