ros mavros stereo读取rosbag并记录IMU和图片到文件夹

R12DS对码及信号切换教程

长安对马

# coding:utf-8
import os
import sys
import rosbag
import roslib;  # roslib.load_manifest(PKG)
import rospy
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image


def CreateFolder(FolderPath):
    folder = os.path.exists(FolderPath)
    if not folder:  # 判断是否存在文件夹如果不存在则创建为文件夹
        os.makedirs(FolderPath)  # makedirs 创建文件时如果路径不存在会创建这个路径
        print("--- new folder... ---")
    else:
        print("--- There is this folder! ---")


BagPath = sys.argv[1]
bag = rosbag.Bag(BagPath, "r")

# SavePath = "./ImgRight"
# CreateFolder(SavePath)
# bridge = CvBridge()
# TopicData = bag.read_messages('/camera/right/image_raw')
# for topic, msg, t in TopicData:
# cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
# sImgName = "/" + str(t) + ".jpg" # 图像命名：时间戳.jpg
# print(sImgName)
# # image_name = SavePath+" "+ str(Idx) + ".jpg" # 图像命名：时间戳.jpg
# image_name = SavePath + sImgName # 图像命名：时间戳.jpg
# cv2.imwrite(image_name, cv_image) # 保存；
# cv2.waitKey(1)

ImuFile = open("./Imu.txt", 'w')
TopicData = bag.read_messages('/mavros/imu/data_raw')
for topic, msg, t in TopicData:
    AccX = "%.6f" % msg.linear_acceleration.x
    AccY = "%.6f" % msg.linear_acceleration.y
    AccZ = "%.6f" % msg.linear_acceleration.z
    AngX = "%.6f" % msg.angular_velocity.x
    AngY = "%.6f" % msg.angular_velocity.y
    AngZ = "%.6f" % msg.angular_velocity.z
    Time = "%19.9f" % msg.header.stamp.to_sec()
    ImuEpoc = Time + " " + AccX + " " + AccY + " " + AccZ + " " + AngX + " " + AngY + " " + AngZ + "\n"
    ImuFile.write(ImuEpoc)
ImuFile.close()