Robotics StackExchange | Archived questions

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/processing-an-image-outside-the-callback-function/

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:3:0:
/opt/ros/indigo/include/cv_bridge/cv_bridge.h:76:7: note: cv_bridge::CvImage& cv_bridge::CvImage::operator=(const cv_bridge::CvImage&)
 class CvImage
       ^
/opt/ros/indigo/include/cv_bridge/cv_bridge.h:76:7: note:   no known conversion for argument 1 from ‘sensor_msgs::ImageConstPtr {aka boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >}’ to ‘const cv_bridge::CvImage&’
/main.cpp: In member function ‘virtual void NodeWithGui::spinOnce()’:
/main.cpp:95:18: error: ‘waitKey’ was not declared in this scope
         waitKey(1);
                  ^
/main.cpp:95:18: note: suggested alternative:
In file included from /main.cpp:6:0:
/usr/include/opencv2/highgui/highgui.hpp:76:18: note:   ‘cv::waitKey’
 CV_EXPORTS_W int waitKey(int delay = 0);
                  ^
/main.cpp: In function ‘int main(int, char**)’:
/main.cpp:106:3: error: ‘Node’ was not declared in this scope
   Node* node = 0;
   ^
/main.cpp:106:9: error: ‘node’ was not declared in this scope
   Node* node = 0;
         ^
/main.cpp:108:22: error: expected primary-expression before ‘bool’
   if(ros::param::get<bool>("~use_gui"))
                      ^
/main.cpp:108:22: error: expected ‘)’ before ‘bool’
/main.cpp:110:18: error: expected type-specifier before ‘=’ token
       node = new = NodeWithGui(nh,nhPrivate);
                  ^
/main.cpp:110:44: error: no matching function for call to ‘NodeWithGui::NodeWithGui(ros::NodeHandle&, ros::NodeHandle&)’
       node = new = NodeWithGui(nh,nhPrivate);
                                            ^
/main.cpp:110:44: note: candidates are:
/main.cpp:89:7: note: NodeWithGui::NodeWithGui()
 class NodeWithGui : public ImageConverter
       ^
/main.cpp:89:7: note:   candidate expects 0 arguments, 2 provided
/main.cpp:89:7: note: NodeWithGui::NodeWithGui(const NodeWithGui&)
/main.cpp:89:7: note:   candidate expects 1 argument, 2 provided
make[2]: *** [subscriber/CMakeFiles/main.dir/main.cpp.o] Error 1
make[1]: *** [subscriber/CMakeFiles/main.dir/all] Error 2
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

Asked by 215 on 2016-03-20 12:46:21 UTC

Comments

let's break the problem to some steps, so, first, can you please post more about the compilation output (what does it say exactly) ?

Asked by Captcha on 2016-03-20 13:01:49 UTC

Error message included..

Asked by 215 on 2016-03-20 13:22:28 UTC

Answers

Asked by DavidN on 2016-03-22 06:46:55 UTC

Comments