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