bag_images_extract.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # This little program will extract some images from a bag file
00004 # and write them out in .pnm format.  The usage is:
00005 #
00006 #    bag_images_extract.py bag_file_name [images_directory]
00007 
00008 import rosbag
00009 import rospy
00010 import cv
00011 from sensor_msgs.msg import Image
00012 from cv_bridge import CvBridge, CvBridgeError
00013 import sys
00014 
00015 # Perform command line argument processing first:
00016 arguments = sys.argv
00017 del arguments[0]
00018 arguments_size = len(arguments)
00019 if arguments_size == 0:
00020     print("Usage: bag_images_extract.py bag_file_name [images_directory]")
00021 else:
00022 
00023     # Default directory is "."
00024     directory = "."
00025     if arguments_size >= 2:
00026         directory = arguments[1]
00027 
00028     # Open the bag file:
00029     bag_file_name = arguments[0]
00030     bag = rosbag.Bag(bag_file_name)
00031     bridge = CvBridge()
00032 
00033     # Sweep through messages that match *topic_name*:
00034     topic_name = "/fiducials_localization/interesting_images"
00035     index = 0
00036     previous_time = None
00037     for topic, msg, t in bag.read_messages(topic_name):
00038         # Extract *cv_image* in RGB8 mode:
00039         index += 1
00040         try:
00041             cv_image = bridge.imgmsg_to_cv(msg, desired_encoding="rgb8")
00042         except CvBridgeError, e:
00043             print e
00044 
00045         # Save the image
00046         cv.SaveImage("{0}/Image{1:03d}.pnm".format(directory, index), cv_image)
00047 
00048         # Print out time changes:
00049         if previous_time == None:
00050             previous_time = t
00051         dt = t - previous_time
00052         previous_time = t
00053         print("[{0}]:{1}".format(index, dt))
00054 


fiducial_lib
Author(s): Wayne Gramlich
autogenerated on Thu Jun 6 2019 18:08:04