ROS: How to read and show image from bag file using CPP?

Share:
[toc] # Objective - Read and show image from bag file using C++ (使用ROS C++ 接口进行图像读取与显示) --- 网上有不少在ROS中使用Python的示例程序,但是我找了很久也没有找到C++版本的程序。 本文主要介绍如何在ROS中使用C++接口对bagfile 文件进行操作。 要使用此程序,需要提供两个参数: 1. 图像的 topic 名称,如果是使用TUM 数据集,那么此名称为 /camera/rgb/image_color 2. bag file 文件的名称, 同样如果是使用TUM数据集的话,此名称设为 freiburg3/rgbd_dataset_freiburg3_walking_xyz.bag 本程序的功能: 读取图像topic的图像并用OpenCV进行简单的操作,之后使用OpcnCV进行显示。 --- # Source code - Input: - image topic name - bag file name ```cpp // http://docs.ros.org/diamondback/api/rosbag/html/c++/classrosbag_1_1MessageInstance.html #include #include #include #include #include #include int main(int argc, char** argv) { std::cout << "Read image from bag file" << std::endl; std::cout << "Please set image_topic_name and bagfile_name" << std::endl; std::string image_topic_name = "/camera/rgb/image_color"; std::string bagfile_name = "/home/yubao/data/Dataset/TUM/freiburg3/rgbd_dataset_freiburg3_walking_xyz.bag"; std::cout << "topic name: " << image_topic_name << std::endl; cv::Mat image; rosbag::Bag bag; bag.open(bagfile_name); for (rosbag::MessageInstance const m : rosbag::View(bag)) { // fetch image topic name std::string imgTopic = m.getTopic(); if (image_topic_name == imgTopic) { try { sensor_msgs::ImageConstPtr imgMsgPtr = m.instantiate(); image = cv_bridge::toCvCopy(imgMsgPtr)->image; } catch (cv_bridge::Exception& e) { ROS_ERROR("Image convert error"); } cv::circle(image, cv::Point(image.cols/ 2, image.rows/ 2), 10, cv::Scalar(255, 0, 255), 3); cv::imshow("image", image); cv::waitKey(1); } } bag.close(); return 0; } ``` # Result read_image_bagfile # Reference - http://docs.ros.org/diamondback/api/rosbag/html/c++/classrosbag_1_1MessageInstance.html - http://wiki.ros.org/rosbag/Tutorials

No comments