TimeSynchronizer in a class using stereo input

edit

Hi, I'm relatively new to ROS. I want to write a class which gets two raw images and creates a callback,where I can afterwards manipulate both obtained images at the same time. I tried TimeSynchronizer but it seems to never reach the Process() function. I want to create later on a disparity map (but not with the stereo_view) so I need synchronized input images. Time stamps are telling me that my input images are synchronized. So how can I get the two corresponding images in a callback function?

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <message_filters/time_synchronizer.h>
#include <message_filters/subscriber.h>

class ImgProcessor
{



ros::NodeHandle nh_;
image_transport::ImageTransport it_;


sensor_msgs::ImageConstPtr img_L;
sensor_msgs::ImageConstPtr img_R;


image_transport::Publisher image_pub_L;
image_transport::Publisher image_pub_R;

std::string IMG_TARGET_L_;
std::string IMG_TARGET_R_;

std::string IMG_DEST_L_;
std::string IMG_DEST_R_;
public:


// Constructor: Initialize the subscribe-act-publish-cycle
ImgProcessor(const std::string IMG_DEST_L, const std::string IMG_TARGET_L,const std::string IMG_DEST_R, const std::string IMG_TARGET_R)
    : it_(nh_)
{
    // Open Window to show output stream
    IMG_TARGET_L_ = IMG_TARGET_L;
    IMG_TARGET_R_ = IMG_TARGET_R;
    IMG_DEST_L_ = IMG_DEST_L;
    IMG_DEST_R_ = IMG_DEST_R;

    // Create output window
    cv::namedWindow(IMG_TARGET_L_);
    cv::namedWindow(IMG_TARGET_R_);

    // Subscribe to input video and publish output video
    message_filters::Subscriber<sensor_msgs::Image> image_sub_L(nh_,IMG_DEST_L_,40);
    message_filters::Subscriber<sensor_msgs::Image> image_sub_R(nh_,IMG_DEST_R_,40);
    message_filters::TimeSynchronizer<sensor_msgs::Image,sensor_msgs::Image> sync(image_sub_L,image_sub_R,40);
    sync.registerCallback(boost::bind(&ImgProcessor::Process,this, _1, _2));

    ROS_INFO("Synched.");

    image_pub_L = it_.advertise(IMG_TARGET_L_,1);
    image_pub_R = it_.advertise(IMG_TARGET_R_,1);

}

// Destructor: Close all opened images
~ImgProcessor()
{
    cv::destroyAllWindows();
}


// Process a message obtained in IMG_DEST
void Process(const sensor_msgs::ImageConstPtr& msg_l,const sensor_msgs::ImageConstPtr& msg_r)
{

    ROS_INFO("Origin Stamp = [%i] ", int(msg_l->header.stamp.toSec()));

    // Create a CV image copy from incoming message
    cv_bridge::CvImagePtr cv_ptr[2];
    try
    {
        cv_ptr[0] = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::MONO8);
        cv_ptr[1] = cv_bridge::toCvCopy(msg_r, sensor_msgs::image_encodings::MONO8);
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }


    // Process the incoming image
    //cv::medianBlur(cv_ptr->image,cv_ptr->image,3);
    cv::equalizeHist(cv_ptr[0]->image,cv_ptr[0]->image);
    cv::Canny(cv_ptr[0]->image,cv_ptr[0]->image,50,255);

    cv::equalizeHist(cv_ptr[1]->image,cv_ptr[1]->image);
    cv::Canny(cv_ptr[1]->image,cv_ptr[1]->image,50,255);


    // Update GUI
    cv::imshow(IMG_TARGET_L_, cv_ptr[0]->image);
    cv::waitKey(3);
    cv::imshow(IMG_TARGET_R_, cv_ptr[1]->image);
    cv::waitKey(3);

    img_L = cv_ptr[0]->toImageMsg();
    img_R = cv_ptr[1]->toImageMsg();
    //ROS_INFO("Processed Stamp = [%i] ", int(msg_o->header.stamp.nsecs));
    //Publish transformed video
    image_pub_L.publish(img_L);
    image_pub_R.publish(img_R);
}

};





int main(int argc, char **argv)
{
// Initialize the ROS system
ros::init(argc, argv, "stereo_proc_node");

// Tell the user that the node has started to process images
ROS_INFO("Processing images...");

// Create image processor object
ImgProcessor ip("/stereo/left/image_rect","stereo_proc/left/image","/stereo/right/image_rect","stereo_proc/right/image");


//sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return 0;

}