使用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: