转载：将SLAM数据集中的图片格式转换成rosbag格式

转载：https://blog.csdn.net/LilyNothing/article/details/79206938

感谢分享，解决了困扰许久的问题。
由于很多的slam代码是有ROS版的,ROS版的一般数据的输入都是rosbag的形式,所以为了使用,测试新的数据集,需要将没有rosbag的数据集转为rosbag的形式,其实这里涉及的就是将普通的照片转为ros的图片,使用一个节点发布这写图片流,然后使用rosbag进行记录,然后生成一个rosbag的包.整体就是这样的思路.ROS的官网上也有相关的Tutorials.Tutorials 里面命令有一些小错误,但是不会影响很大.接下来将分两个部分来记录一下自己转换的过程.第一部分是学习ROS WIKI上的tutorials的内容,第二部分是使用自己的数据集进行实验.

一. Tutorials的学习

在ROS的image_transport的tutorial上,根据它的教程进行如下的命令操作,这些操作的前提是你已经在你的系统上安装了ROS的系统,哇是跟着官方教程 安装的,这里因为设置软件源的问题,折腾了好久,最后先删除出问题的软件源,最后再安装官方的教程就可以成功了.然后按照image_transport的教程走通如下的命令:

$ mkdir -p ~/image_transport_ws/src
$ cd ~/image_transport_ws/src
$ source /opt/ros/kinetic/setup.bash  #notice 看你安装的是什么版本的ros,我的是kinetic
$ catkin_create_pkg learning_image_transport image_transport cv_bridge #生成一个包
$ cd ~/image_transport_ws  #或者 cd ..
$ rosdep install --from-paths src -i -y --rosdistro kinetic #这里也需要改成自己的安装
$ catkin_make
$ source devel/setup.bash
$ git clone https://github.com/ros-perception/image_common.git
$ ln -s `pwd`/image_common/image_transport/tutorial/ ./src/image_transport_tutorial
$catkin_make #编译源文件
 
 
1
2
3
4
5
6
7
8
9
10
11

以上操作完成之后,你就可以使用如下步骤你就可以发布一张图片到ROS的节点中去了.

1. 在你编译的命令行窗口新开一个窗口,保证其是在 /image_transport_ws 这个路径下的,然后运行:

$roscore 
 
 
1

1. 在你编译的命令行窗口新开一个窗口,保证其是在 /image_transport_ws 这个路径下的,然后运行:

$ source /opt/ros/kinetic/setup.bash
$ source devel/setup.bash
$ rosrun image_transport_tutorial my_publisher path/to/some/image.jpg
 
 
1
2
3

以上source是为了保证某些路径,没有写进bash里面的话每次新开命令行都需要运行source一下.以上命令是发布了一个图片消息.想要看你启动的节点以及发布的图像数据.紧接着做第3步.

1. 查看节点

$rostopic list -v #查看节点
 
 
1

输出如下内容:(根据有些插件的不同可能输出不同,但是只要具有查不多的就好)
Published topics:
* /rosout [roslib/Log] 1 publisher
* /camera/image [sensor_msgs/Image] 1 publisher
* /rosout_agg [roslib/Log] 1 publisher

Subscribed topics:
* /rosout [roslib/Log] 1 subscriber

1. 订阅节点

$rosrun image_transport_tutorial my_subscriber
 
 
1

如果有view的话,运行上诉命令之后就会出现你发布的image图片.

二. 使用自己的数据集做rosbag包

方法一:使用ros进行数据发布

目前我找到了两种方法实现ros-bag的生成,第一种就是接下来要详细介绍的直接使用ros的publisher节点进行数据的发布,然后使用rosbag进行录制.
这个部分主要是根据第一部分的tutorial进行改进的,主要是针对自己的数据集对publisher.cpp文件进行了改进.改进的代码我直接贴在下面:

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <vector>
#include <glob.h>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
using namespace std;

vector<string> getFiles(string dirc){
    vector<string> files;
    struct dirent *ptr;
    char base[1000];
    DIR *dir;
    dir = opendir(dirc.c_str());
    /*
    if(dir == NULL){
        perror("open dir error ...");
        exit(1);
    }*/
    while((ptr = readdir(dir)) != NULL){
        if(ptr->d_type == 8)//it;s file
        {
            files.push_back(dirc+'/'+ptr->d_name);
        }

        else if(ptr->d_type == 10)//link file
            continue;
        else if(ptr->d_type == 4){
            files.push_back(ptr->d_name);
        }
    }
    closedir(dir);
    sort(files.begin(),files.end());
    for(size_t i = 0; i < files.size();++i){
        cout << files[i] << endl;
    }
    return files;
}

void origin(int argc,char** argv){
  /*
  **这个函数和官网提供的一张照片的tutorial很类似,只用于测试一张照片的情况.
  **前面部分是基本的ros发布数据的操作,在这里不多说.
  */
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1);
    cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cout << argv[1] << endl;
    cv::waitKey(30);
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    ros::Rate loop_rate(5);
    while (nh.ok()) {
        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        }
}


void directory(int argc,char** argv){
/*
**这个函数针对的是大量数据的
*/
    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("camera/image", 1);
    vector<string> result = getFiles(argv[1]);// /home/***/***/dataset/Mars/*.pgm

    //the argv is the url of the image,may we can use that for all images
    ros::Rate loop_rate(16);
    for(size_t i = 0; i < result.size();++i){
        if(!nh.ok())
            break;
        ostringstream stringStream;
        stringStream << result[i];
        cv::Mat image = cv::imread(stringStream.str(),CV_LOAD_IMAGE_COLOR);
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
        msg->header.stamp = ros::Time(111);//这里的时间戳换成你自己的,需要自己再写一个读取时间戳的函数,注意和image进行对应.一般时间戳文件也包含在图片的文件夹中.

        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

int main(int argc, char** argv)
{
  DIR *dir;
  dir = opendir(argv[1]);
  if(dir == NULL)
      origin(argc,argv);
  else
      directory(argc,argv);

}
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

以上是publisher的部分,然后到subscriber部分还是之前tutorial的代码,然后按照之前的操作进行,最后使用rosbag record -a(-a表示录制全部发布的话题,其它参数参考官方文档).
以上就是使用ros生成自己数据集的rosbag的方法.

方法二.使用rosbag的包,直接写入

这个部分是参考大神的代码
我只是当了以下搬运工,将其应用到自己的数据集上了.
以下是我的代码,因为我需要整合IMU数据和image的数据,而且两者都是有时间戳timestamp的.
以下是我的代码:

import cv2
import time, sys, os
from ros import rosbag
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image,Imu
from geometry_msgs.msg import Vector3

# import ImageFile
from PIL import ImageFile
from PIL import Image as ImagePIL

def CompSortFileNamesNr(f):
    g = os.path.splitext(os.path.split(f)[1])[0] #get the file of the
    numbertext = ''.join(c for c in g if c.isdigit())
    return int(numbertext)

def ReadImages(filename):
    '''Generates a list of files from the directory'''
    print("Searching directory %s" % dir)
    all = []
    left_files = []
    right_files = []
    files = os.listdir(filename)
    for f in sorted(files):
        if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg', '.pgm']:
            '''
            if 'left' in f or 'left' in path:
                left_files.append(os.path.join(path, f))
            elif 'right' in f or 'right' in path:
                right_files.append(os.path.join(path, f))
            '''
            all.append(os.path.join(filename, f))
    return all

def ReadIMU(filename):
    '''return IMU data and timestamp of IMU'''
    file = open(filename,'r')
    all = file.readlines()
    timestamp = []
    imu_data = []
    for f in all:
        line = f.rstrip('\n').split(' ')
        timestamp.append(line[0])
        imu_data.append(line[1:])
    return timestamp,imu_data


def CreateBag(args):#img,imu, bagname, timestamps
    '''read time stamps'''
    imgs = ReadImages(args[0])
    imagetimestamps=[]
    imutimesteps,imudata = ReadIMU(args[1]) #the url of  IMU data
    file = open(args[3], 'r')
    all = file.readlines()
    for f in all:
        imagetimestamps.append(f.rstrip('\n').split(' ')[1])
    file.close()
    '''Creates a bag file with camera images'''
    if not os.path.exists(args[2]):
    os.system(r'touch %s' % args[2])
    bag = rosbag.Bag(args[2], 'w')

    try:
        for i in range(len(imudata)):
            imu = Imu()
            angular_v = Vector3()
            linear_a = Vector3()
            angular_v.x = float(imudata[i][0])
            angular_v.y = float(imudata[i][1])
            angular_v.z = float(imudata[i][2])
            linear_a.x = float(imudata[i][3])
            linear_a.y = float(imudata[i][4])
            linear_a.z = float(imudata[i][5])
            imuStamp = rospy.rostime.Time.from_sec(float(imutimesteps[i]))
            imu.header.stamp=imuStamp
            imu.angular_velocity = angular_v
            imu.linear_acceleration = linear_a

            bag.write("IMU/imu_raw",imu,imuStamp)

        for i in range(len(imgs)):
            print("Adding %s" % imgs[i])
            fp = open(imgs[i], "r")
            p = ImageFile.Parser()

            '''read image size'''
            imgpil = ImagePIL.open(imgs[0])
            width, height = imgpil.size
            # print "size:",width,height

            while 1:
                s = fp.read(1024)
                if not s:
                    break
                p.feed(s)

            im = p.close()

            Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))

            '''set image information '''
            Img = Image()

            Img.header.stamp = Stamp
            Img.height = height
            Img.width = width
            Img.header.frame_id = "camera"

            '''for mono8'''
            Img.encoding = "mono8"
            Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata]
            Img.step = Img.width


            Img.data = Img_data
            bag.write('camera/image_raw', Img, Stamp)
    finally:
        bag.close()

if __name__ == "__main__":
      print(sys.argv)
      CreateBag(sys.argv[1:])
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124

这个直接使用python运行就可以了,不需要ros的帮助,但是前提是你已经装了ros,而且将ros的环境变量类的写进了bash文件.
运行命令:

python img2bag_Mars_imu.py /home/***/***/dataset/Mars/Regular_1/img_data/left /home/***/***/dataset/Mars/Regular_1/imu_data.txt /home/***/***/test.bag /home/***/***/dataset/Mars/Regular_1/img_data/timestamps.txt
 
 
1

第一个参数是image的文件夹名称,第二个参数是IMU的数据,第三个参数是你要生成的rosbag的名称,第四个是image的时间戳.
这种方法最先在这里看到.

以上,就是自己折腾了将近两天的结果了,虽然最后测试的效果不是很好,至少数据进行了处理.希望对大家有用.

转载地址： https://blog.csdn.net/LilyNothing/article/details/79206938
参考：https://blog.csdn.net/ziqian0512/article/details/77371361