根据图像和imu生成ros1的bag包的脚本

记录一下挺有用的脚本：

#!/usr/bin/env python

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

def read_images(image_directory, bag):
    image_files = sorted(os.listdir(image_directory))
    for image_file in image_files:
        image_path = os.path.join(image_directory, image_file)
        image = cv2.imread(image_path)
        # Convert image to ROS message
        image_msg = CvBridge().cv2_to_imgmsg(image, encoding="bgr8")
        basename = os.path.basename(image_file)
        timestamp_str = basename.rsplit('.', 1)[0]
        
        image_msg.header.stamp = rospy.Time.from_sec(float(timestamp_str))  # Assuming timestamp is in the filename
        print("handle img:", image_file)
        bag.write('/image_topic', image_msg, image_msg.header.stamp)

def read_imu(imu_file_path, bag):
    with open(imu_file_path, 'r') as f:
        lines = f.readlines()

        for line in lines:
            data = line.strip().split(',')
            if len(data) == 7:
                # Assuming the format is timestamp, acc_x, acc_y, acc_z, gyr_x, gyr_y, gyr_z
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.from_sec(float(data[0]))  # Assuming timestamp is the first value
                imu_msg.linear_acceleration.x = float(data[1])
                imu_msg.linear_acceleration.y = float(data[2])
                imu_msg.linear_acceleration.z = float(data[3])
                imu_msg.angular_velocity.x = float(data[4])
                imu_msg.angular_velocity.y = float(data[5])
                imu_msg.angular_velocity.z = float(data[6])
                bag.write('/imu_topic', imu_msg, imu_msg.header.stamp)

if __name__ == '__main__':
    rospy.init_node('bag_generator_node', anonymous=True)
    rate = rospy.Rate(10)  # Adjust the publishing rate as needed

    image_directory = '/home/xxx/dataset/small_images/'  # Replace with your image directory
    imu_directory = '/home/xxx/dataset/imus.txt'  # Replace with your IMU data directory

    bag_path = '/home/xxx/dataset/your_output_bag.bag'  # Replace with the desired output bag file path

    with rosbag.Bag(bag_path, 'w') as bag:
        read_images(image_directory, bag)
        read_imu(imu_directory, bag)