rosweb.py

Go to the documentation of this file.

#!/usr/bin/env python
"""
usage: %(progname)s [--debug]
"""

# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

PKG_NAME = 'rosweb'
import roslib; roslib.load_manifest(PKG_NAME)

import getopt
import cStringIO
import os
import signal
import string
import sys
import threading
import time
import gzip
import traceback
import BaseHTTPServer
#import logging

import BaseHTTPServer, select, socket, SocketServer, urlparse
SocketServer.ThreadingMixIn.daemon_threads = True

import Cookie
import cgi

import roslib.scriptutil
import rospy
import rosservice
import rosjson

from rosservice import ROSServiceException
from roslib.message import ROSMessageException
from roslib import rosenv

import roslib.message

class ROSWebException(Exception): pass

def splitTopic(topic):
  parts = topic.split(":", 2)
  topic = parts[0]
  subtopic = None
  params = None
  if len(parts) >= 2: 
    subtopic = parts[1]
  if len(parts) >= 3:
    params = parts[2]
  return topic, subtopic, params


## Factory for ROSWebTopic instances
class RWTFactory(object):
    def __init__(self, webserver):
        self.map = {}
        self.lock = threading.Lock()

        self.webserver = webserver

        self.counter = 1
        self.cond = threading.Condition()

    def close(self):
      try:
        self.lock.acquire()
        for topic in self.map.keys():
          rwt = self.map[topic]
          rwt.factory = None
          rwt = None
          del self.map[topic]
      finally:
        self.lock.release()

    def unsubscribe(self, topic):
      try:
        self.lock.acquire()
        if topic in self.map:
          del self.map[topic]
      finally:
        self.lock.release()

    def subscribe(self, topic):
        maintopic, subtopic, params = splitTopic(topic)
        main_rwt = None
        try:
            self.lock.acquire()
            if topic in self.map:
                return self.map[topic]
            main_rwt = ROSWebTopic(maintopic, self)
            self.map[maintopic] = main_rwt
            rwt = main_rwt
            if subtopic:
              handlerClass = self.webserver._subtopics["%s:%s" % (maintopic, subtopic)]
              rwt = handlerClass(topic, self, main_rwt)
              self.map[topic] = rwt
        finally:
            self.lock.release()
        return main_rwt

    def get(self, topic):
      return self.map.get(topic, None)

    def registerVirtualTopic(self, topic, thread):
      vt = VirtualTopic(topic, self, thread)
      try:
        self.lock.acquire()
        self.map[topic] = vt
        vt.init()
      finally:
        self.lock.release()


class ROSWebTopic(object):
    def __init__(self, topic, factory):
        self.topic = topic
        self.initialized = False
        self.sub = None
        self.factory = factory

        self.last_message = None
        self.messages = []
        self.subtopics = []

    def init(self):
        if self.initialized: return

        try:
            self.factory.cond.acquire()
            m = roslib.scriptutil.get_master()

            code, _, topics = m.getPublishedTopics('/'+PKG_NAME, '/')
            if code != 1:
              raise ROSWebException("unable to communicate with master")
            
            for t, topic_type in topics:
                if t == self.topic: break
            else:
                raise ROSWebException("%s is not a published topic"%self.topic)

            msg_class = roslib.message.get_message_class(topic_type)

            #print "RosWebtopic", self.topic, msg_class
            try:
              self.sub = rospy.Subscriber(self.topic, msg_class, self.topic_callback)
            except:
              print self.topic, msg_class
              raise 
            self.initialized = True
        except ROSWebException:
            raise
        except Exception, e:
            # fatal for now
            rospy.signal_shutdown(str(e))
            raise
        finally:
            self.factory.cond.release()

    def close(self):
      self.factory = None
      for rwt, subtopic in self.subtopics:
        subtopic.close()

    def topic_callback(self, data):
        try:
            self.factory.cond.acquire()

            self.messages.append((self.factory.counter, data))
            self.factory.counter = self.factory.counter + 1
            self.messages = self.messages[-1:]

            for rwt, subtopic in self.subtopics:
              rwt.subtopic_callback(data)

            self.factory.cond.notifyAll()
        finally:
            self.factory.cond.release()

    def subscribeSubTopic(self, rwt, subtopic):
      self.subtopics.append((rwt, subtopic))
      

class ROSWebSubTopic(object):
    def __init__(self, topic, factory, main_rwt):
      self.factory = factory
      self.topic = topic

      self.messages = []
      self.maintopic, self.subtopic, params = splitTopic(topic)
      main_rwt.subscribeSubTopic(self, self.subtopic)

    def init(self):
      pass

    def subtopic_callback(self, msg):
      ## no locks needed since this will be called from the main topic

      newmsg = self.transform(msg)

      if not newmsg: return

      self.messages.append((self.factory.counter, newmsg))
      self.factory.counter = self.factory.counter + 1
      self.messages = self.messages[-1:]
      
      
    

class VirtualTopic(object):
  def __init__(self, topic, factory, thread, intopics=[]):
    self.factory = factory
    self.topic = topic
    self.intopics = intopics

    self.messages = []

    self.thread = thread
    self.initialized = False

  def init(self):
    if not self.initialized:
      for _topic in self.intopics: 
        self.factory.subscribe(_topic)

      self.initialized = True
      self.thread.setCallback(self.callback)
      self.thread.start()

  def callback(self, data):
    try:
      self.factory.cond.acquire()

      self.messages.append((self.factory.counter, data))
      self.factory.counter = self.factory.counter + 1
      self.messages = self.messages[-3:]

      self.factory.cond.notifyAll()
    finally:
      self.factory.cond.release()
    

class ROSWebHandler(BaseHTTPServer.BaseHTTPRequestHandler):
    def __init__(self, *args):
        self.failed_topics = []
        BaseHTTPServer.BaseHTTPRequestHandler.__init__(self, *args)

    def subscribe(self, topic):
        rwt = self.server.factory.subscribe(topic)
        try:
          rwt.init()
        except ROSWebException, e:
          self.server.factory.unsubscribe(topic)
          if topic not in self.failed_topics:
            rospy.logwarn("Cannot subscribe to %s" % topic)
            self.failed_topics.append(topic)
          #traceback.print_exc()
          return None
        return rwt

    def _do_GET_topic(self, topics, since, nowait=False, callback=None):
        rwttopics = []

        for topic in topics:
          rwt = self.server.factory.get(topic)
          if rwt:
            rwttopics.append((topic, rwt))
          else:
            rwt = self.subscribe(topic)
            if rwt: rwttopics.append((topic, rwt))

        if len(rwttopics) == 0:
          rospy.logwarn("no valid topics")
          self.send_failure()
          return

        (messages, lastSince) = self._get_messages(rwttopics, since, nowait)

        buf = cStringIO.StringIO()
        buf.write("{")
        buf.write('"since": %s,' % lastSince)
        buf.write('"msgs": [')
        _i = 0
        for (topic, msg) in messages:
          _i = _i + 1
          if _i > 1: buf.write(',')
          buf.write("{")
          buf.write('"topic": "%s",' % topic)
          buf.write('"msg": ')
          buf.write(rosjson.ros_message_to_json(msg))
          buf.write("}")
        buf.write(']')
        buf.write('}')

        buf = buf.getvalue()

        self.send_success(buf, callback)


    def _get_messages(self, rwttopics, since, nowait=False):
        messages = []
        lastSince = 0
        try:
            self.server.factory.cond.acquire()
            # timeout?

            if since > self.server.factory.counter:
              ## reset the since if it is invalid.
              rospy.logerr("invalid since counter, reseting... %s" % (since, ))
              since = 0

            ## check for some old messages first
            lastSince = since
            for topic, rwt in rwttopics:
              if not rwt: continue
              for t,msg in rwt.messages:
                if t > since:
                  messages.append((topic, msg))
                  lastSince = max(t, lastSince)

            if nowait:
              ## do not wait for message, for grabbing messages
              return messages, lastSince
                
            ## if no old messages, then wait for a new one.
            while not messages:
              self.server.factory.cond.wait()

              # get the last message under lock
              for topic, rwt in rwttopics:
                if not rwt: continue
                for t,msg in rwt.messages:
                  if t > lastSince:
                    messages.append((topic, msg))
                    lastSince = max(t, lastSince)
        finally:
            self.server.factory.cond.release()
        return (messages, lastSince)


    def _connect_to(self, netloc, soc):
        i = netloc.find(':')
        if i >= 0:
          host_port = netloc[:i], int(netloc[i+1:])
        else:
            host_port = netloc, 80
#        print "\t" "connect to %s:%d" % host_port
        try: soc.connect(host_port)
        except socket.error, arg:
          try: msg = arg[1]
          except: 
            msg = arg
            self.send_error(404, msg)
            return 0
        return 1

    def _send_responsepage(self, retcode=200, buf = "{}", callback = None):
      try:
        self.send_response(retcode)
        if callback:
          #buf = callback + "(" + buf + "); var _xsajax$transport_status=200;"
          buf = callback + "=" + buf + ";\nvar _xsajax$transport_status=200;"
          self.send_header('Content-Type', 'text/javascript')
        else:
          self.send_header('Content-Type', 'application/json')

        if len(buf) > 500:
          acceptEncoding = self.headers.get('Accept-Encoding', '')
          if acceptEncoding.find('gzip') != -1:
            zbuf = cStringIO.StringIO()
            zfile = gzip.GzipFile(None, "wb", 9, zbuf)
            zfile.write(buf)
            zfile.close()

            nbuf = len(buf)
            buf = zbuf.getvalue()
            self.send_header('Content-encoding', 'gzip')

        self.send_header('Content-Length', str(len(buf)))

        if self.headers.get("Connection", '').lower() == "keep-alive":
          self.send_header('Connection', 'keep-alive')

        self.send_header("Access-Control-Allow-Origin", "*")  #"http://ta110.willowgarage.com")

        self.end_headers()


        self.wfile.write(buf)
      except socket.error, (ecode, reason):
        if ecode == 32:
          print ecode, reason
        else:
          raise
                        

    def send_success(self, buf = "{}", callback=None):
      self._send_responsepage(200, buf, callback)

    def send_failure(self, buf = "{}", callback=None):
      self._send_responsepage(404, buf, callback)

    def send_forbidden(self, buf = "{}", callback=None):
      self._send_responsepage(403, buf, callback)

    def address_string(self):
      host, port = self.client_address[:2]
      return host

    def handle_ROS(self, path, qdict):
      cstring = self.headers.get('Cookie', '')

      cookie = Cookie.BaseCookie()
      cookie.load(cstring)

      path_parts = path.split("/")

      username = ""

      cmd = path_parts[2]

      ## parse out the username from the cookie
      if cookie.has_key('MB_L1'):
        l1 = cookie['MB_L1'].value
        if l1.startswith("V1/"):
          code = l1[3:]
          parts = code.split(":", 1)
          username = parts[0]

      ## Make a service call
      ##   Args can be passed as an ordered list of parameters:
      ##     /ros/service/<service_name>?args=<arg1>&args=<arg2>&<...>
      ##   or as a dictionary of named parameters
      ##     /ros/service/<service_name>?json=<json-encoded dictionary of args>
      if cmd == "ping":
        self.send_success("{'ping': %f}" % time.time())

      if cmd == "service":
        name = "/".join(path_parts[3:])
        callback = qdict.get("callback", [''])[0]

        try:
          service_class = rosservice.get_service_class_by_name("/" + name)
        except ROSServiceException:
          self.send_failure()
          return
          
        request = service_class._request_class()

        args = qdict.get('args', None)
        if not args:
          args = qdict.get('json', ['{}'])[0]
          args = eval(args)
        try:
          now = rospy.get_rostime()
          keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
          roslib.message.fill_message_args(request, args, keys=keys)
        except ROSMessageException:
          raise ROSServiceException("Not enough arguments to call service.\n"+\
              "Args should be: [%s], but are actually [%s] " %
              (roslib.message.get_printable_message_args(request), args))

        rospy.logdebug("service call: name=%s, args=%s" % (name, args))
        rospy.wait_for_service(name)
        service_proxy = rospy.ServiceProxy(name, service_class)
        try:
          msg = service_proxy(request)
        except rospy.ServiceException:
          self.send_failure()
          return
        msg = rosjson.ros_message_to_json(msg)

        rospy.logdebug("service call: name=%s, resp=%s" % (name, msg))
        self.send_success(msg, callback=callback)
        return

      if cmd == "publish":
        topic = string.join(path_parts[3:], "/")

        callback = qdict.get("callback", [''])[0]
        topic_type = qdict.get("topic_type", [''])[0]

        message_klass = roslib.message.get_message_class(topic_type)

        message = message_klass()

        args = qdict.get('args', None)
        if not args:
          args = qdict.get('json', ['{}'])[0]
          args = eval(args)
        try:
          now = rospy.get_rostime()
          keys = { 'now': now, 'auto': rospy.Header(stamp=now) }
          roslib.message.fill_message_args(message, args, keys=keys)
        except ROSMessageException:
          raise ROSServiceException("Not enough arguments to call service.\n"+\
              "Args should be: [%s], but are actually [%s] " %
              (roslib.message.get_printable_message_args(message), args))

        rospy.logdebug("publish: topic=%s, topic_type=%s, args=%s" % (topic, topic_type, args))

        pub = rospy.Publisher(topic, message_klass, latch=True)

        try:
          pub.publish(message)
        except rospy.ServiceException:
          self.send_failure()
          return

        self.send_success(callback=callback)
        return

      if cmd == "receive":
        since = int(qdict.get('since', ['0'])[0])
        topics = qdict.get("topic", "")
        callback = qdict.get("callback", [''])[0]
        nowait = int(qdict.get('nowait', ['0'])[0])
        self._do_GET_topic(topics, since, nowait=nowait, callback=callback)
        return

    
      
    def do_GET(self):
        if get_robot_type().startswith("texas"):
          try:
            stored_cookie = open("/var/ros/user_cookie.dat").read()
            request_cookie = self.headers.get('Cookie', '')
            if stored_cookie.find(request_cookie) == -1:
              rospy.logwarn("cookie did not match stored cookie \n%s\n%s" % (stored_cookie, request_cookie))
              self.send_forbidden()
              return
          except:
            self.send_forbidden()
            return

        (scm, netloc, path, params, query, fragment) = urlparse.urlparse(
            self.path, 'http')
        qdict = cgi.parse_qs(query)
        pdict = cgi.parse_qs(str(self.rfile._rbuf))
        qdict.update(pdict)

        self.log_request()

        for handler in self.server._handlers:
          if path.startswith(handler['url']):
            handler['handler'](self, path, qdict)
            return

        netloc = "localhost:80"
        scm = "http"

        soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
          if self._connect_to(netloc, soc):
                soc.send("%s %s %s\r\n" % (
                    self.command,
                    urlparse.urlunparse(('', '', path, params, query, '')),
                    self.request_version))
                self.headers['Connection'] = 'close'
                del self.headers['Proxy-Connection']
                for key_val in self.headers.items():
                    soc.send("%s: %s\r\n" % key_val)
                soc.send("\r\n")
                self._read_write(soc)
        finally:
            soc.close()
            self.connection.close()

      
    def _read_write(self, soc, max_idling=20):
        iw = (self.connection, soc)
        ow = ()
        count = 0

        while 1:
            count += 1
            (ins, _, exs) = select.select(iw, ow, iw, 3)
            if exs: break
            if ins:
              for i in ins:
                if i is soc:
                  out = self.connection
                elif i is self.connection:
                  out = soc
                else:
                  print "error"
                data = i.recv(8192)
                if data:
                  bytes_sent = 0
                  while bytes_sent != len(data):
                    n = out.send(data[bytes_sent:])
                    bytes_sent += n
                  count = 0
            else:
                pass
            if count == max_idling: break

    do_POST = do_GET
    do_OPTIONS = do_GET

    def log_message(self, format, *args):
      self.server.accesslog_fp.write("%s - - [%s] %s\n" %
                              (self.address_string(),
                               self.log_date_time_string(),
                               format%args))
      self.server.accesslog_fp.flush()

      
class MasterMonitor:
  def run(self):
    global running
    master = roslib.scriptutil.get_master()
    while running:
      try:
        master.getSystemState('/rostopic')
      except:
        import traceback
        #traceback.print_exc()
        return
      time.sleep(1)

class ThreadingHTTPServer (threading.Thread,
                           SocketServer.ThreadingMixIn,
                           BaseHTTPServer.HTTPServer): 
  def __init__(self, hostport, handlerClass):
    threading.Thread.__init__(self)
    SocketServer.TCPServer.__init__(self, hostport, handlerClass)

    self.factory = RWTFactory(self)

    logfn = os.path.join(rosenv.get_log_dir(), "access_log")
    self.accesslog_fp = open(logfn, "a+")

    self._handlers = []
    self.register_handler('/ros', ROSWebHandler.handle_ROS)

    self._subtopics = {}

    self.__load_plugins()
    self.__registerVirtualTopics()

  def register_handler(self, urlprefix, handler):
    h = {'url': urlprefix, 'handler': handler}
    self._handlers.append(h)

  def register_subtopic(self, topic, handlerClass):
    self._subtopics[topic] = handlerClass

  def __load_plugins(self):
    plugins = roslib.rospack.rospack_plugins(PKG_NAME)
    for (package, plugin) in plugins:
      try:
        roslib.load_manifest(package)
        mods = plugin.split('.')
        mod = __import__(plugin)
        for sub_mod in mods[1:]:
          mod = getattr(mod, sub_mod)

        mod.config_plugin(self)

      except Exception, reason:
        rospy.logerr("plugin %s got exception %s" % (plugin, reason))


  def __registerVirtualTopics(self):
    import topics
    ut = topics.TopicThread()
    ut.setDaemon(True)
    self.factory.registerVirtualTopic("/topics", ut)

    import params
    ut = params.ParamThread()
    ut.setDaemon(True)
    self.factory.registerVirtualTopic("/parameters", ut)

  def run(self):
    while 1:
      self.handle_request()

def rosweb_start():
    web_server = None
    mm = None

    try:
      rospy.init_node(PKG_NAME, disable_signals=True)

      web_server = ThreadingHTTPServer(('', 8068), ROSWebHandler)

      web_server.setDaemon(True)
      web_server.start()
      rospy.loginfo("starting Web server")

      mm = MasterMonitor()
      mm.run()

    finally:
      rospy.signal_shutdown(PKG_NAME + ' exiting')
      if web_server: web_server.server_close()

error_log_fp = None

def signal_handler(signal, frame):
  global running
  running = False

def usage(progname):
  print __doc__ % vars()

  
def get_robot_type():
  try:
    return open(os.path.join("/etc/ros/env", "ROBOT")).read().strip()
  except IOError:
    return "desktop"

def main(argv, stdout, environ):
  progname = argv[0]
  optlist, args = getopt.getopt(argv[1:], "", ["help", "debug"])

  debugFlag = False
  for (field, val) in optlist:
    if field == "--help":
      usage(progname)
      return
    elif field == "--debug":
      debugFlag = True

  global running, error_log
  running = True
  #if debugFlag:
  #  logging.basicConfig(level=logging.DEBUG)
  #  pass
  #else:
  #  logging.basicConfig(filename="error_log", level=logging.DEBUG)

  signal.signal(signal.SIGINT, signal_handler)


  while running:
    rospy.loginfo("starting")
    try:
      rosweb_start()
    except:
      import traceback
      traceback.print_exc()
    time.sleep(1)

  if error_log_fp: error_log_fp.close()

if __name__ == '__main__':
  main(sys.argv, sys.stdout, os.environ)

---

rosweb
Author(s): Scott Hassan, Rob Wheeler, Nate Koenig
autogenerated on Fri Jan 11 09:54:47 2013