背景

采集数据的时候没有想到之后要是用ros的话还得转rosbag的事，所以还得后处理一下，自己也搜了一些博客，根据其他博客的内容进行了修改，分别完成了python的imu、图片和GNSS结果文件转rosbag和C++ imu、图片以及GNSS相关信息转rosbag(GNSS的还没完成)，代码的github地址是SLAM-Tool

python代码实现

代码实现还是比较简单的，只要能和sensor_msg中的类型对应上就行，代码还是比较好懂的

python转IMU

# -*- coding: UTF-8 -*-
import rosbag
import sys
import os
import numpy as np
from sensor_msgs.msg import Imu
import rospy
from geometry_msgs.msg import Vector3def readIMU(imu_path):timestamps = []wxs = []wys = []wzs = []axs = []ays = []azs = []fin = open(imu_path, 'r')fin.readline()line = fin.readline().strip()while line:parts = line.split( )ts = float(parts[0])+315964800+604800*2258-8*3600wx = float(parts[1])wy = float(parts[2])wz = float(parts[3])ax = float(parts[4])ay = float(parts[5])az = float(parts[6])timestamps.append(ts)wxs.append(wx)wys.append(wy)wzs.append(wz)axs.append(ax)ays.append(ay)azs.append(az)line = fin.readline().strip()return timestamps, wxs, wys, wzs, axs, ays, azsif __name__ == '__main__':imu_path = sys.argv[1]  # IMU数据文件路径imu_topic_name = sys.argv[2]    # Topic名称bag_path = sys.argv[3]  # 输出Bag路径bag_out = rosbag.Bag(bag_path,'w')imu_ts, wxs, wys, wzs, axs, ays, azs = readIMU(imu_path)imu_msg = Imu()angular_v = Vector3()linear_a = Vector3()for i in range(len(imu_ts)):imu_ts_ros = rospy.rostime.Time.from_sec(imu_ts[i])imu_msg.header.stamp = imu_ts_rosangular_v.x = wxs[i]angular_v.y = wys[i]angular_v.z = wzs[i]linear_a.x = axs[i]linear_a.y = ays[i]linear_a.z = azs[i]imu_msg.angular_velocity = angular_vimu_msg.linear_acceleration = linear_abag_out.write(imu_topic_name, imu_msg, imu_ts_ros)print('imu:',i,'/',len(imu_ts))bag_out.close()

因为文件存的是GPS周秒，所以和ROS的时间之间需要进行一个转换，ts = float(parts[0])+315964800+604800*2258-8*3600这一行就是用来干这个的，315964800是从网上找的两个时间的一个差，然后2258就是周数了
IMU这里用的是bag_out = rosbag.Bag(bag_path,‘w’)，可以简单理解为就是新建一个bag文件。因为我在采集数据的时候IMU一般都是时间最长的，所以就以IMU为基础写一个新的bag，图像和GNSS后加进去

python转GNSS

# -*- coding: UTF-8 -*-
import rosbag
import sys
import os
import numpy as np
from sensor_msgs.msg import NavSatFix
import math
import rospy
from geometry_msgs.msg import Vector3def readGNSS(imu_path):timestamps = []lats = []lons = []alts = []fin = open(imu_path, 'r')fin.readline()for i in range(1,23) :line = fin.readline().strip()i=i+1while line:parts = line.split( )ts  = float(parts[0])+315964800+604800*2258-8*3600lat = float(parts[2]) * 180 / math.pilon = float(parts[3]) * 180 / math.pialt = float(parts[4])timestamps.append(ts)lats.append(lat)lons.append(lon)alts.append(alt)line = fin.readline().strip()return timestamps, lats, lons, altsif __name__ == '__main__':gnss_path = sys.argv[1]  # GNSS数据文件路径gnss_topic_name = sys.argv[2]    # Topic名称bag_path = sys.argv[3]  # 输出Bag路径bag_out = rosbag.Bag(bag_path,'a')gnss_ts, lats, lons, alts = readGNSS(gnss_path)gnss_msg = NavSatFix()blh = Vector3()for i in range(len(gnss_ts)):gnss_ts_ros = rospy.rostime.Time.from_sec(gnss_ts[i])gnss_msg.header.stamp = gnss_ts_rosgnss_msg.latitude = lats[i]gnss_msg.longitude = lons[i]gnss_msg.altitude = alts[i]bag_out.write(gnss_topic_name, gnss_msg, gnss_ts_ros)print('gnss:',i,'/',len(gnss_ts))bag_out.close()

bag_out = rosbag.Bag(bag_path,'a')这里的a是append，就是在现有的bag上继续增加内容

image转bag

# coding=utf-8
import rosbag
import sys
import os
import numpy as np
import cv2
from cv_bridge import CvBridge
import rospydef findFiles(root_dir, filter_type, reverse=False):"""在指定目录查找指定类型文件 -> paths, names, files:param root_dir: 查找目录:param filter_type: 文件类型:param reverse: 是否返回倒序文件列表,默认为False:return: 路径、名称、文件全路径"""separator = os.path.seppaths = []names = []files = []for parent, dirname, filenames in os.walk(root_dir):for filename in filenames:if filename.endswith(filter_type):paths.append(parent + separator)names.append(filename)for i in range(paths.__len__()):files.append(paths[i] + names[i])print(names.__len__().__str__() + " files have been found.")paths = np.array(paths)names = np.array(names)files = np.array(files)index = np.argsort(files)paths = paths[index]names = names[index]files = files[index]paths = list(paths)names = list(names)files = list(files)if reverse:paths.reverse()names.reverse()files.reverse()return paths, names, filesdef ReadTimestamps( ):# 这里需修改timestamps文件的路径file = open("/home/zyp/DATA/106_107/107_1/image_01_dw/timestamps.txt",'r')all = file.readlines()timestamp = []index = 0for f in all:index = index + 1if index == 1:continueline = float(f.rstrip('\n'))timestamp.append(line+315964800+604800*2258-8*3600)print("Total add %i images!"%(index))return timestampif __name__ == '__main__':img_dir = sys.argv[1]   # 影像所在文件夹路径img_type = sys.argv[2]  # 影像类型topic_name = sys.argv[3]    # Topic名称bag_path = sys.argv[4]  # 输出Bag路径bag_out = rosbag.Bag(bag_path,'a')cb = CvBridge()paths, names, files = findFiles(img_dir,img_type)timestamp = ReadTimestamps()for i in range(len(files)):frame_img = cv2.imread(files[i], flags = 0)# timestamp = int(names[i].split(".")[0])/10e8ts = timestamp[i-1]ros_ts = rospy.rostime.Time.from_sec(ts)ros_img = cb.cv2_to_imgmsg(frame_img,encoding='mono8')ros_img.header.stamp = ros_ts# print(ros_img.header.stamp)bag_out.write(topic_name,ros_img,ros_ts)print('image:',i+1,'/',len(files))bag_out.close()

这里面有一个findFiles函数，也很好理解，就是用来找到文件夹下的全部文件的；以及还有一个ReadTimestamps函数，这个是用来读取时间戳的，因为图像本身没有带时间信息，所以需要再有一个单独的文件来获取时间戳。
然后image的话是先用opencv读取一下，再用cb(cv_bridge)转到imgmsg类型

C++代码实现

目前只用C++写了转imu和image的代码，之后可能会继续写转换GNSS相关数据的代码。这个代码在仓库的ROS/src/data_to_rosbag/src里

C++转IMU

/* imu数据转bag，bag包以IMU数据为基础(因为通常imu数据是时间最长的) */
void imu2bag(rosbag::Bag &bag, const std::string imuFile, const std::string outBag, int gpsWeek)
{std::ifstream file(imuFile);if (!file.is_open()){ROS_ERROR_STREAM("Failed to open file!");exit(1);}bag.open(outBag, rosbag::bagmode::Write);std::string line;std::getline(file, line);   // 跳过第一行while (std::getline(file, line)){// 将每行数据分割为各个字段std::istringstream iss(line);double time, gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z;if (!(iss >> time >> gyro_x >> gyro_y >> gyro_z >> accel_x >> accel_y >> accel_z)){ROS_WARN_STREAM("Failed to parse line: " << line);continue;}// 创建IMU消息sensor_msgs::Imu imu_msg;// 315964800是GPS起始时间和计算机起始时间的一个固定差值time = time + 315964800 + 604800 * gpsWeek - 8 * 3600;imu_msg.header.stamp = ros::Time(time);imu_msg.angular_velocity.x = gyro_x;imu_msg.angular_velocity.y = gyro_y;imu_msg.angular_velocity.z = gyro_z;imu_msg.linear_acceleration.x = accel_x;imu_msg.linear_acceleration.y = accel_y;imu_msg.linear_acceleration.z = accel_z;// 写入ROSbag文件bag.write("/imu0", ros::Time(time), imu_msg);}bag.close();file.close();std::cout << "imu data convert finished!" << std::endl;
}

我是把读文件，以及写bag的操作都放在子函数里了，这样主函数只需要简单地一行调用即可

imu2bag(bag, imuFile, outBag, gpsWeek);

C++转image

/* image转bag */
void image2bag(rosbag::Bag &bag, const std::string &strPathToImage, const std::string outBag, int gpsWeek, std::string LorR)
{std::ifstream fTime;std::string strPathToTime = strPathToImage + "/timestamps.txt";fTime.open(strPathToTime);if (!fTime.is_open()){ROS_ERROR_STREAM("Failed to open timestamp file!");exit(1);}// 保存时间戳std::vector<double> vTimeStamp;while(!fTime.eof()){std::string s;getline(fTime, s);if (!s.empty()) {std::stringstream ss;ss << s;double t;ss >> t;vTimeStamp.push_back(t);}}fTime.close();// 开始转换图片bag.open(outBag, rosbag::bagmode::Append);double time;cv::Mat image;for (int i =0; i < vTimeStamp.size(); i++){std::stringstream ss;ss << std::setfill('0') << std::setw(10) << i;std::string imageName = strPathToImage + "/data/" + ss.str() + ".png"; image = cv::imread(imageName, CV_LOAD_IMAGE_GRAYSCALE);time = vTimeStamp[i] + 315964800 + 604800 * gpsWeek - 8 * 3600;sensor_msgs::ImagePtr rosImg;cv_bridge::CvImage rosImage;rosImage.encoding = "mono8";rosImage.image = image;rosImg = rosImage.toImageMsg();rosImg->header.stamp = ros::Time(time);bag.write(LorR, ros::Time(time), rosImg);std::cout << "done: " << i << "/" << vTimeStamp.size() << std::endl; }bag.close();std::cout << "image convert finished!" << std::endl;
}

这里是把时间戳读取、图片读取、图片转换都放在一个函数里了，所以外部调用也是一行

image2bag(bag, LstrPathToImage, outBag, gpsWeek, "/cam0");

因为我是双目，所以这里把topic也作为一个参数传入了，其实IMU感觉也改成这样传入topic比较好，这样就没有写死

参考致谢

博客1

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！