ros图像话题保存图像

将ros图像话题中的图像保存下来，代码如下：

import rosbag
from cv_bridge import CvBridge
import cv2
import argparse
import os
import datetime

def save_images_from_bag(bag_file, image_topic, output_folder, prefix):
    # 检查并创建输出文件夹
    if os.path.exists(output_folder):
        print(f"Output folder {output_folder} already exists. Deleting it...")
        os.system(f"rm -r {output_folder}")
    os.makedirs(output_folder)
    print(f"Created output folder {output_folder}")

    bag = rosbag.Bag(bag_file, 'r')
    bridge = CvBridge()

    for topic, msg, t in bag.read_messages(topics=[image_topic]):
        if topic == image_topic:
            try:
                cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
                timestamp = datetime.datetime.fromtimestamp(msg.header.stamp.to_sec()).strftime('%Y%m%d_%H%M%S')
                milliseconds = msg.header.stamp.to_nsec() // 1000000 % 1000  # Convert nanoseconds to milliseconds
                image_filename = f"{output_folder}/{prefix}_{timestamp}{milliseconds:03d}.png"
                cv2.imwrite(image_filename, cv_image)
                print(f"Saved {image_filename}")
            except Exception as e:
                print(f"Failed to save image: {e}")

    bag.close()


if __name__ == '__main__':
    # 使用argparse处理命令行参数
    parser = argparse.ArgumentParser(description="Save images from a ROS bag file.")
    parser.add_argument("--bag_file", help="Path to the ROS bag file.")
    parser.add_argument("--image_topic", help="Name of the image topic.")
    parser.add_argument("--output_folder", help="Path to the output folder.")
    parser.add_argument("--prefix", help="Prefix for the image filenames.")
    args = parser.parse_args()

    # 调用函数
    save_images_from_bag(args.bag_file, args.image_topic, args.output_folder, args.prefix)