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()