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 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)
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
00066
00067
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
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()
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)