handler.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import math
00004 import rospy
00005 import rosservice
00006 import roslib
00007 
00008 import rosweb
00009 import threading
00010 
00011 import cv
00012 from cv_bridge import CvBridge
00013 
00014 subscriptions = {}
00015 images = {}
00016 sub_lock = threading.Lock()
00017 
00018 def config_plugin(context):
00019   context.register_handler("/image", image_handler)
00020 
00021 def image_callback(msg, t):
00022   (topic, cond) = t
00023   bridge = CvBridge()
00024   img = bridge.imgmsg_to_cv(msg, "bgr8")
00025   data = cv.EncodeImage(".jpeg", img).tostring()
00026   #convert to jpeg
00027   cond.acquire()
00028   images[topic].append(data)
00029   cond.notifyAll()
00030   cond.release()
00031 
00032 def image_handler(self, path, qdict):
00033   global images
00034   topic = qdict.get('topic', [''])[0]
00035   if not topic:
00036     return False
00037   m = roslib.scriptutil.get_master()
00038   code, _, topics = m.getPublishedTopics('/'+rosweb.PKG_NAME, '/')
00039   if code != 1:
00040     raise rosweb.ROSWebException("unable to communicate with master")
00041 
00042   for t, topic_type in topics:
00043     if t == topic:
00044       break
00045   else:
00046     raise rosweb.ROSWebException("%s is not a published topic" % topic)
00047 
00048   msg_class = roslib.message.get_message_class(topic_type)
00049   sub_lock.acquire()
00050   if topic in subscriptions:
00051     cond = subscriptions[topic][1]
00052   else:
00053     cond = threading.Condition()
00054     images[topic] = []
00055     subscriptions[topic] = (rospy.Subscriber(topic, msg_class, image_callback, (topic, cond)), cond)
00056   sub_lock.release()
00057 
00058   self.send_response(200)
00059   self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=--myboundary')
00060   self.end_headers()
00061   while 1:
00062     cond.acquire()
00063     while len(images[topic]) == 0:
00064       #print "waiting for an image"
00065       cond.wait()
00066     try:
00067       if len(images[topic]) > 0:
00068         data = images[topic][-1]
00069         images[topic] = []
00070         self.wfile.write('--myboundary\r\n')
00071         self.wfile.write('Content-Type: image/jpeg\r\n')
00072         self.wfile.write('Content-Length: %s\r\n' % len(data))
00073         self.wfile.write('\r\n')
00074         self.wfile.write(data)
00075     except:
00076       pass
00077     cond.release()
00078 
00079   return True
00080 


image_stream
Author(s): Rob Wheeler
autogenerated on Wed Apr 23 2014 10:35:56