rosbridge.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 from ROSProxy import ROSProxy
00003 from terribleWSS import Session, serveForever
00004 from version import VersionChecker
00005 import sys, traceback
00006 import json
00007 from threading import Lock, Thread
00008 from time import time, sleep
00009 from urllib2 import urlopen
00010 import re
00011 
00012 class ServiceCaller(Thread):
00013         def __init__(self, ros, session, service, receiverID, arguments):
00014                 Thread.__init__(self)
00015                 self.daemon = True
00016                 self.ros = ros
00017                 self.session = session
00018                 self.service = service
00019                 self.receiverID = receiverID
00020                 self.arguments = arguments
00021 
00022         def run(self):
00023                 try:
00024                         def handleResponse(rsp):
00025                                 call = {'receiver':self.service + self.receiverID,'msg':self.ros.generalize(rsp)}
00026                                 self.session.sock.send(encode(call))
00027                         self.ros.callService(str(self.service),self.arguments,handleResponse)
00028                 except:
00029                         print "Problem calling service:"
00030                         traceback.print_exc()
00031 
00032 def encode(obj):
00033         return '\x00' + json.dumps(obj) + '\n' + '\xff'
00034 
00035 def handleFrameHelper(frame, session, handleMsgFactory, sub, passfile, ros):
00036         try:
00037                 call = ''
00038                 if frame[0] != '\x00':
00039                         call = json.loads(frame)
00040                 else:
00041                         call = json.loads(frame[1:])
00042 
00043                 orgReceiver = call["receiver"]
00044                 receiver = re.sub(r'^\/rosjs','/rosbridge',call["receiver"])
00045                 msg = call["msg"]
00046 
00047 
00048                 if 'type' in call.keys():
00049                         #print "Publish Topic!"
00050                         if (not passfile or (session.authStatus == 'accepted')):
00051                                 typ = call["type"]
00052 
00053                                 first = True
00054                                 if not receiver in session.publishers.keys():
00055                                         session.publishers[receiver] = rospy.Publisher(receiver.encode('ascii','ignore'), ros.msgClassFromTypeString(typ))
00056                                 else:
00057                                         first = False
00058 
00059                                 if first:
00060                                                 sleep(.16) #publisher creation doesn't block
00061                                 if (first and msg != {} or msg != []) or not first or typ=="std_msgs/Empty":
00062                                                 session.publishers[receiver].publish(ros.specify(typ,msg))
00063 
00064                 else:
00065                         #print "Service Call!"
00066 
00067                         #prep the receiverID
00068                         receiverParts = receiver.split('#')
00069                         receiverID = ('#').join(receiverParts[1:])
00070                         if receiverID != '':
00071                                 receiverID = '#' + receiverID
00072                         receiver = receiverParts[0]
00073 
00074                         if (not passfile or (session.authStatus == 'accepted') or receiver.find('/rosbridge/challenge') == 0 or receiver.find('/rosbridge/authenticate') == 0):
00075                                 if receiver.find('/rosbridge') == 0:
00076                                         if (receiver == "/rosbridge/topics"):
00077                                                 call = {'receiver':orgReceiver,'msg':ros.topics}
00078                                                 session.sock.send(encode(call))
00079                                         elif (receiver == "/rosbridge/publish"):
00080                                                 receiver = msg[0].encode('ascii','ignore')
00081                                                 typ = msg[1]
00082                                                 if not receiver in session.publishers.keys():
00083                                                         session.publishers[receiver] = rospy.Publisher(receiver.encode('ascii','ignore'), ros.msgClassFromTypeString(typ))
00084                                                 call = {'receiver':orgReceiver,'msg':'OK'}
00085                                                 session.sock.send(encode(call))
00086                                         elif (receiver == "/rosbridge/services"):
00087                                                 call = {'receiver':orgReceiver,'msg':ros.services}
00088                                                 session.sock.send(encode(call))
00089                                         elif (receiver == "/rosbridge/get_param"):
00090                                                 name = msg[0].encode('ascii','ignore')
00091                                                 value = None
00092                                                 try:
00093                                                         if len(msg) == 2:
00094                                                                 value = msg[1]
00095                                                                 value = rospy.get_param(name, value)
00096                                                         else:
00097                                                                 value = rospy.get_param(name)
00098                                                 except KeyError:
00099                                                         pass
00100                                                 call = {'receiver':orgReceiver,'msg':value}
00101                                                 session.sock.send(encode(call))
00102                                         elif (receiver == "/rosbridge/set_param"):
00103                                                 name = msg[0].encode('ascii','ignore')
00104                                                 value = msg[1]
00105                                                 rospy.set_param(name, value)
00106                                                 call = {'receiver':orgReceiver,'msg':None}
00107                                                 session.sock.send(encode(call))
00108                                         elif (receiver == "/rosbridge/has_param"):
00109                                                 name = msg[0].encode('ascii','ignore')
00110                                                 value = rospy.has_param(name)
00111                                                 call = {'receiver':orgReceiver,'msg':value}
00112                                                 session.sock.send(encode(call))
00113                                         elif (receiver == "/rosbridge/delete_param"):
00114                                                 name = msg[0].encode('ascii','ignore')
00115                                                 value = None
00116                                                 try:
00117                                                         value = rospy.get_param(name)
00118                                                         rospy.delete_param(name)
00119                                                 except KeyError:
00120                                                         pass
00121                                                 call = {'receiver':orgReceiver,'msg':value}
00122                                                 session.sock.send(encode(call))
00123                                         elif (receiver == "/rosbridge/search_param"):
00124                                                 name = msg[0].encode('ascii','ignore')
00125                                                 params = rospy.search_param(name)
00126                                                 call = {'receiver':orgReceiver,'msg':params}
00127                                                 session.sock.send(encode(call))
00128                                         elif (receiver == "/rosbridge/get_param_names"):
00129                                                 params = rospy.get_param_names()
00130                                                 call = {'receiver':orgReceiver,'msg':params}
00131                                                 session.sock.send(encode(call))
00132                                         elif (receiver == "/rosbridge/unsubscribe"):
00133                                                 topic = msg[0].encode('ascii','ignore')
00134                                                 print "unsubscribing to: %s" % (topic,)
00135                                                 session.latestLock.acquire()
00136                                                 if topic in session.latest.keys():
00137                                                         del session.latest[topic]
00138                                                 if topic in session.latestQueue.keys():
00139                                                         del session.latestQueue[topic]
00140                                                 if topic in session.latestSubs.keys():
00141                                                         session.latestSubs[topic].unregister()
00142                                                         del session.latestSubs[topic]
00143                                                 session.latestLock.release()
00144                                                 call = {'receiver':orgReceiver,'msg':'OK'}
00145                                                 session.sock.send(encode(call))
00146                                         elif (receiver == "/rosbridge/subscribe"):
00147                                                 topic = msg[0].encode('ascii','ignore')
00148                                                 delay = msg[1]
00149                                                 status = 'OK'
00150                                                 handleLatch = None
00151 
00152                                                 if len(msg) == 3:
00153                                                         status,handleLatch = sub(session, topic, handleMsgFactory(session, topic), msg[2])
00154                                                 else:
00155                                                         status,handleLatch = sub(session, topic, handleMsgFactory(session, topic))
00156                                                 session.data[topic] = {'delay':delay,'lastEmission':0,'lastSeq':0}
00157 
00158                                                 if delay == -1:
00159                                                         session.latestLock.acquire()
00160                                                         if not topic in session.latestQueue.keys():
00161                                                                 session.latestQueue[topic] = []
00162                                                         session.latestLock.release()
00163 
00164                                                 call = {'receiver':orgReceiver,'msg':status}
00165                                                 session.sock.send(encode(call))
00166 
00167                                                 if handleLatch:
00168                                                         handleLatch()
00169 
00170                                         elif (receiver == "/rosbridge/log"):
00171                                                 filename = msg[0].encode('ascii','ignore')
00172                                                 filename = ''.join(c for c in filename if c.isalnum()) + '.log'
00173                                                 obj = msg[1];
00174 
00175                                                 success = True
00176                                                 try:
00177                                                         log = open(filename, 'w')
00178                                                         log.write(obj)
00179                                                         log.close()
00180                                                 except:
00181                                                         success = False
00182 
00183                                                 call = {'receiver':orgReceiver,'msg':'OK'}
00184                                                 if (not success):
00185                                                         call = {'receiver':orgReceiver,'msg':'ERROR'}
00186                                                 session.sock.send(encode(call))
00187                                         elif (receiver == "/rosbridge/challenge"):
00188                                                 user = msg[0].encode('ascii','ignore')
00189 
00190                                                 call = {'receiver':orgReceiver,'msg':{'challenge':'','salt':''}}
00191                                                 if not user in passfile.keys():
00192                                                         session.authStatus = 'rejected'
00193                                                 else:
00194                                                         session.challenge = gensalt(5)
00195                                                         session.user = user
00196                                                         call = {'receiver':orgReceiver,'msg':{'challenge':session.challenge,'salt':passfile[user]['salt']}}
00197                                                         session.sock.send(encode(call))
00198                                         elif (receiver == "/rosbridge/authenticate"):
00199                                                 response = msg[0].encode('ascii','ignore')
00200                                                 salt = msg[1].encode('ascii','ignore')
00201 
00202                                                 if (response == hashpw(passfile[session.user]['hash']+session.challenge,salt)):
00203                                                         session.authStatus = 'accepted'
00204                                                         call = {'receiver':orgReceiver,'msg':session.authStatus}
00205                                                         session.sock.send(encode(call))
00206                                                 else:
00207                                                         session.authStatus = 'rejected'
00208                                         elif (receiver == "/rosbridge/typeStringFromTopic"):
00209                                                 topic = msg[0].encode('ascii','ignore')
00210                                                 call = {'receiver':orgReceiver,'msg':ros.typeStringFromTopic(topic)}
00211                                                 session.sock.send(encode(call))
00212                                         elif (receiver == "/rosbridge/typeStringFromService"):
00213                                                 service = msg[0].encode('ascii','ignore');
00214                                                 call = {'receiver':orgReceiver,'msg':ros.typeStringFromService(service)}
00215                                                 session.sock.send(encode(call))
00216                                         elif (receiver == "/rosbridge/msgClassFromTypeString"):
00217                                                 typStr = msg[0].encode('ascii','ignore');
00218                                                 call = {'receiver':orgReceiver,'msg':ros.generalize(ros.msgClassFromTypeString(typStr)())}
00219                                                 session.sock.send(encode(call))
00220                                         elif (receiver == "/rosbridge/reqClassFromTypeString"):
00221                                                 typStr = msg[0].encode('ascii','ignore');
00222                                                 call = {'receiver':orgReceiver,'msg':ros.generalize(ros.srvClassFromTypeString(typStr)._request_class())}
00223                                                 session.sock.send(encode(call))
00224                                         elif (receiver == "/rosbridge/rspClassFromTypeString"):
00225                                                 typStr = msg[0].encode('ascii','ignore');
00226                                                 call = {'receiver':orgReceiver,'msg':ros.generalize(ros.srvClassFromTypeString(typStr)._response_class())}
00227                                                 session.sock.send(encode(call))
00228                                         elif (receiver == "/rosbridge/classFromTopic"):
00229                                                 topic = msg[0].encode('ascii','ignore')
00230                                                 call = {'receiver':orgReceiver,'msg':ros.generalize(ros.classFromTopic(topic)())}
00231                                                 session.sock.send(encode(call))
00232                                         elif (receiver == "/rosbridge/classesFromService"):
00233                                                 service = msg[0].encode('ascii','ignore')
00234                                                 call = {'receiver':orgReceiver,'msg':{'req':ros.generalize(ros.classFromService(service)._request_class()),'rsp':ros.generalize(ros.classFromService(service)._response_class())}}
00235                                                 session.sock.send(encode(call))
00236                                 else:
00237                                         idx = receiver.find('protected')
00238                                         if idx >= 0 and idx <=1:
00239                                                 print "ignoring call to protected service"
00240                                                 call = {'receiver':orgReceiver,'msg':{}}
00241                                                 session.sock.send(encode(call))
00242                                         else:
00243                                                 cls = ros.classFromService(receiver)
00244                                                 arg = None;
00245                                                 if isinstance(msg, list):
00246                                                         arg = ros.specify(cls._request_class._slot_types, msg)
00247                                                 else:
00248                                                         arg = ros.specify(cls._request_class, msg)
00249                                                 serviceCaller = ServiceCaller(ros,session,receiver,receiverID,arg)
00250                                                 serviceCaller.start()
00251 
00252         except:
00253                 print "Problem " * 10
00254                 traceback.print_exc()
00255                 print "Problem " * 10
00256 
00257         if (not passfile or session.authStatus != 'rejected'):
00258                 session.transition(Session.ready)
00259         else:
00260                 session.transition(Session.closed)
00261 
00262 def handleFrameFactory(handleMsgFactory, sub, passfile, ros):
00263         def handleFrame(frame, session):
00264                 return handleFrameHelper(frame, session, handleMsgFactory, sub, passfile, ros)
00265         return handleFrame
00266 
00267 if __name__ == "__main__":
00268         ros = ROSProxy()
00269         import rospy
00270         rospy.init_node('rosbridge')
00271 
00272         passfile = rospy.get_param('/brown/rosbridge/passfile','')
00273         if "--passfile" in sys.argv:
00274                 idx = sys.argv.index("--passfile")+1
00275                 if idx < len(sys.argv):
00276                         passfile = sys.argv[idx]
00277                 else:
00278                         print "Please provide the path to the passfile."
00279                         sys.exit(-1)
00280         if (passfile != ''):
00281                 print "Only users from the passfile will be accepted."
00282                 filesrc = open(passfile,'r')    
00283                 passfile = filesrc.readlines()
00284                 passfile = ''.join(passfile)
00285                 passfile = json.loads(passfile)
00286                 filesrc.close()
00287                 from bcrypt import hashpw, gensalt
00288 
00289         host = rospy.get_param('/brown/rosbridge/host','')
00290         if "--host" in sys.argv:
00291                 idx = sys.argv.index("--host")+1
00292                 if idx < len(sys.argv):
00293                         host = sys.argv[idx]
00294                         print "rosbridge is bound to %s." % (host,)
00295                 else:
00296                         print "Please provide a hostname."
00297                         sys.exit(-1)
00298 
00299         port = rospy.get_param('/brown/rosbridge/port',9090)
00300         if "--port" in sys.argv:
00301                 idx = sys.argv.index("--port")+1
00302                 if idx < len(sys.argv):
00303                         port = int(sys.argv[idx])
00304                         print "rosbridge is will use port %s." % (port,)
00305                 else:
00306                         print "Please provide a port number."
00307                         sys.exit(-1)
00308 
00309         hz = rospy.get_param('/brown/rosbridge/hz',100000)
00310         if "--hz" in sys.argv:
00311                 idx = sys.argv.index("--hz")+1
00312                 if idx < len(sys.argv):
00313                         hz = int(sys.argv[idx])
00314                         print "rosbridge will sample at %shz." % (hz,)
00315                 else:
00316                         print "Please provide a sample rate (in hz)."
00317                         sys.exit(-1)
00318 
00319         def sub(session, topic, handler, typ = None):
00320                 idx = topic.find('protected')
00321                 if idx >= 0 and idx <=1:
00322                         print "ignoring request to listen to protected topic"
00323                         return
00324                 session.latestLock.acquire()
00325                 repeat = False
00326                 if topic in session.latestSubs.keys():
00327                         repeat = True
00328                 session.latestLock.release()
00329 
00330                 status = 'OK'
00331                 latch = None
00332                 subscriber = None
00333 
00334                 if not repeat:
00335                         try:
00336                                 print "subscribing to: %s" % (topic,)
00337                                 cls = None
00338                                 if typ == None:
00339                                         cls = ros.classFromTopic(topic)
00340                                 else:
00341                                         cls = ros.msgClassFromTypeString(typ)
00342                                 subscriber = rospy.Subscriber(topic, cls, handler, queue_size=10)
00343                                 session.latestLock.acquire()
00344                                 session.latestSubs[topic] = subscriber
00345                                 session.latestLock.release()
00346                         except:
00347                                 print "subscription to %s failed." % (topic,)
00348                                 status = 'ERROR'
00349                 else:
00350                         session.latestLock.acquire()
00351                         subscriber = session.latestSubs[topic]
00352                         session.latestLock.release()
00353 
00354                 if len(subscriber.impl.connections):
00355                         latch = subscriber.impl.connections[-1].latch
00356                         if latch:
00357                                 def handleLatchFactory(handle,latch):
00358                                         def handleLatch():
00359                                                 handler(latch)
00360                                         return handleLatch
00361                                 latch = handleLatchFactory(handler,latch)
00362 
00363                 return status,latch
00364 
00365         def handleMsgFactory(session, topic):
00366                 def handleMsg(msg):
00367                         session.latestLock.acquire()
00368 
00369                         seq = 0
00370                         if topic in session.latest.keys():
00371                                 seq = session.latest[topic]['seq']
00372                         item= {'seq':seq + 1,'msg':msg}
00373                         session.latest[topic] = item
00374                         if topic in session.latestQueue.keys():
00375                                 session.latestQueue[topic].append(item)
00376 
00377                         session.latestLock.release()
00378 
00379                 return handleMsg
00380 
00381         def handleOutput(session):
00382                 if (passfile):
00383                         if (session.authStatus == 'rejected'):
00384                                 session.transition(Session.closed)
00385                                 return
00386 
00387                 for topic in session.data.keys():
00388                         delay = session.data[topic]['delay']
00389                         lastEmission = session.data[topic]['lastEmission']
00390                         lastSeq = session.data[topic]['lastSeq']
00391                         data = session.data[topic]
00392 
00393                         session.latestLock.acquire()
00394 
00395                         if topic in session.latest.keys():
00396                                 current = session.latest[topic]
00397 
00398                                 #release as soon as possible
00399                                 session.latestLock.release()
00400 
00401                                 if current['seq'] > lastSeq or delay == -1:
00402                                         elapsed = (time() - lastEmission)*1000
00403 
00404                                         if delay == -1:
00405                                                 if len(session.latestQueue[topic]) == 0:
00406                                                         current = None
00407                                                 else:
00408                                                         current = session.latestQueue[topic].pop(0)
00409 
00410                                         if elapsed > delay and current != None:
00411                                                 call = {'receiver':topic, 'msg': ros.generalize(current['msg'])}
00412                                                 session.sock.send(encode(call))
00413                                                 session.data[topic] = {'delay':delay,'lastEmission':time(),'lastSeq':current['seq']}
00414                         else:
00415                                 session.latestLock.release()
00416 
00417         def loop():
00418                 return not rospy.is_shutdown()
00419 
00420         VersionChecker(2451).check() #manually updated  
00421 
00422         restart = True
00423         while loop():
00424                 if restart:
00425                         restart = False
00426                         try:
00427                                 serveForever(handleFrameFactory(handleMsgFactory, sub, passfile, ros), handleOutput, loop, host, port, 1.0/(hz*2.0))
00428                         except:
00429                                 restart = True
00430                                 print "Problem " * 10
00431                                 traceback.print_exc()
00432                                 print "Problem " * 10
00433                 sleep(1)


rosbridge
Author(s): Graylin Trevor Jay
autogenerated on Fri Jan 3 2014 11:11:38