message_filters::sync::ApproximateTime does not work properly [closed]
Good evening,
i have an important question. I try to subscribe to two image topics and want them to be synchronized and ready to be processed in one callback.
In the callback function i want to convert them via cv_bridge to do some image processing.
I've already learned the best way to do this is using the message_filters::sync::ApproximateTime
class. The synchronizer works and i can see the images with imshow()
. The problem is that the callback function only get called each second so i get a very SLOW image stream. If i move an object i'll see it seconds later moving on the screen.
void callback(const ImageConstPtr& sub1, const ImageConstPtr& sub2){
double stamp_1 =sub1->header.stamp.toSec();
double stamp_2 =sub2->header.stamp.toSec();
ROS_INFO("\nCALLBACK GETS CALLED\n TIME_STAMP_IMAGE1: %f\n TIME_STAMP_IMAGE2: %f\n", stamp_1, stamp_2);
Mat image_front, image_back;
//Converting Image 1
image1 = *(sub1.get());
frameptr1=cv_bridge::toCvCopy(bild1, sensor_msgs::image_encodings::BGR8);
image_front=frameptr1->image;
//Converting Image 2
image2 = *(sub2.get());
frameptr2=cv_bridge::toCvCopy(bild2, sensor_msgs::image_encodings::BGR8);
image_back=frameptr2->image;
//Show images
imshow("window1", image_front);
imshow("window2", image_back);
}
int main(int argc, char** argv) {
ros::init( argc, argv, "sync_test" );
ros::NodeHandle nh;
namedWindow("window1", CV_WINDOW_NORMAL);
namedWindow("window2", CV_WINDOW_NORMAL);
cv::startWindowThread();
message_filters::Subscriber<Image> sub1(nh, "/camera1/image_raw", 1);
message_filters::Subscriber<Image> sub2(nh, "/camera2/image_raw", 1);
typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), sub1, sub2);
sync.setAgePenalty(1.0);
sync.registerCallback(boost::bind(&callback, _1, _2));
while(ros::ok()){
ros::spinOnce();
}
return 0;
}
Could anybody see a possibly mistake? I really need help, it's very important for me.