Time synchronization problem to show the images from a binocular camera [closed]

edit

Backgrounds: Hi, everyone. I am a beginner of vision SLAM and now trying to show images in the real time of a binocular camera. Something I have done is that I have used the usb-cam package to get the image data and shown them in the rviz and rqt. But I meet a time synchroniazation problem with the OpenCV imshow(). Specificlly, I want to use usb-cam to get the data and do some image procession with OpenCV and at first I try to display the images.

Environment: Ubuntu 16.04 in the Vmware WorkStation 12 (in win10), ROS Kinetic, OpenCV 3.3.0. Binocular camera can support the mjepg format with 640*480 and 30fps.

Problem: I meet a problem that one of image from a binocular camera has a time delay corresponding to another image and the image is fixed that I mean that there is only that one camera with the time delay in the image display. Further, the time delay has a increase trend to be longer.

Codes :

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <opencv2/opencv.hpp>

#include <queue>
#include <thread>
#include <mutex>
#include <iostream>

std::queue<sensor_msgs::ImageConstPtr> img0_buf;
std::queue<sensor_msgs::ImageConstPtr> img1_buf;
std::mutex m_buf;

//Test for rqt, 0 for cv, else for rviz
int flag = 0;

ros::Publisher pubImg0;
ros::Publisher pubImg1;

void img0_callback(const sensor_msgs::ImageConstPtr &img0)
{
    m_buf.lock();
    //For rqt
    if(flag != 0)
    {
        sensor_msgs::Image img;
img.header=img0->header;
img.height = img0->height;
img.width = img0->width;
img.is_bigendian = img0->is_bigendian;
img.step = img0->step;
img.data=img0->data;
img.encoding=img0->encoding;
pubImg0.publish(img);
// std::cout<<"0."<<img.header<<std::endl;
}

else
    {
            img0_buf.push(img0);
        }
    m_buf.unlock();
}

void img1_callback(const sensor_msgs::ImageConstPtr &img1)
{
    m_buf.lock();
    if(flag != 0)
    {
        sensor_msgs::Image img;
        img.header=img1->header;
img.height = img1->height;
img.width = img1->width;
img.is_bigendian = img1->is_bigendian;
img.step = img1->step;
img.data=img1->data;
img.encoding=img1->encoding;
pubImg1.publish(img);
// std::cout<<"1."<<img.header<<std::endl;
}

else
{
    img1_buf.push(img1);
}
m_buf.unlock();

}

//Use the cv_bridge of ros to change the image data format from msgs to cv
cv::Mat msg2cv(const sensor_msgs::ImageConstPtr &img_msg)
{
    cv_bridge::CvImageConstPtr ptr;
    sensor_msgs::Image img_tmp;
    img_tmp.header = img_msg->header;
    img_tmp.height = img_msg->height;
    img_tmp.width = img_msg->width;
    img_tmp.is_bigendian = img_msg->is_bigendian;
    img_tmp.step = img_msg->step;
    img_tmp.data = img_msg->data;
    img_tmp.encoding =img_msg->encoding;

    ptr = cv_bridge::toCvCopy(img_tmp, sensor_msgs::image_encodings::BGR8);
    cv::Mat img = ptr->image.clone();

    return img;
}

//With reference of VINS rosNodeTest.cpp
void display()

{
    while(1)
    {
        cv::Mat image0, image1;
        // double t1,t2;
        m_buf.lock();
        if(!img0_buf.empty() && !img1_buf.empty())
        {
            ROS_INFO("Two cameras work");
            image0=msg2cv(img0_buf.front());
        // ROS_INFO("img0 %.9lf", img0_buf.front()->header.stamp.toSec());
    // t1=img0_buf.front()->header.stamp.toSec();
    img0_buf.pop();
    imshow("camera1", image0);

    image1=msg2cv(img1_buf.front());
    // ROS_INFO("img1 %.9lf", img1_buf.front()->header.stamp.toSec());
    // t2=img1_buf.front()->header.stamp.toSec();
    img1_buf.pop();
    cv::imshow("camera2", image1);
    cv::waitKey(1);
}
   m_buf.unlock();

// //display with cv
// if(!image0.empty())
// {
//     imshow("camera1", image0);
//     // cv::waitKey(1);
// }
// // else
// // { std::cout<<"image0 is empty!"<<std::endl;}


// if(!image1.empty())
// {
//     imshow("camera2", image1);
//     // cv::waitKey(1);

// }
// else ...