利用ROS话题的订阅功能实现从bag文件中提取信息
在验证定位算法的时候，为了更好的说明问题，我们会实际采集数据来进行实验。通常由于定位算法不支持直接运行bag文件，（当然你也可以将算法设计成ROS版）我们需要从bag文件中提取出所需要的数据。
本文以zr300相机采集的数据为例，利用ROS的话题订阅功能从bag文件中提取出图像信息及IMU信息，以及对应的时间戳。
1. ROS工作空间的创建
mkdir -p ~/catkin_ws/src ## 工作空间文件夹，也可以手动新建文件夹实现
cd ~/catkin_ws/src
catkin_init_workspace ###//创建工作空间
此时在src目录下会出现一个CMakeLists.txt 文件
2. 功能包创建
cd ~/catkin_ws/src ###在当前工作空间的src目录下进行
catkin_create_pkg extract cv_bridge roscpp std_msgs sensor_msgs ###功能包创建
###其中extract 是功能包名称，后面是依赖项
关于上述ROS工作空间和功能包创建不懂的可以参考ROS入门21讲，在B站上面有视频教学。
执行完上述命令之后，在src的目录下会出现一个extract 的文件夹，在此文件夹下有一个src 文件夹和一个CMakeLists.txt 文件是需要你去编写的。其中src文件夹下放置代码文件，CMakeLists.txt 文件代表电脑将如何对代码进行编译。（与平常利用cmake编程一样）
3. 获取bag文件中的数据的话题名，用于订阅
rosbag info xxx.bag ###h后面是你所要提取的bag文件
4. 代码的编写
话题订阅：
ros::Subscriber image_sub = n.subscribe(IMAGE_TOPIC, 1000, imageCallback);
////订阅图像信息
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback);
/////订阅imu信息
回调函数提取信息：
图像以及时间戳：
double timestamps=img_msg->header.stamp.toSec();
image_times.push_back(timestamps); ####时间戳
cv_bridge::CvImageConstPtr ptr;
ptr = cv_bridge::toCvCopy(img_msg, img_msg->encoding);
cv::Mat show_img = ptr->image;
images.push_back(show_img); ####图像
注意ROS中的图像存储格式与OPENCV之间不能直接转换，需要使用cv_bridge进行过渡。(我是这样理解的)
IMU以及时间戳：
double time = imu_msg->header.stamp.toSec();
imu_times.push_back(time);
a_x.push_back(imu_msg->linear_acceleration.x);
a_y.push_back(imu_msg->linear_acceleration.y);
a_z.push_back(imu_msg->linear_acceleration.z);
w_x.push_back(imu_msg->angular_velocity.x);
w_y.push_back(imu_msg->angular_velocity.y);
w_z.push_back(imu_msg->angular_velocity.z);
信息输出
IMU：
ofstream p;
p.open(IMU_PATH+"data.csv",ios::out|ios::trunc);
int n=a_x.size();
for(int i=0;i<n;i++)
{
p<<imu_times[i]<<" "<<w_x[i]<<" "<<w_y[i]<<" "<<w_z[i]<<" "
<<a_x[i]<<" "<<a_y[i]<<" "<<a_z[i]<<'\n';
}
图像以及时间戳：
string str1,image_path;
int n=images.size();
for (int i=0;i<n;i++)
{
stringstream ss1;
ss1 << i;
ss1 >> str1;
image_path=IMAGE_PATH + str1 + ".png";
cv::imwrite(image_path,images[i]);
}
ofstream p;
p.open(TIME_PATH+"times.txt",ios::out|ios::trunc);
int n=image_times.size();
for (int i=0;i<n;i++)
{
p<<i<<" "<<image_times[i]<<'\n';
}
5. 编译运行
在代码.cpp文件和CMakeLists.txt文件写好之后，进行编译运行。
catkin_make ### 在catkin_ws文件夹下利用catkin_make编译
rosrun xxxx ### 利用rosrun运行
同时另起一个终端，利用 rosbag paly xxxx.bag 进行bag文件的播放。
注意，代码的编译需要在开启ros ，利用roscore 命令。同时运行时需要注意source路径。
关于完整的代码文件，可以在我分享的GITHUB上进行获取，链接如下：
https://github.com/Tanyong66/-bag-.git
如果有任何不清楚的地方，或者有任何错误，请指正。可以邮箱联系我，本人邮箱：[email protected]