00001
00002 """
00003 Created from image_sequence_publisher.py
00004 Mathieu Labbe
00005
00006 Copyright (c) 2012,
00007 Systems, Robotics and Vision Group
00008 University of the Balearican Islands
00009 All rights reserved.
00010
00011 Redistribution and use in source and binary forms, with or without
00012 modification, are permitted provided that the following conditions are met:
00013 * Redistributions of source code must retain the above copyright
00014 notice, this list of conditions and the following disclaimer.
00015 * Redistributions in binary form must reproduce the above copyright
00016 notice, this list of conditions and the following disclaimer in the
00017 documentation and/or other materials provided with the distribution.
00018 * Neither the name of Systems, Robotics and Vision Group, University of
00019 the Balearican Islands nor the names of its contributors may be used to
00020 endorse or promote products derived from this software without specific
00021 prior written permission.
00022
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 """
00034
00035
00036 PKG = 'bag_tools'
00037
00038 import roslib; roslib.load_manifest(PKG)
00039 import rospy
00040 import sensor_msgs.msg
00041 import cv_bridge
00042 import camera_info_parser
00043 import glob
00044 import cv
00045 import numpy as np
00046 import re
00047
00048 def natural_sort(l):
00049 convert = lambda text: int(text) if text.isdigit() else text.lower()
00050 alphanum_key = lambda key: [ convert(c) for c in re.split('([0-9]+)', key) ]
00051 return sorted(l, key = alphanum_key)
00052
00053 def collect_image_files(image_dir,file_pattern):
00054 images = glob.glob(image_dir + '/' + str(file_pattern))
00055 images = natural_sort(images)
00056 return images
00057
00058 def playback_images(image_dir_l, image_dir_r, file_pattern, camera_info_file_l, camera_info_file_r, publish_rate):
00059 frame_id = "/camera"
00060 if camera_info_file_l != "":
00061 cam_info_l = camera_info_parser.parse_yaml(camera_info_file_l)
00062 frame_id = cam_info_l.header.frame_id
00063 publish_cam_info_l = True
00064 else:
00065 publish_cam_info_l = False
00066 if camera_info_file_r != "":
00067 cam_info_r = camera_info_parser.parse_yaml(camera_info_file_r)
00068 publish_cam_info_r = True
00069 else:
00070 publish_cam_info_r = False
00071
00072 image_files_l = collect_image_files(image_dir_l, file_pattern)
00073 image_files_r = collect_image_files(image_dir_r, file_pattern)
00074 rospy.loginfo('Found %i left images.',len(image_files_l))
00075 rospy.loginfo('Found %i right images.',len(image_files_r))
00076
00077 bridge = cv_bridge.CvBridge()
00078 rate = rospy.Rate(publish_rate)
00079 image_publisher_l = rospy.Publisher('stereo_camera/left/image_raw', sensor_msgs.msg.Image, queue_size = 5)
00080 image_publisher_r = rospy.Publisher('stereo_camera/right/image_raw', sensor_msgs.msg.Image, queue_size = 5)
00081 if publish_cam_info_l:
00082 cam_info_publisher_l = rospy.Publisher('stereo_camera/left/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5)
00083 if publish_cam_info_r:
00084 cam_info_publisher_r = rospy.Publisher('stereo_camera/right/camera_info', sensor_msgs.msg.CameraInfo, queue_size = 5)
00085
00086 rospy.loginfo('Starting playback.')
00087 for image_file_l, image_file_r in zip(image_files_l, image_files_r):
00088 if rospy.is_shutdown():
00089 break
00090 now = rospy.Time.now()
00091 image_l = cv.LoadImage(image_file_l)
00092 image_r = cv.LoadImage(image_file_r)
00093 image_msg = bridge.cv2_to_imgmsg(np.asarray(image_l[:,:]), encoding='bgr8')
00094 image_msg.header.stamp = now
00095 image_msg.header.frame_id = frame_id
00096 image_publisher_l.publish(image_msg)
00097 image_msg = bridge.cv2_to_imgmsg(np.asarray(image_r[:,:]), encoding='bgr8')
00098 image_msg.header.stamp = now
00099 image_msg.header.frame_id = frame_id
00100 image_publisher_r.publish(image_msg)
00101 if publish_cam_info_l:
00102 cam_info_l.header.stamp = now
00103 cam_info_publisher_l.publish(cam_info_l)
00104 if publish_cam_info_r:
00105 cam_info_r.header.stamp = now
00106 cam_info_r.header.frame_id = frame_id
00107 cam_info_publisher_r.publish(cam_info_r)
00108 rate.sleep()
00109 rospy.loginfo('No more images left. Stopping.')
00110
00111 if __name__ == "__main__":
00112 rospy.init_node('image_sequence_publisher')
00113 try:
00114 image_dir_l = rospy.get_param("~image_dir_left")
00115 image_dir_r = rospy.get_param("~image_dir_right")
00116 file_pattern = rospy.get_param("~file_pattern")
00117 camera_info_file_l = rospy.get_param("~camera_info_file_left", "")
00118 camera_info_file_r = rospy.get_param("~camera_info_file_right", "")
00119 frequency = rospy.get_param("~frequency", 10)
00120 playback_images(image_dir_l, image_dir_r, file_pattern, camera_info_file_l, camera_info_file_r, frequency)
00121 except KeyError as e:
00122 rospy.logerr('Required parameter missing: %s', e)
00123 except Exception, e:
00124 import traceback
00125 traceback.print_exc()