00001
00002
00003 """ avi2ros.py - Version 0.1 2011-04-28
00004
00005 Read in an AVI video file and republish as a ROS Image topic.
00006
00007 Created for the Pi Robot Project: http://www.pirobot.org
00008 Copyright (c) 2011 Patrick Goebel. All rights reserved.
00009
00010 This program is free software; you can redistribute it and/or modify
00011 it under the terms of the GNU General Public License as published by
00012 the Free Software Foundation; either version 2 of the License, or
00013 (at your option) any later version.
00014
00015 This program is distributed in the hope that it will be useful,
00016 but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00018 GNU General Public License for more details at:
00019
00020 http://www.gnu.org/licenses/gpl.html
00021
00022 """
00023
00024 import roslib
00025 roslib.load_manifest('ros2opencv')
00026 import sys
00027 import rospy
00028 import cv
00029 from std_msgs.msg import String
00030 from sensor_msgs.msg import Image
00031 from cv_bridge import CvBridge, CvBridgeError
00032
00033 class AVI2ROS:
00034 def __init__(self):
00035 rospy.init_node('avi2ros', anonymous=True)
00036
00037 self.input = rospy.get_param("~input", "")
00038 image_pub = rospy.Publisher("output", Image)
00039
00040 self.fps = rospy.get_param("~fps", 25)
00041 self.loop = rospy.get_param("~loop", False)
00042 self.width = rospy.get_param("~width", "")
00043 self.height = rospy.get_param("~height", "")
00044 self.start_paused = rospy.get_param("~start_paused", False)
00045 self.show_text = True
00046
00047 rospy.on_shutdown(self.cleanup)
00048
00049 video = cv.CaptureFromFile(self.input)
00050 fps = int(cv.GetCaptureProperty(video, cv.CV_CAP_PROP_FPS))
00051
00052 """ Bring the fps up to the specified rate """
00053 try:
00054 fps = int(fps * self.fps / fps)
00055 except:
00056 fps = self.fps
00057
00058 cv.NamedWindow("AVI Video", True)
00059 cv.MoveWindow("AVI Video", 650, 100)
00060
00061 bridge = CvBridge()
00062
00063 self.paused = self.start_paused
00064 self.keystroke = None
00065 self.restart = False
00066
00067
00068 frame = cv.QueryFrame(video)
00069 image_size = cv.GetSize(frame)
00070
00071 if self.width and self.height and (self.width != image_size[0] or self.height != image_size[1]):
00072 rospy.loginfo("Resizing! " + str(self.width) + " x " + str(self.height))
00073 resized_frame = cv.CreateImage((self.width, self.height), frame.depth, frame.channels)
00074 cv.Resize(frame, resized_frame)
00075 frame = cv.CloneImage(resized_frame)
00076
00077 text_frame = cv.CloneImage(frame)
00078 cv.Zero(text_frame)
00079
00080 while not rospy.is_shutdown():
00081 """ Handle keyboard events """
00082 self.keystroke = cv.WaitKey(1000 / fps)
00083
00084 """ Process any keyboard commands """
00085 if 32 <= self.keystroke and self.keystroke < 128:
00086 cc = chr(self.keystroke).lower()
00087 if cc == 'q':
00088 """ user has press the q key, so exit """
00089 rospy.signal_shutdown("User hit q key to quit.")
00090 elif cc == ' ':
00091 """ Pause or continue the video """
00092 self.paused = not self.paused
00093 elif cc == 'r':
00094 """ Restart the video from the beginning """
00095 self.restart = True
00096 elif cc == 't':
00097 """ Toggle display of text help message """
00098 self.show_text = not self.show_text
00099
00100 if self.restart:
00101 video = cv.CaptureFromFile(self.input)
00102 self.restart = None
00103
00104 if not self.paused:
00105 frame = cv.QueryFrame(video)
00106 if frame and self.width and self.height:
00107 if self.width != image_size[0] or self.height != image_size[1]:
00108 cv.Resize(frame, resized_frame)
00109 frame = cv.CloneImage(resized_frame)
00110
00111 if frame == None:
00112 if self.loop:
00113 self.restart = True
00114 else:
00115 if self.show_text:
00116 frame_size = cv.GetSize(frame)
00117 text_font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.2, 1, 0, 1, 8)
00118 cv.PutText(text_frame, "Keyboard commands:", (20, int(frame_size[1] * 0.6)), text_font, cv.RGB(255, 255, 0))
00119 cv.PutText(text_frame, " ", (20, int(frame_size[1] * 0.65)), text_font, cv.RGB(255, 255, 0))
00120 cv.PutText(text_frame, "space - toggle pause/play", (20, int(frame_size[1] * 0.72)), text_font, cv.RGB(255, 255, 0))
00121 cv.PutText(text_frame, " r - restart video from beginning", (20, int(frame_size[1] * 0.79)), text_font, cv.RGB(255, 255, 0))
00122 cv.PutText(text_frame, " t - hide/show this text", (20, int(frame_size[1] * 0.86)), text_font, cv.RGB(255, 255, 0))
00123 cv.PutText(text_frame, " q - quit the program", (20, int(frame_size[1] * 0.93)), text_font, cv.RGB(255, 255, 0))
00124
00125 cv.Add(frame, text_frame, text_frame)
00126 cv.ShowImage("AVI Video", text_frame)
00127 cv.Zero(text_frame)
00128
00129 try:
00130 image_pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
00131 except CvBridgeError, e:
00132 print e
00133
00134 def cleanup(self):
00135 print "Shutting down vision node."
00136 cv.DestroyAllWindows()
00137
00138 def main(args):
00139 help_message = "Hot keys: \n" \
00140 "\tq - quit the program\n" \
00141 "\tr - restart video from beginning\n" \
00142 "\tspace - toggle pause/play\n"
00143
00144 print help_message
00145
00146 try:
00147 a2r = AVI2ROS()
00148 except KeyboardInterrupt:
00149 print "Shutting down avi2ros..."
00150 cv.DestroyAllWindows()
00151
00152 if __name__ == '__main__':
00153 main(sys.argv)
