Process image outside my callback function?
I at moment trying to write A generic codepiece for extracting an image from a callback function, such that the processing of it is separated from the callback itself.
The problem for me seems indeed very trivial, but I am having issue structuring and i am quite sure i understand how to properly do it.
I found this post http://answers.ros.org/question/53234...
which seem to have concern the same issue, and based on the responds i tried generate A generic code piece.
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
static const std::string OPENCV_WINDOW_l = "Image window_l";
static const std::string OPENCV_WINDOW_r = "Image window_r";
char capture;
class ImageConverter
{
protected:
ros::NodeHandle nh_;
image_transport::ImageTransport it_l;
image_transport::ImageTransport it_r;
image_transport::Subscriber image_sub_l;
image_transport::Subscriber image_sub_r;
image_transport::Publisher image_pub_l;
image_transport::Publisher image_pub_r;
public:
sensor_msgs::ImageConstPtr imageIn_;
cv_bridge::CvImage imageOut_;
ImageConverter(ros::NodeHandle &nh_ , ros::NodeHandle &private_)
: it_l(nh_), it_r(nh_)
{
// Subscrive to input video feed and publish output video feed
image_sub_l = it_l.subscribe("/stereo_camera/left/image_raw", 1,
&ImageConverter::imageCb_l, this);
image_sub_r = it_r.subscribe("/stereo_camera/right/image_raw", 1,
&ImageConverter::imageCb_r, this);
image_pub_l = it_l.advertise("/stereo_camera/left/image_raw", 1);
image_pub_r = it_r.advertise("/stereo_camera/right/image_raw", 1);
}
~ImageConverter()
{
cv::destroyWindow(OPENCV_WINDOW_l);
}
void imageCb_l(const sensor_msgs::ImageConstPtr& msg)
{
imageIn_ = msg;
}
void imageCb_r(const sensor_msgs::ImageConstPtr& msg)
{
std::cout << "to be added" << std::endl;
}
void process()
{
sensor_msgs::ImageConstPtr imageIn = imageIn_;
//Check if new image has arrived
if(imageOut_.header.seq == imageIn.header.seq)
return;
//Replace old image with new one
imageOut_ = imageIn;
//Perform Image processing.
//publish imageOut_.toMsg();
}
virtual void spin()
{
ros::Rate rate(30);
while(ros::ok())
{
spinOnce();
rate.sleep();
}
}
virtual void spinOnce()
{
process();
spinOnce();
}
};
class NodeWithGui : public ImageConverter
{
virtual void spinOnce()
{
ImageConverter::spinOnce();
imshow("test",ImageConverter::imageOut_.image);
waitKey(1);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "image_converter");
ros::NodeHandle nh;
ros::NodeHandle nhPrivate("~");
//ImageConverter ic;
ImageConverter* node = 0;
if(ros::param::get<bool>("~use_gui"))
{
node = new = NodeWithGui(nh,nhPrivate);
}
else
{
node = new ImageConverter(nh,nhPrivate);
}
//ros::spin();
return 0;
}
My intention with this code piece it to seperate the image processing with the image extraction.
The intention was the inside the process()
should i be able to perform my processing, without interfering with my callback function.
First of all will the code not compile due to error with non existing member functions (imageIn.header.seq), secondly does the code do what i want.. I am having some problems understanding the example, and does it even operate as i want it?
Error code:
/main.cpp: In member function ‘void ImageConverter::process()’:
/main.cpp:61:42: error: ‘sensor_msgs::ImageConstPtr’ has no member named ‘header’
if(imageOut_.header.seq == imageIn.header.seq)
^
/main.cpp:64:17: error: no match for ‘operator=’ (operand types are ‘cv_bridge::CvImage’ and ‘sensor_msgs::ImageConstPtr {aka boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >}’)
imageOut_ = imageIn;
^
/main.cpp:64:17: note: candidate is:
In file included from /main.cpp ...
let's break the problem to some steps, so, first, can you please post more about the compilation output (what does it say exactly) ?
Error message included..