ros 下使用iai_kinect2同步获取Kinect2的彩色图，深度图和时间戳

 

      使用ROS的exactTime同步策略获取相同时间戳的RGB图与深度图

参考：https://blog.csdn.net/chishuideyu/article/details/77479758

/**
 * 
 * Description：
 *   Collect the rgb and depth images from iai_kinect2 bridge in ExactTime synchronize policy
 * in which the rgb and depth messages have exactly the same timestamp.And the timestamp we 
 * saved is in seconds (s) to 6 decimal places (us).
 * Functions:
 *
 */

#include <iostream>
#include <string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <fstream>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <std_msgs/String.h>
#include <cv_bridge/cv_bridge.h>//将ROS下的sensor_msgs/Image消息类型转化成cv::Mat。

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>//头文件sensor_msgs/Image是ROS下的图像的类型，这个头文件中包含对图像进行编码的函数


using namespace std;
using namespace cv;

Mat rgb, depth;
struct timeval tv;

string topic1_name = "/kinect2/qhd/image_color"; //topic 名称
string topic2_name = "/kinect2/qhd/image_depth_rect";

string filename_rgbdata="/home/nld/catkin_ws/recordData/RGBD/rgbdata.txt";
string filename_depthdata="/home/nld/catkin_ws/recordData/RGBD/depthdata.txt";
string save_imagedata = "/home/nld/catkin_ws/recordData/RGBD/";

ofstream of_depthInfo(filename_depthdata.c_str());
ofstream of_rgbInfo(filename_rgbdata.c_str());

void callback(const sensor_msgs::Image::ConstPtr imageColor, const sensor_msgs::Image::ConstPtr imageDepth);   
void readImage(const sensor_msgs::Image::ConstPtr msgImage, cv::Mat &image) ;
void saveImgdata(ofstream &of_rgbInfo,ofstream &of_depthInfo);

int main(int argc,char** argv)
{
   
	namedWindow("image color",CV_WINDOW_AUTOSIZE);
    namedWindow("image depth",CV_WINDOW_AUTOSIZE);

    
    if(!of_rgbInfo)
    {
    	std: