手上有一堆录制好的rosbag,融合了多个传感器的消息。我现在要提取其中指定topic的消息，将每一帧消息都保存成一个对应的pcd文件和bin文件。我一开始想的思路是先把rosbag play发布出来，写一个ros节点订阅指定的topic并从中做pcd和bin的转换和文件保存工作。但是这种方式一方面是不方便自动化处理，还得手动去play这些rosbag。另一方面，保存文件的时候文件名和rosbag包不太好建立对应关系。重新思考了一下，改用扫面存放rosbag的目录，直接读取rosbag文件中指定topic的内容。

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <string>
#include <stdlib.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>

ros::Publisher pub;

void SaveToPCD(const pcl::PointCloud<pcl::PointXYZI>& cloud, const std::string& path) {
    std::cout << "pcd_path: " << path << std::endl;
    pcl::io::savePCDFileASCII(path,cloud);
}

void SaveToBin(const pcl::PointCloud<pcl::PointXYZI>& cloud, const std::string& path) {
    std::cout << "bin_path: " << path << std::endl;
    //Create & write .bin file
    std::ofstream out(path.c_str(), ios::out|ios::binary|ios::app);
    if(!out.good()) {
        cout<<"Couldn't open "<<path<<endl;
        return;
    }
    for (size_t i = 0; i < cloud.points.size (); ++i) {
        out.write((char*)&cloud.points[i].x, 3*sizeof(float));
        out.write((char*)&cloud.points[i].intensity, sizeof(float));
    }
    out.close();
}

void ToPublish(const pcl::PointCloud<pcl::PointXYZI>& cloud) {
    sensor_msgs::PointCloud2 msg_publish;
    pcl::toROSMsg(cloud, msg_publish);
    msg_publish.header.frame_id = "innovusion";
    pub.publish(msg_publish);
}

int Rosbag2PcdAndBin(std::string path) {
    boost::filesystem::path boost_path(path);
    std::string parent_path=boost_path.parent_path().string();
    std::string stem = boost_path.stem().string();
    std::cout << "input: " << path << std::endl;

    int frame_cnt = 0;
    rosbag::Bag bag;
    bag.open(path, rosbag::bagmode::Read); //open *.bag
    std::vector<std::string> topics; //set topics which you want to read
    topics.push_back(std::string("/iv_points2")); //这个是我指定要读取的消息topic
    rosbag::View view(bag, rosbag::TopicQuery(topics));; //read defined topics
    for (rosbag::MessageInstance const m : view) {
        sensor_msgs::PointCloud2::ConstPtr input = m.instantiate<sensor_msgs::PointCloud2>();
        ++frame_cnt;
        if (input == NULL) {
            continue;
        }
        //std::cout << path << "," << frame_cnt << std::endl;
        {
            int point_bytes = input->point_step;
            int offset_x,offset_y,offset_z,offset_intensity;

            const auto& fields = input->fields;
            for (int f = 0; f < fields.size(); ++f) {
                if (fields[f].name == "x") offset_x = fields[f].offset;
                if (fields[f].name == "y") offset_y = fields[f].offset;
                if (fields[f].name == "z") offset_z = fields[f].offset;
                if (fields[f].name == "intensity") offset_intensity = fields[f].offset;
            }

            //坐标转换
            bool do_convert = true;
            pcl::PointCloud<pcl::PointXYZI> cloud;//定义转换的点云数据类型
            if (do_convert) {
                for (int i=0; i<input->width; ++i) {
                    pcl::PointXYZI point;
                    point.x = *(float*)(input->data.data() + point_bytes*i + offset_x);
                    point.y = *(float*)(input->data.data() + point_bytes*i + offset_y);
                    point.z = *(float*)(input->data.data() + point_bytes*i + offset_z);
                    point.intensity = *(unsigned char*)(input->data.data() + point_bytes*i + offset_intensity);

                    auto tmp = point.x;
                    point.x = point.z;
                    point.y = -point.y;
                    point.z = tmp;

                    cloud.push_back(point);
                }
                cloud.width = input->width;
                cloud.height = input->height;
            } else {
                pcl::fromROSMsg(*input, cloud);
            }

            if (cloud.empty()) {
                std::cerr << "path:" << path << ",frame:" << frame_cnt << " no point cloud." << std::endl;
                continue;
            }

            if (1) {
                std::string pcd_path = parent_path + "/" + stem + "_frame_" + std::to_string(frame_cnt) + ".pcd";
                SaveToPCD(cloud, pcd_path);
            }

            if (1) {
                ToPublish(cloud);
            }

            if (1) {
                std::string bin_path = parent_path + "/" + stem + "_frame_" + std::to_string(frame_cnt) + ".bin";
                SaveToBin(cloud, bin_path);
            }
        }
    }
    bag.close();
    return 0;
}

int main(int argc,char** argv) {
    // Initialize ROS
    ros::init (argc, argv, "Rosbag2PcdAndBin");
    ros::NodeHandle nh;

    // Create publish
    pub = nh.advertise<sensor_msgs::PointCloud2>("iv_points2_convert",1);

    //do convert
    std::string data_dir("/data/to/rosbag/");
    boost::filesystem::directory_iterator end_iter;
    for (boost::filesystem::directory_iterator iter(data_dir); iter != end_iter ; iter++ ) {
        if (boost::filesystem::is_regular_file(*iter) and (*iter).path().string().find(".bag") != std::string::npos) {
            Rosbag2PcdAndBin((*iter).path().string());
        }
    }
}

            代码中用到了坐标转换,这个是应为我所读取的雷达传感器中的坐标系的定义和我们平常习惯的激光雷达坐标系的定义不一样,我在这里作了转换,一般情况下是不需要的。

【补充】

ros节点CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(hive_converter)
add_compile_options(-std=c++11 -g)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pg")
SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pg")
SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -pg")
find_package(catkin REQUIRED COMPONENTS
    roscpp
    rosbag
    std_msgs
    sensor_msgs
)
find_package(PCL REQUIRED)
catkin_package(
)
include_directories(
  include
  /usr/include/pcl-1.8
  /usr/local/include/pcl-1.8
  ../../../src/liblog/include/
   ../../../src/liblog/include/log4cplus
  #include/log4cplus
  ${PCL_INCLUDE_DIRS}
  ${EIGEN_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
)
add_executable(${PROJECT_NAME} src/rosbag_to_pcd_and_bin.cpp)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
target_link_libraries(${PROJECT_NAME}
    ${catkin_LIBRARIES}
    ${PCL_LIBRARIES}
    )