ros 将图像和点云信息转换为bag形式

0.写在前面

• 这篇博客的目的我是为了做标定
• 已有的材料是单线激光雷达扫描一圈得到的点云信息和固定相机所拍摄的图片
• 标定用的是autoware，所需要的是rosbag这样子的具有ros topic的信息，所以进行转化
• 注意：我这是一个ros程序，这方面的内容可以参考 ros软件包创建流程
• 因为是写给自己看的，所以文字解释较为简略。

2020/7/15 更新 优化了文件的排序，按数字大小升序

1.主要思想

1.cv::Mat转化为sensor_msgs::ImagePtr
2.将pcl::PointCloud转化为sensor_msgs::PointCloud2

2.torosbag.cpp

#include <string>
#include <ros/console.h>
#include <rosbag/bag.h>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT>::Ptr PointPtr;

void GetFileNames(string path,vector<string>& filenames, string con, string form);
void GetFileNamesByGlob(cv::String path,vector<cv::String>& filenames, string con);
int main(int argc, char **argv)
{
    //输入文件和输出文件路径
    string base_dir = "/home/jinye/datasets/cal_image_laser/";
    string laser_dir = base_dir + "scan_pcd/";
    string img_dir = base_dir + "image/";
    string output_bag="./res.bag";
    string img_format = ".jpg", laser_format = ".pcd";//格式
    string img_shape = "img", laser_shape = "scanf";
//    if(argc != 4)
//    {
//        cerr<<"输入参数数量不对!"<<endl;
//        exit(1);
//    }

//    string pcd_file = argv[1];
//    string image_dir = argv[2];
//    string output_filename = argv[3];

    //读取图片和点云信息
    vector<string> img_names;
    //string con = ".png";
    GetFileNames(img_dir, img_names, img_format, img_shape);
    cout<<"图片读取完成"<<endl;

//    for(int i = 0;i < img_names.size();i++)
//        cout<<img_names[i]<<endl;

    cout<<"----"<<endl;
    //读取一个即可 只需点云和图片命名形式相同
//    vector<string> laser_names;
//    string con2 = ".pcd";
//    GetFileNames(laser_dir, laser_names, laser_format);
//    cout<<"点云读取完成"<<endl;

//opencv::glob not used
//    std::vector<cv::String> img_names;
//    cv::String img_dir2 = img_dir;
//    std::vector<cv::String> laser_names;
//    cv::String laser_dir2 = laser_dir;
//    string con = "";
//    GetFileNamesByGlob(img_dir2, img_names, con);
//    GetFileNamesByGlob(laser_dir2, laser_names, con);

    ros::Time::init();
    rosbag::Bag bag;
    bag.open(output_bag, rosbag::bagmode::Write);
    int seq = 0;
    vector<string>::iterator it;
    for(it = img_names.begin(); it != img_names.end();it++)//todo 之后改成图片数量的多少
    {
        string tmp = *it;
        //cout<<tmp<<endl;
        string strLaserrFile = laser_dir + laser_shape + tmp + laser_format;
        string strImgFile =  img_dir + img_shape + tmp + img_format;

        // --- for laser points ---//
        PointPtr cloud(new pcl::PointCloud<PointT>);
        if (pcl::io::loadPCDFile(strLaserrFile, *cloud) == -1)
        {
            cout << "点云数据读取失败: "<< strLaserrFile << endl;
        }

        sensor_msgs::PointCloud2 laserCloud;
        pcl::toROSMsg(*cloud, laserCloud);
        ros::Time timestamp_ros = ros::Time::now();
        laserCloud.header.stamp = timestamp_ros;
        laserCloud.header.frame_id = "/scan";

        // --- for image ---//
        cv::Mat img = cv::imread(strImgFile);
        if (img.empty())
            cout<<"图片为空: "<<strImgFile<<endl;
        cv_bridge::CvImage ros_image;
        sensor_msgs::ImagePtr ros_image_msg;

        ros_image.image = img;
        ros_image.encoding = "bgr8";
        //cout<<"debug_______"<<endl;
        //ros::Time timestamp_ros2 = ros::Time::now();
        ros_image_msg = ros_image.toImageMsg();
        ros_image_msg->header.seq = seq;
        ros_image_msg->header.stamp = timestamp_ros;
        ros_image_msg->header.frame_id = "/image_raw";

        bag.write("/points_raw", laserCloud.header.stamp, laserCloud);
        bag.write("/cam", laserCloud.header.stamp, ros_image_msg);
        cout<<"write frame: "<<seq<<endl;
        seq++;
    }

    cout<<"---end---"<<endl;

    return 0;
}

//con:文件格式 form:文件命名形式
void GetFileNames(string path,vector<string>& filenames, string con, string form)
{
    DIR *pDir;
    struct dirent* ptr;
    string filename, format, name, name2;

    if(!(pDir = opendir(path.c_str())))
        return;

    while((ptr = readdir(pDir))!=0) {
        //跳过.和..文件
        if(strcmp(ptr->d_name, ".") == 0 || strcmp(ptr->d_name, "..") == 0)
            continue;
        filename = ptr->d_name;
        format = filename.substr(filename.find("."), filename.length());
        //name = filename.substr(0, filename.find("."));
        name = filename.substr(form.length(), filename.find(".") - form.length());
        cout<<filename<<"\t"<<name<<endl;

        //cout<<format<<endl;
        if(format == con)//也可以添加对文件名的要求
            filenames.push_back(name);
        //filenames.push_back(filename);
    }
    closedir(pDir);
    //sort(filenames.begin(), filenames.end());
    //string 类型的数字比较大小有问题， 所以转成int在排序
    sort(filenames.begin(), filenames.end(), [](string a, string b){
        return stoi(a) < stoi(b);
    });
}

void GetFileNamesByGlob(cv::String path,vector<cv::String>& filenames, string con)
{
    cv::glob(path, filenames, false);//不递归
    for(size_t i = 0; i < filenames.size(); ++i)
    {
        std::cout<<filenames[i]<<std::endl;
    }
}

3.CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(pkg)

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  rosbag
  cv_bridge
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES pkg
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${rosbag_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  #${PCL_INCLUDE_DIRS}
)

# pcl
find_package( PCL REQUIRED COMPONENT common io rosbag )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )

# 寻找OpenCV库
find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/pkg.cpp
# )


## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(node src/torosbag.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(node
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
  ${rosbag_LIBRARIES}
  ${OpenCV_LIBS}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_pkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

参考
https://www.cnblogs.com/minskychu/p/11890243.html