pack_dataset_to_rosbag

打包数据集成rosbag

资料参考

代码参考kalibr：https://github.com/ethz-asl/kalibr/blob/master/aslam_offline_calibration/kalibr/python/kalibr_bagcreater

数据类型Image：https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html

数据类型Imu：http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html

过程

创建功能包，用的ros的opencv

cd ~/catkin_ws/src
catkin_create_pkg pack_datasets std_msgs sensor_msgs OpenCV rosbag rospy roscpp
cd ..
catkin_make

编写代码

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <opencv2/opencv.hpp>
#include <dirent.h>
#include <fstream>

std::string remove_suffix(std::string name)
{
    return (name.substr(0, name.rfind(".")));
}

bool get_dir_sort_names(std::string dir, std::vector<std::string> &result)
{
    // C++17可以用filesystem
    DIR *dir_ptr = opendir(dir.c_str());
    if (dir_ptr == NULL)
        return false;

    struct dirent *dirp;
    std::vector<std::string> names;
    while (((dirp = readdir(dir_ptr)) != NULL))
    {
        if (dirp->d_name[0] == '.')
            continue; // 剔除掉. 和 ..

        names.push_back(dirp->d_name);
    }

    std::sort(names.begin(), names.end(), [](std::string a, std::string b)
              { return (std::stod(remove_suffix(a)) < std::stod(remove_suffix(b))); }); // 升序排序

    result.assign(names.begin(), names.end());

    return true;
}

bool write_images_to_bag_topic(rosbag::Bag &bag, std::string dir, std::vector<std::string> names, std::string frame_id, std::string topic_name)
{
    std::cout << "Write image data..." << std::endl;
    for (auto name : names)
    {
        std::string full_path = dir + name;
        cv::Mat image = cv::imread(full_path);
        if(image.empty())
            return false;

        // 分离秒和纳秒 获得时间戳
        size_t dot_index = name.find(".");
        std::string time_sec_string = name.substr(0, dot_index);
        std::string time_nsec_string = name.substr(dot_index + 1, name.size());
        ros::Time timestamp(std::stoi(time_sec_string), std::stoi(time_nsec_string)); // s ns

        sensor_msgs::Image image_msg;
        image_msg.header.stamp = timestamp;
        image_msg.header.frame_id = frame_id;
        image_msg.height = image.rows;
        image_msg.width = image.cols;
        image_msg.step = image.step;
        image_msg.encoding = "bgr8";
        image_msg.data = (std::vector<uchar>)(image.reshape(1, 1));

        bag.write(topic_name, timestamp, image_msg);
        // ROS_INFO("Write %s to topic: %s", full_path.c_str(), topic_name.c_str());
    }
    return true;
}

bool write_imu_txt_to_bag_topic(rosbag::Bag &bag, std::string txt_path, std::string frame_id, std::string topic_name)
{
    std::cout << "Write imu data..." << std::endl;
    std::ifstream imu_txt(txt_path);
    if(!imu_txt.is_open())
        return false;

    
    while (!imu_txt.eof())
    {
        // 时间戳
        std::string time_string;
        imu_txt >> time_string;
        if (time_string.size() == 0)
            break;

        size_t dot_index = time_string.find(".");
        std::string time_sec_string = time_string.substr(0, dot_index);
        std::string time_nsec_string = time_string.substr(dot_index + 1, time_string.size());
        ros::Time timestamp(std::stoi(time_sec_string), std::stoi(time_nsec_string)); // s ns

        // 旋转四元数
        double quaternion[4]; // qx qy qz qw
        for (auto &q : quaternion)
            imu_txt >> q;
        geometry_msgs::Quaternion quaternion_msg;
        quaternion_msg.x = quaternion[0];
        quaternion_msg.y = quaternion[1];
        quaternion_msg.z = quaternion[2];
        quaternion_msg.w = quaternion[3];

        // 角速度
        double angular_velocity[3];
        for (auto &w : angular_velocity)
            imu_txt >> w;
        geometry_msgs::Vector3 angular_velocity_msg;
        angular_velocity_msg.x = angular_velocity[0];
        angular_velocity_msg.y = angular_velocity[1];
        angular_velocity_msg.z = angular_velocity[2];

        // 加速度
        double linear_acceleration[3];
        for (auto &a : linear_acceleration)
            imu_txt >> a;
        geometry_msgs::Vector3 linear_acceleration_msg;
        linear_acceleration_msg.x = linear_acceleration[0];
        linear_acceleration_msg.y = linear_acceleration[1];
        linear_acceleration_msg.z = linear_acceleration[2];

        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = timestamp;
        imu_msg.header.frame_id = frame_id;
        imu_msg.orientation = quaternion_msg;
        imu_msg.angular_velocity = angular_velocity_msg;
        imu_msg.linear_acceleration = linear_acceleration_msg;

        bag.write(topic_name, timestamp, imu_msg);

        /*
        ROS_INFO("Write %d.%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf to topic: %s",
                 (int)imu_msg.header.stamp.sec, (int)imu_msg.header.stamp.nsec,
                 imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w,
                 imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.z,
                 imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z,
                 topic_name.c_str());
        */
    }

    return true;
}

int main(int argc, char **argv)
{
    /*
        rosrun pack_datasets pack_datasets \
        _save_bag_path:=SAVE_PATH \
        _cam0_images_dir:=CAM0_DIR \
        _cam0_topic_name:=CAM0_TOPIC_NAME \
        _cam1_images_dir:=CAM1_DIR \
        _cam1_topic_name:=CAM1_TOPIC_NAME \
        _imu_txt_path:=IMU_PATH \
        _imu_topic_name:=IMU_TOPIC_NAME 
    */
   
    // 初始化ROS节点
    ros::init(argc, argv, "pack_datasets");
    ros::NodeHandle nh("~");

    // rosbag
    rosbag::Bag bag;
    std::string save_bag_path;
    nh.param<std::string>("save_bag_path", save_bag_path);
    nh.getParam("save_bag_path", save_bag_path);
    if(save_bag_path.size() == 0)
    {
        ROS_ERROR("Please specify the package save path by _save_bag_path:=SAVE_PATH");
        return -1;
    }

    // cam0
    std::string cam0_images_dir;
    std::string cam0_topic_name;
    nh.param<std::string>("cam0_images_dir", cam0_images_dir);
    nh.param<std::string>("cam0_topic_name", cam0_topic_name);
    nh.getParam("cam0_images_dir", cam0_images_dir);
    nh.getParam("cam0_topic_name", cam0_topic_name);
    if((cam0_images_dir.size() == 0) ^ (cam0_topic_name.size() == 0)) 
    {
        ROS_ERROR("Please specify the image path of cam0 and the topic name to save to by _cam0_images_dir:=CAM0_DIR _cam0_topic_name:=CAM0_TOPIC_NAME");
        return -1;
    }

    // cam1
    std::string cam1_images_dir;
    std::string cam1_topic_name;
    nh.param<std::string>("cam1_images_dir", cam1_images_dir);
    nh.param<std::string>("cam1_topic_name", cam1_topic_name);
    nh.getParam("cam1_images_dir", cam1_images_dir);
    nh.getParam("cam1_topic_name", cam1_topic_name);
    if((cam1_images_dir.size() == 0) ^ (cam1_topic_name.size() == 0)) 
    {
        ROS_ERROR("Please specify the image path of cam1 and the topic name to save to by _cam1_images_dir:=CAM1_DIR _cam1_topic_name:=CAM1_TOPIC_NAME");
        return -1;
    }

    // imu
    std::string imu_txt_path;
    std::string imu_topic_name;
    nh.param<std::string>("imu_txt_path", imu_txt_path);
    nh.param<std::string>("imu_topic_name", imu_topic_name);
    nh.getParam("imu_txt_path", imu_txt_path);
    nh.getParam("imu_topic_name", imu_topic_name);
    if((imu_txt_path.size() == 0) ^ (imu_topic_name.size() == 0)) 
    {
        ROS_ERROR("Please specify the txt path of imu and the topic name to save to by _imu_txt_path:=IMU_PATH _imu_topic_name:=IMU_TOPIC_NAME");
        return -1;
    }

    bag.open(save_bag_path, rosbag::bagmode::Write);
    if((cam0_images_dir.size() != 0) && (cam0_topic_name.size() != 0))
    {
        std::vector<std::string> cam0_image_names;
        get_dir_sort_names(cam0_images_dir, cam0_image_names);
        write_images_to_bag_topic(bag, cam0_images_dir, cam0_image_names, "cam0", cam0_topic_name);
    }
    if((cam1_images_dir.size() != 0) && (cam1_topic_name.size() != 0))
    {
        std::vector<std::string> cam1_image_names;
        get_dir_sort_names(cam1_images_dir, cam1_image_names);
        write_images_to_bag_topic(bag, cam1_images_dir, cam1_image_names, "cam1", cam1_topic_name);
    }
    if((imu_txt_path.size() != 0) && (imu_topic_name.size() != 0))
    {
        write_imu_txt_to_bag_topic(bag, imu_txt_path, "imu0", imu_topic_name);
    }
    bag.close();

}

cmake添加

add_executable(pack_images_imu src/pack_images_imu.cpp)
target_link_libraries(pack_images_imu ${catkin_LIBRARIES})

编译

cd ~/catkin_ws
catkin_make

运行

rosrun pack_datasets pack_images_imu \
_save_bag_path:=./src/pack_datasets/result.bag \
_cam0_images_dir:=./src/pack_datasets/dataset/left/ \
_cam0_topic_name:=/cam0/image_raw \
_cam1_images_dir:=./src/pack_datasets/dataset/right/ \
_cam1_topic_name:=/cam1/image_raw \
_imu_txt_path:=./src/pack_datasets/dataset/imu.txt \
_imu_topic_name:=/imu0