提取ROS bag文件中的图像、IMU和GPS数据

?ROS bag 文件对于存储和重播机器人传感器数据非常有用。本代码旨在从 ROS bag 文件中提取图像、IMU 和 GPS 数据,并将其保存到磁盘以进行进一步分析。

具体来说,代码完成了以下工作:

1. 检查输出图像目录是否存在,如果不存在则创建目录

2. 初始化 ROS-OpenCV 图像转换桥

3. 定义图像保存函数,将 ROS Image 消息转换为 OpenCV 图像并保存

4. 遍历 bag 文件,提取以下主题的数据:

? ? - /camera/color/image_raw : 保存为图像文件
? ? - /chcnav_demo/imu : 将 IMU 数据保存到文本文件?
? ? - /chcnav_demo/fix : 将 GPS 数据(经纬度、高度)保存到文本文件

保存后的文件可以用来进行视觉和导航算法的开发与测试。

总的来说,本代码通过解析 ROS bag,高效地组织和存储了不同传感器的数据,为后续算法研究提供了基础数据支持。

#!/usr/bin/env python

import os
import rosbag
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

bag_name = "2023-05-24-12-38-32"
base_path = "/media/fairlee/389409F29409B402/RoboteData/2023_05_24/"

bag_file = base_path + bag_name + ".bag"
output_directory = base_path + bag_name + "/images/"
output_file_imu = base_path + bag_name + "/imu_data.txt"
output_file_fix = base_path + bag_name + "/fix_data.txt"

# Check if the directory exists, if not create it
if not os.path.exists(output_directory):
    os.makedirs(output_directory)

bridge = CvBridge()

def save_image(msg, image_count):
    try:
        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
    except CvBridgeError as e:
        print(e)
        return

    image_filename = output_directory + "{:06d}.png".format(image_count)
    cv2.imwrite(image_filename, cv_image)
    print("Saved image:", image_filename)


latitude_list = []
longitude_list = []
altitude_list = []

with rosbag.Bag(bag_file, 'r') as bag:
    image_count = 0
    imu_count = 0
    for topic, msg, t in bag.read_messages():
        if topic == "/camera/color/image_raw":
            save_image(msg, image_count)
            image_count += 1
        elif topic == "/chcnav_demo/imu":
            with open(output_file_imu, 'a') as f:
                orientation_x = msg.orientation.x
                orientation_y = msg.orientation.y
                orientation_z = msg.orientation.z
                orientation_w = msg.orientation.w
                angular_velocity_x = msg.angular_velocity.x
                angular_velocity_y = msg.angular_velocity.y
                angular_velocity_z = msg.angular_velocity.z
                linear_acceleration_x = msg.linear_acceleration.x
                linear_acceleration_y = msg.linear_acceleration.y
                linear_acceleration_z = msg.linear_acceleration.z

                f.write("{} {} {} {} {} {} {} {} {} {}\n".format(orientation_x, orientation_y, orientation_z,
                                                                 orientation_w, angular_velocity_x, angular_velocity_y,
                                                                 angular_velocity_z, linear_acceleration_x,
                                                                 linear_acceleration_y, linear_acceleration_z))
                imu_count += 1
                print("Extracted data for IMU message", imu_count)
        elif topic == "/chcnav_demo/fix":
            latitude = msg.latitude
            longitude = msg.longitude
            altitude = msg.altitude

            latitude_list.append(str(latitude))
            longitude_list.append(str(longitude))
            altitude_list.append(str(altitude))

    if len(latitude_list) == len(longitude_list) == len(altitude_list):
        with open(output_file_fix, 'w') as f:
            for i in range(len(latitude_list)):
                f.write("{} {} {}\n".format(latitude_list[i], longitude_list[i], altitude_list[i]))
        print("Data extraction for fix completed.")
    else:
        print("Error: The lists have different lengths.")

请注意
上面的代码是基于Ubuntu 18.04
最终结果如下（我是在win10系统下查看的）