rosbag Convert all raw images to compressed images. All other topics are copied from input bag to output bag

rosbag_images_to_compressedimages.py

#!/usr/bin/env python
"""Convert all raw images to compressed images. All other topics are copied from input bag
to output bag

USAGE:::
python image_raw_to_compressed.py _bag:=/home/ttekep/data/gtsam/random2.bag

"""
​
import rospy
import rosbag
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
import cv2
import os
​
def main():
    """main loop to process data"""
    rospy.init_node('image_raw_to_compressed')
    bridge = CvBridge()
    # parameters
    bag_file_path = rospy.get_param('~bag', '')
    if not bag_file_path:
        rospy.loginfo("Please specify the bag (file path) to convert.")
        rospy.loginfo("Usage: image_raw_to_compressed.py _bag:=<bag_file_path>")
        rospy.signal_shutdown("Invalid input.")
    # read input
    bag_in = rosbag.Bag(bag_file_path)
    # get topics and msg types
    raw_image_type = "sensor_msgs/Image"
    raw_image_topic_msg_count = {}
    for topic, info in bag_in.get_type_and_topic_info()[1].items():
        if info[0] == raw_image_type:
            raw_image_topic_msg_count[topic] = info[1]
    if not raw_image_topic_msg_count:
        rospy.loginfo("The bag {} does not contain topics with message type {}".format(
            bag_file_path, raw_image_type
        ))
        rospy.loginfo("Nothing to convert. Exiting..")
        rospy.signal_shutdown("No raw images found in the bag file.")
        return
    else:
        rospy.loginfo("Found raw images:")
        for topic, msg_count in raw_image_topic_msg_count.items():
            rospy.loginfo("Topic: {}, msg count: {}".format(topic, msg_count))
    # prepare output
    output_bag_file_path = bag_file_path.replace('.bag', '_compressed.bag')
    bag_out = rosbag.Bag(output_bag_file_path, mode='w')
    # do the conversion
    for topic, msg, t in bag_in.read_messages():
        if rospy.is_shutdown():
            rospy.loginfo("CTRL+C pressed. Exiting..")
            break
        if topic in raw_image_topic_msg_count:
            try:
                image_np = bridge.imgmsg_to_cv2(msg, "bgr8")
            except CvBridgeError as e:
                rospy.logerr("Failed to convert image. Error: {}".format(e))
            c_msg = CompressedImage()
            c_msg.header.stamp = msg.header.stamp
            c_msg.header.seq = msg.header.seq
            c_msg.header.frame_id = msg.header.frame_id
            c_msg.format = "jpeg"
            c_msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
            c_topic = os.path.join(topic, "compressed")
            bag_out.write(c_topic, c_msg, t=t)
        else:
            bag_out.write(topic, msg, t=t)
    bag_in.close()    
    bag_out.close()
    rospy.loginfo("All done!")
    rospy.loginfo("Bag file with compressed images: {}".format(output_bag_file_path))
​
if __name__ == '__main__':
    main()