class VideoPub:
def __init__(self, capture, topic, show_window=False, show_info=False, rate=32):
self.show_window = show_window
self.show_info = show_info
self.videoCapture = capture
self.topic = topic
self.rate = rate
self.bridge = CvBridge()
self.KEY_ECS = 27
self.imgPlayer = ImagePlayer()
self.mutex = threading.Lock()
self.fps = FPS()
self.msg = None
def run(self):
self.handle_pub()
def handle_pub(self):
rospy.init_node('video_publisher')
pub = rospy.Publisher(topic, Image, queue_size=2)
self.handle_threading()
count = 0
self.fps.start()
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
success, img = self.videoCapture.read()
if not success:
break
try:
msg = self.bridge.cv2_to_imgmsg(img, "bgr8")
except CvBridgeError as e:
print(e)
rospy.signal_shutdown(e)
count += 1
self.fps.update()
self.msg = self.add_timestamp(msg, count)
pub.publish(self.msg)
self.show_image(img)
rate.sleep()
# end wile
# ....
def handle_threading(self):
threading.Thread(target=self.exit_key_event).start()
if self.show_info:
threading.Thread(target=self.show_data_info).start()
def exit_key_event(self):
while not rospy.is_shutdown():
with self.mutex:
ch = getch()
if len(ch) > 0 and (ch == 'q' or ord(ch) == imtools.KEY_CTRL_C):
print 'user press crtl + c or q to exit'
rospy.signal_shutdown('user press crtl + c or q to exit')
break
def add_timestamp(self, msg, count):
now = rospy.Time.now()
msg.header.frame_id = get_hostname() + '-' + str(count)
msg.header.stamp.secs = now.secs
msg.header.stamp.nsecs = now.nsecs
return msg
def show_image(self, img):
if self.show_window:
self.imgPlayer.show(img)
key = self.imgPlayer.get_key()
if 0xFF & key == self.KEY_ECS:
rospy.signal_shutdown('user press ESC to exit')
def show_data_info(self):
import utility
import time
while not rospy.is_shutdown():
time.sleep(.5)
with self.mutex:
utility.show_msg_info(self.msg)
print('fps: %s' % round(self.fps, 2))
print '%s' % (quit_msg)
def cleanup(self):
self.videoCapture.release()
cv2.destroyAllWindows()
class ImgSub:
def __init__(self, topic, should_mirror, verbose, limit=1000):
self.topic = topic
self.KEY_ECS = 27
self.should_mirror = should_mirror
self.verbose = verbose
self.bridge = CvBridge()
rospy.on_shutdown(self.cleanup)
self.shutdowm_msg = "Shutting down."
self.node_name = 'image_subscriber'
self.time_start = clock()
self.limit = limit
self.frame_count = 0
self.total_latency = 0
self.fps = FPS()
def run(self):
print '%s node turn on' % (self.node_name)
self.fps = self.fps.start()
self.handle_sub()
def handle_sub(self):
rospy.init_node(self.node_name, anonymous=True)
cv2.namedWindow("show %s" % (self.topic), 10)
self.sub = rospy.Subscriber(self.topic, Image, self.callback)
try:
rospy.spin()
except KeyboardInterrupt:
print(self.shutdowm_msg)
cv2.destroyAllWindows()
def callback(self, msg):
try:
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
if self.verbose:
self.show_data_info(msg)
except CvBridgeError as e:
print(e)
cv_image = self.mirror_image(cv_image)
self.show_video(cv_image)
self.frame_count += 1
if self.frame_count >= self.limit:
rospy.signal_shutdown("reach the limit of frames.")
# ...
def show_video(self, img):
self.fps.update()
draw_str(img, (5, 30), 'fps: %s' % self.fps)
cv2.imshow("show %s" % (self.topic), img)
key = cv2.waitKey(1)
if 0xFF & key == self.KEY_ECS:
rospy.signal_shutdown("User hit q key to quit.")
elif 0xFF & key == ord('a'):
file_name = 'image_%s.jpg' % (str(int(clock())))
cv2.imwrite(file_name, img)
print '%s has saved.' % file_name
def mirror_image(self, img):
if self.should_mirror:
from imtools import mirror_image
img = mirror_image(img)
return img
def show_data_info(self, msg):
import utility
self.total_latency += utility.show_msg_info(msg, showLatency=True)
print('fps: %s' % self.fps)
def cleanup(self):
print self.shutdowm_msg
cv2.destroyAllWindows()