00001
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
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
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
