Robotics StackExchange | Archived questions

message_filters::sync::ApproximateTime does not work properly

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 cvbridge to do some image processing. I've already learned the best way to do this is using the `messagefilters::sync::ApproximateTimeclass. The synchronizer works and i can see the images withimshow()`. 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.

Asked by Gren8 on 2016-01-11 15:13:33 UTC

Comments

Answers

In main() where you create sub1 and sub2 try increasing the queue_size from 1 to 5 or 10. This will allow the approximate algorithm enough samples to find which messages are closest in time. Alternatively, try reducing the queue_size passed to the sync_policy. You are using 100 and could try dropping that to about 10.

Also, instead of wrapping spinOnce() inside a while loop you can replace the entire while loop with a single call to ros::spin().

Asked by Thomas D on 2016-01-11 17:34:11 UTC

Comments

Thanks for the advice, but i already tried it with different values for the queue_size as you said, but it didn't change anything.

Asked by Gren8 on 2016-01-12 02:35:43 UTC

Did you ever figure out how to fix this? I have a similar problem.

Asked by reben on 2018-01-23 19:20:58 UTC