class RosbridgeWebSocket(WebSocketHandler): def open(self): global client_id_seed, clients_connected, authenticate try: self.protocol = RosbridgeProtocol(client_id_seed) self.protocol.outgoing = self.send_message self.authenticated = False client_id_seed = client_id_seed + 1 clients_connected = clients_connected + 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", clients_connected) if authenticate: rospy.loginfo("Awaiting proper authentication...") def on_message(self, message): global authenticate # check if we need to authenticate if authenticate and not self.authenticated: try: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn( "Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def on_close(self): global clients_connected clients_connected = clients_connected - 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", clients_connected) def send_message(self, message): IOLoop.instance().add_callback(partial(self.write_message, message)) def check_origin(self, origin): return True
class RosbridgeWebSocket(WebSocketHandler): def open(self): global client_id_seed, clients_connected, authenticate try: self.protocol = RosbridgeProtocol(client_id_seed) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False client_id_seed = client_id_seed + 1 clients_connected = clients_connected + 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", clients_connected) if authenticate: rospy.loginfo("Awaiting proper authentication...") def on_message(self, message): global authenticate # check if we need to authenticate if authenticate and not self.authenticated: try: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn("Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def on_close(self): global clients_connected clients_connected = clients_connected - 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", clients_connected) def send_message(self, message): IOLoop.instance().add_callback(partial(self.write_message, message)) def check_origin(self, origin): return True
class RosbridgeWebSocket(WebSocketHandler): """ This was ripped off from rosbridge and should be worked back in. """ client_id_seed = 0 clients_connected = 0 def open(self): cls = self.__class__ try: from rosbridge_library.rosbridge_protocol import RosbridgeProtocol self.protocol = RosbridgeProtocol(cls.client_id_seed) self.protocol.outgoing = self.send_message self.set_nodelay(True) assert hasattr(cls, 'client_id_seed') assert hasattr(cls, 'clients_connected') except Exception as exc: rospy.logerr( 'Unable to accept incoming connection. Reason: %s', str(exc)) self.close(code=PROTO_OPEN_FAILED, reason=str(exc)) return cls.client_id_seed += 1 cls.clients_connected += 1 rospy.loginfo('Client connected. %d clients total.', cls.clients_connected) def on_message(self, message): self.protocol.incoming(message) def on_close(self): if self.close_code == PROTO_OPEN_FAILED: return cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() rospy.loginfo('Client disconnected. %d clients total.', cls.clients_connected) def send_message(self, message): IOLoop.instance().add_callback(partial(self.write_message, message)) def check_origin(self, origin): return True
class RosbridgeWebSocket(WebSocketHandler): client_id_seed = 0 clients_connected = 0 authenticate = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes bson_only_mode = False def open(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn("Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def on_close(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): binary = type(message)==bson.BSON IOLoop.instance().add_callback(partial(self.write_message, message, binary)) def check_origin(self, origin): return True
class RosbridgeUdpSocket: client_id_seed = 0 clients_connected = 0 client_count_pub = None authenticate = False # The following parameters are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds def __init__(self, write): self.write = write self.authenticated = False def startProtocol(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 if cls.client_count_pub: cls.client_count_pub.publish(cls.clients_connected) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def datagramReceived(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn( "Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def stopProtocol(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_count_pub: cls.client_count_pub.publish(cls.clients_connected) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): binary = type(message) == bson.BSON self.write(message) def check_origin(self, origin): return False
class RosbridgeUdpSocket: client_id_seed = 0 clients_connected = 0 authenticate = False # The following parameters are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes def __init__(self,write): self.write = write self.authenticated = False def startProtocol(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def datagramReceived(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn("Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def stopProtocol(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): binary = type(message)==bson.BSON self.write(message) def check_origin(self, origin): return False
class RosbridgeTcpSocket(SocketServer.BaseRequestHandler): """ TCP Socket server for rosbridge """ busy = False queue = [] client_id_seed = 0 clients_connected = 0 # list of parameters incoming_buffer = 65536 # bytes socket_timeout = 10 # seconds # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes bson_only_mode = False def setup(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message cls.client_id_seed += 1 cls.clients_connected += 1 self.protocol.log( "info", "connected. " + str(cls.clients_connected) + " client total.") except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) def recvall(self, n): # http://stackoverflow.com/questions/17667903/python-socket-receive-large-amount-of-data # Helper function to recv n bytes or return None if EOF is hit data = '' while len(data) < n: packet = self.request.recv(n - len(data)) if not packet: return None data += packet return data def recv_bson(self): # Read 4 bytes to get the length of the BSON packet BSON_LENGTH_IN_BYTES = 4 raw_msglen = self.recvall(BSON_LENGTH_IN_BYTES) if not raw_msglen: return None msglen = struct.unpack('i', raw_msglen)[0] # Retrieve the rest of the message data = self.recvall(msglen - BSON_LENGTH_IN_BYTES) if data == None: return None data = raw_msglen + data # Prefix the data with the message length that has already been received. # The message length is part of BSONs message format # Exit on empty message if len(data) == 0: return None self.protocol.incoming(data) return True def handle(self): """ Listen for TCP messages """ cls = self.__class__ self.request.settimeout(cls.socket_timeout) while 1: try: if self.bson_only_mode: if self.recv_bson() == None: break continue # non-BSON handling data = self.request.recv(cls.incoming_buffer) # Exit on empty string if data.strip() == '': break elif len(data.strip()) > 0: self.protocol.incoming(data.strip('')) else: pass except Exception as e: pass self.protocol.log( "debug", "socket connection timed out! (ignore warning if client is only listening..)" ) def finish(self): """ Called when TCP connection finishes """ cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() self.protocol.log( "info", "disconnected. " + str(cls.clients_connected) + " client total.") def send_message(self, message=None): """ Callback from rosbridge """ self.request.send(message)
class MyClientProtocol(WebSocketClientProtocol): state = 0 def onConnect(self, response): print("Server connected: {0}".format(response.peer)) def onOpen(self): global client_id_seed, clients_connected, authenticate, login try: self.protocol = RosbridgeProtocol(client_id_seed) self.protocol.outgoing = self.send_message self.authenticated = True client_id_seed = client_id_seed + 1 clients_connected = clients_connected + 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", clients_connected) print("ROBOTCLIENT: Start websocket connection to server.") self.Message = 'REGISTRATION:ROBOT|' + login self.sendMessage(self.Message.encode('utf8')) def onMessage(self, payload, isBinary): if isBinary: print("Binary message received: {0} bytes".format(len(payload))) else: print("Text message received: {0}".format(payload.decode('utf8'))) self.parseData = payload.split('|') if len(self.parseData) != 0: self.id = self.parseData[0].split(':') if self.id[0] == 'STOP': if self.id[1] == 'STREAM': streamProcess[0].kill() if self.id[0] == 'REGISTRATION': print('REGISTRATION OK') if self.id[0] == 'STREAM': if self.id[1] == 'REGISTRATION': time.sleep(5) self.process = Popen(['./client', '37.205.11.196', self.parseData[1], 'shell=True']) streamProcess.append(self.process) #Send message to rosbridge if self.id[0] == 'MESSAGE': print('ROBOTCLIENT: Send message to rosbridge') self.protocol.incoming(self.parseData[1]) if self.id[0] == 'PING': if self.id[1] == 'WEBCLIENT': self.Message = 'PONG:WEBCLIENT|' + self.parseData[1] self.sendMessage(self.Message.encode('utf8')) elif self.id[1] == 'ROBOT': self.Message = 'PONG:ROBOT|' + self.parseData[1] self.sendMessage(self.Message.encode('utf8')) else: self.protocol.incoming(payload) def onClose(self, wasClean, code, reason): global clients_connected clients_connected = clients_connected - 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", clients_connected) def send_message(self, message): print("SERVER: ODOSIELAM SPRAVU:" + message) self.Message = "MESSAGE:ROBOT|" + message self.sendMessage(self.Message.encode('utf8'))
class RosbridgeWebSocket(WebSocketClientProtocol): client_id_seed = 0 clients_connected = 0 authenticate = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False def onOpen(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) producer = OutgoingValve(self) self.transport.registerProducer(producer, True) producer.resumeProducing() self.protocol.outgoing = producer.relay self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() self.peer = self.transport.getPeer().host if cls.client_manager: cls.client_manager.add_client(self.client_id, self.peer) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) def onMessage(self, message, binary): cls = self.__class__ if not binary: message = message.decode('utf-8') self.protocol.incoming(message) def outgoing(self, message): if type(message) == bson.BSON: binary = True message = bytes(message) elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False message = message.encode('utf-8') self.sendMessage(message, binary) def onClose(self, was_clean, code, reason): if not hasattr(self, 'protocol'): return # Closed before connection was opened. cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.peer) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected)
class RosbridgeWebSocket(WebSocketHandler): client_id_seed = 0 clients_connected = 0 client_count_pub = None authenticate = False use_compression = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False def open(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False self._write_lock = threading.RLock() cls.client_id_seed += 1 cls.clients_connected += 1 if cls.client_count_pub: cls.client_count_pub.publish(cls.clients_connected) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn( "Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def on_close(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_count_pub: cls.client_count_pub.publish(cls.clients_connected) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): if type(message) == bson.BSON: binary = True elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False with self._write_lock: IOLoop.instance().add_callback( partial(self.prewrite_message, message, binary)) @coroutine def prewrite_message(self, message, binary): with self._write_lock: yield self.write_message(message, binary) def check_origin(self, origin): return True def get_compression_options(self): # If this method returns None (the default), compression will be disabled. # If it returns a dict (even an empty one), it will be enabled. cls = self.__class__ if not cls.use_compression: return None return {}
class RosbridgeTcpSocket(SocketServer.BaseRequestHandler): """ TCP Socket server for rosbridge """ busy = False queue = [] def setup(self): global client_id_seed, clients_connected parameters = { "port": port, "host": host, "incoming_buffer": incoming_buffer, "socket_timeout": socket_timeout, "retry_startup_delay": retry_startup_delay, "service_request_timeout": service_request_timeout, "check_response_delay": check_response_delay, "wait_for_busy_service_provider": wait_for_busy_service_provider, "max_service_requests": max_service_requests, "fragment_timeout": fragment_timeout, "delay_between_messages": delay_between_messages, "max_message_size": max_message_size } try: self.protocol = RosbridgeProtocol(client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message client_id_seed = client_id_seed + 1 clients_connected = clients_connected + 1 self.protocol.log( "info", "connected. " + str(clients_connected) + " client total.") except Exception as exc: logerr("Unable to accept incoming connection. Reason: %s", str(exc)) def handle(self): """ Listen for TCP messages """ self.request.settimeout(socket_timeout) while True: try: data = self.request.recv(incoming_buffer) # Exit on empty string if data.strip() == '': break elif len(data.strip()) > 0: # time.sleep(5) self.protocol.incoming(data.strip('')) else: pass except Exception as e: pass self.protocol.log( "debug", "socket connection timed out!\ (ignore warning if client is only listening..)") def finish(self): """ Called when TCP connection finishes """ global clients_connected clients_connected = clients_connected - 1 self.protocol.finish() self.protocol.log( "info", "disconnected. " + str(clients_connected) + " client total.") def send_message(self, message=None): """ Callback from rosbridge """ self.request.send(message)
class RosbridgeWebSocket(WebSocketHandler): client_id_seed = 0 clients_connected = 0 authenticate = False use_compression = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = 10000000 # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False node_handle = None @log_exceptions def open(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, cls.node_handle, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False self._write_lock = threading.RLock() cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() if cls.client_manager: cls.client_manager.add_client(self.client_id, self.request.remote_ip) except Exception as exc: cls.node_handle.get_logger().error( "Unable to accept incoming connection. Reason: {}".format( exc)) cls.node_handle.get_logger().info( "Client connected. {} clients total.".format( cls.clients_connected)) if cls.authenticate: cls.node_handle.get_logger().info( "Awaiting proper authentication...") @log_exceptions def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv_client = cls.node_handle.create_client( Authentication, 'authenticate') auth_srv_req = Authentication.Request() auth_srv_req.mac = msg['mac'] auth_srv_req.client = msg['client'] auth_srv_req.dest = msg['dest'] auth_srv_req.rand = msg['rand'] auth_srv_req.t = Time(seconds=msg['t']).to_msg() auth_srv_req.level = msg['level'] auth_srv_req.end = Time(seconds=msg['end']).to_msg() while not auth_srv_client.wait_for_service( timeout_sec=1.0): cls.node_handle.get_logger().info( 'Authenticate service not available, waiting again...' ) future = auth_srv_client.call_async(auth_srv_req) rclpy.spin_until_future_complete(cls.node_handle, future) # Log error if service could not be called. if future.result() is not None: self.authenticated = future.result().authenticated else: self.authenticated = False cls.node_handle.get_logger.error( 'Authenticate service call failed') if self.authenticated: cls.node_handle.get_logger().info( "Client {} has authenticated.".format( self.protocol.client_id)) return # if we are here, no valid authentication was given cls.node_handle.get_logger().warn( "Client {} did not authenticate. Closing connection.". format(self.protocol.client_id)) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) @log_exceptions def on_close(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.request.remote_ip) cls.node_handle.get_logger().info( "Client disconnected. {} clients total.".format( cls.clients_connected)) def send_message(self, message): if type(message) == bson.BSON: binary = True elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False with self._write_lock: IOLoop.instance().add_callback( partial(self.prewrite_message, message, binary)) @coroutine def prewrite_message(self, message, binary): # Use a try block because the log decorator doesn't cooperate with @coroutine. try: with self._write_lock: future = self.write_message(message, binary) # When closing, self.write_message() return None even if it's an undocument output. # Consider it as WebSocketClosedError # For tornado versions <4.3.0 self.write_message() does not have a return value if future is None and tornado_version_info >= (4, 3, 0, 0): raise WebSocketClosedError yield future except WebSocketClosedError: cls.node_handle.get_logger().warn( 'WebSocketClosedError: Tried to write to a closed websocket', throttle_duration_sec=1.0) raise except StreamClosedError: cls.node_handle.get_logger().warn( 'StreamClosedError: Tried to write to a closed stream', throttle_duration_sec=1.0) raise except BadYieldError: # Tornado <4.5.0 doesn't like its own yield and raises BadYieldError. # This does not affect functionality, so pass silently only in this case. if tornado_version_info < (4, 5, 0, 0): pass else: _log_exception() raise except: _log_exception() raise @log_exceptions def check_origin(self, origin): return True @log_exceptions def get_compression_options(self): # If this method returns None (the default), compression will be disabled. # If it returns a dict (even an empty one), it will be enabled. cls = self.__class__ if not cls.use_compression: return None return {}
class MyClientProtocol(WebSocketClientProtocol): state = 0 def onConnect(self, response): print("Server connected: {0}".format(response.peer)) def onOpen(self): global client_id_seed, clients_connected, authenticate, login try: self.protocol = RosbridgeProtocol(client_id_seed) self.protocol.outgoing = self.send_message self.authenticated = True client_id_seed = client_id_seed + 1 clients_connected = clients_connected + 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", clients_connected) print("ROBOTCLIENT: Start websocket connection to server.") self.Message = 'REGISTRATION:ROBOT|' + login self.sendMessage(self.Message.encode('utf8')) def onMessage(self, payload, isBinary): if isBinary: print("Binary message received: {0} bytes".format(len(payload))) else: print("Text message received: {0}".format(payload.decode('utf8'))) if is_json(payload): self.protocol.incoming(payload) else: self.parseData = payload.split('|') if len(self.parseData) != 0: self.id = self.parseData[0].split(':') if self.id[0] == 'STOP': if self.id[1] == 'STREAM': streamProcess[0].kill() if self.id[0] == 'REGISTRATION': if self.id[1] == 'BRIDGE': if self.parseData[1] == 'OK': print "ROBOTCLIENT-%s: CONNECTION TO SERVER OK" % os.getpid() def onClose(self, wasClean, code, reason): global clients_connected clients_connected = clients_connected - 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", clients_connected) def send_message(self, message): print("SERVER: ODOSIELAM SPRAVU:" + message) self.Message = "MESSAGE:ROBOT|" + message self.sendMessage(self.Message.encode('utf8'))
class RosbridgeTcpSocket(SocketServer.BaseRequestHandler): """ TCP Socket server for rosbridge """ busy = False queue = [] client_id_seed = 0 clients_connected = 0 # list of parameters incoming_buffer = 65536 # bytes socket_timeout = 10 # seconds # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False def setup(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message cls.client_id_seed += 1 cls.clients_connected += 1 self.protocol.log("info", "connected. " + str(cls.clients_connected) + " client total.") except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) def recvall(self,n): # http://stackoverflow.com/questions/17667903/python-socket-receive-large-amount-of-data # Helper function to recv n bytes or return None if EOF is hit data = '' while len(data) < n: packet = self.request.recv(n - len(data)) if not packet: return None data += packet return data def recv_bson(self): # Read 4 bytes to get the length of the BSON packet BSON_LENGTH_IN_BYTES = 4 raw_msglen = self.recvall(BSON_LENGTH_IN_BYTES) if not raw_msglen: return None msglen = struct.unpack('i', raw_msglen)[0] # Retrieve the rest of the message data = self.recvall(msglen - BSON_LENGTH_IN_BYTES) if data == None: return None data = raw_msglen + data # Prefix the data with the message length that has already been received. # The message length is part of BSONs message format # Exit on empty message if len(data) == 0: return None self.protocol.incoming(data) return True def handle(self): """ Listen for TCP messages """ cls = self.__class__ self.request.settimeout(cls.socket_timeout) while 1: try: if self.bson_only_mode: if self.recv_bson() == None: break continue # non-BSON handling data = self.request.recv(cls.incoming_buffer) # Exit on empty string if data.strip() == '': break elif len(data.strip()) > 0: self.protocol.incoming(data.strip('')) else: pass except Exception as e: pass self.protocol.log("debug", "socket connection timed out! (ignore warning if client is only listening..)") def finish(self): """ Called when TCP connection finishes """ cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() self.protocol.log("info", "disconnected. " + str(cls.clients_connected) + " client total." ) def send_message(self, message=None): """ Callback from rosbridge """ self.request.sendall(message)
class RosbridgeWebSocket(WebSocketServerProtocol): client_id_seed = 0 clients_connected = 0 authenticate = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False def onOpen(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) producer = OutgoingValve(self) self.transport.registerProducer(producer, True) producer.resumeProducing() self.protocol.outgoing = producer.relay self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() self.peer = self.transport.getPeer().host if cls.client_manager: cls.client_manager.add_client(self.client_id, self.peer) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def onMessage(self, message, binary): cls = self.__class__ if not binary: message = message.decode('utf-8') # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn( "Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.sendClose() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def outgoing(self, message): if type(message) == bson.BSON: binary = True message = bytes(message) elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False message = message.encode('utf-8') self.sendMessage(message, binary) def onClose(self, was_clean, code, reason): if not hasattr(self, 'protocol'): return # Closed before connection was opened. cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.peer) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected)
class RosbridgeWebSocket(WebSocketHandler): client_id_seed = 0 clients_connected = 0 authenticate = False def open(self): cls = self.__class__ try: self.protocol = RosbridgeProtocol(cls.client_id_seed) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False cls.client_id_seed += 1 cls.clients_connected += 1 except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: msg = json.loads(message) if msg["op"] == "auth": # check the authorization information auth_srv = rospy.ServiceProxy("authenticate", Authentication) resp = auth_srv( msg["mac"], msg["client"], msg["dest"], msg["rand"], rospy.Time(msg["t"]), msg["level"], rospy.Time(msg["end"]), ) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn("Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) def on_close(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): binary = type(message) == bson.BSON IOLoop.instance().add_callback(partial(self.write_message, message, binary)) def check_origin(self, origin): return True
class RosbridgeWebSocket(WebSocketHandler): client_id_seed = 0 clients_connected = 0 authenticate = False use_compression = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = None # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False @log_exceptions def open(self): cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False self._write_lock = threading.RLock() cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() if cls.client_manager: cls.client_manager.add_client(self.client_id, self.request.remote_ip) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") @log_exceptions def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn("Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) @log_exceptions def on_close(self): cls = self.__class__ cls.clients_connected -= 1 self.protocol.finish() if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.request.remote_ip) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) def send_message(self, message): if type(message) == bson.BSON: binary = True elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False with self._write_lock: IOLoop.instance().add_callback(partial(self.prewrite_message, message, binary)) @coroutine def prewrite_message(self, message, binary): # Use a try block because the log decorator doesn't cooperate with @coroutine. try: with self._write_lock: future = self.write_message(message, binary) # When closing, self.write_message() return None even if it's an undocument output. # Consider it as WebSocketClosedError if future is None: raise WebSocketClosedError yield future except WebSocketClosedError: rospy.logwarn_throttle(1, 'WebSocketClosedError: Tried to write to a closed websocket') raise except StreamClosedError: rospy.logwarn_throttle(1, 'StreamClosedError: Tried to write to a closed stream') raise except BadYieldError: # Tornado <4.5.0 doesn't like its own yield and raises BadYieldError. # This does not affect functionality, so pass silently only in this case. if tornado_version_info < (4, 5, 0, 0): pass else: _log_exception() raise except: _log_exception() raise @log_exceptions def check_origin(self, origin): return True @log_exceptions def get_compression_options(self): # If this method returns None (the default), compression will be disabled. # If it returns a dict (even an empty one), it will be enabled. cls = self.__class__ if not cls.use_compression: return None return {}
class Bridge(WebSocketHandler): client_id_seed = 0 clients_connected = 0 authenticate = False use_compression = False # The following are passed on to RosbridgeProtocol # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0 # seconds max_message_size = 10 * 1024 * 1024 # bytes unregister_timeout = 10.0 # seconds bson_only_mode = False master = False clients = {} loop = IOLoop.current() first = True @log_exceptions def open(self): print("[BRIDGE]: open") rospy.logwarn_once( 'The Tornado Rosbridge WebSocket implementation is deprecated.' ' See rosbridge_server.autobahn_websocket' ' and rosbridge_websocket.py') cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False self._write_lock = threading.RLock() cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() self.io_loop = IOLoop.current() cls.clients[self.client_id] = (self.io_loop, self.close) if cls.client_manager: cls.client_manager.add_client(self.client_id, self.request.remote_ip) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...") @log_exceptions def on_message(self, message): #print("[BRIDGE]: message, ", message) cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv = rospy.ServiceProxy('authenticate', Authentication) resp = auth_srv(msg['mac'], msg['client'], msg['dest'], msg['rand'], rospy.Time(msg['t']), msg['level'], rospy.Time(msg['end'])) self.authenticated = resp.authenticated if self.authenticated: rospy.loginfo("Client %d has authenticated.", self.protocol.client_id) return # if we are here, no valid authentication was given rospy.logwarn( "Client %d did not authenticate. Closing connection.", self.protocol.client_id) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message) @log_exceptions def on_close(self): print("[BRIDGE]: close") try: cls = self.__class__ cls.clients.pop(self.client_id) cls.clients_connected -= 1 self.protocol.finish() print("[BRIDGE]: protocol finished") if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.request.remote_ip) rospy.loginfo("Client disconnected. %d clients total.", cls.clients_connected) except: if cls.client_manager: cls.client_manager.remove_client(self.client_id, self.request.remote_ip) rospy.logerr("Client disconnected. %d clients total.", cls.clients_connected) @log_exceptions def send_message(self, message): if type(message) == bson.BSON: binary = True elif type(message) == bytearray: binary = True message = bytes(message) else: binary = False self.io_loop.add_callback(self.prewrite_message, message, binary) @coroutine def prewrite_message(self, message, binary): # Use a try block because the log decorator doesn't cooperate with @coroutine. try: future = self.write_message(message, binary) # When closing, self.write_message() return None even if it's an undocument output. # Consider it as WebSocketClosedError # For tornado versions <4.3.0 self.write_message() does not have a return value if future is None and tornado_version_info >= (4, 3, 0, 0): raise WebSocketClosedError yield future except WebSocketClosedError: rospy.logwarn_throttle( 1, 'WebSocketClosedError: Tried to write to a closed websocket') #raise except StreamClosedError: rospy.logwarn_throttle( 1, 'StreamClosedError: Tried to write to a closed stream') #raise except BadYieldError: # Tornado <4.5.0 doesn't like its own yield and raises BadYieldError. # This does not affect functionality, so pass silently only in this case. if tornado_version_info < (4, 5, 0, 0): pass else: _log_exception() #raise except: _log_exception() #raise @log_exceptions def check_origin(self, origin): print("[BRIDGE]: check origin") cls = self.__class__ return cls.master @log_exceptions def get_compression_options(self): # If this method returns None (the default), compression will be disabled. # If it returns a dict (even an empty one), it will be enabled. cls = self.__class__ if not cls.use_compression: return None return {} @classmethod def start(cls): print("[BRIDGE]: starting") cont = 0 while cont < 10 and (not rosgraph.is_master_online()): time.sleep(1) cont += 1 if rosgraph.is_master_online(): rospy.init_node("rosbridge_websocket", disable_signals=True) cls.client_manager = ClientManager() #if cls.first : # rospy.init_node("rosbridge_websocket", disable_signals=True) # cls.client_manager = ClientManager() # cls.first = False print("[BRIDGE]: running") cls.master = True else: print("[BRIDGE]: master not running") cls.master = False @classmethod def stop(cls): cls.master = False cls.client_id_seed = 0 print("[BRIDGE]: stopping") for loop, close in cls.clients.values(): loop.add_callback(close) @classmethod def on_master_changes(cls, status): print("Bridge master: ", status) if status: cls.loop.add_callback(cls.start) else: cls.loop.add_callback(cls.stop)