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 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 1: 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, e: pass self.protocol.log( "debug", "socket connection timed out! (ignore warning if client is only listening..)" )
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 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 } 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 handle(self): """ Listen for TCP messages """ cls = self.__class__ self.request.settimeout(cls.socket_timeout) while 1: try: 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, e: pass self.protocol.log("debug", "socket connection timed out! (ignore warning if client is only listening..)")
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
def test_setparam_service(self): from rosbridge_library.rosbridge_protocol import RosbridgeProtocol param = Param("/test_param") test_protocol=RosbridgeProtocol(88) def outgoing(message): print("ROSBRidge Outgoing: {}".format(message)) test_protocol.outgoing=outgoing test_protocol.incoming(param.set_command(885))
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 1: 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, e: pass self.protocol.log("debug", "socket connection timed out! (ignore warning if client is only listening..)")
def test_sync(self): with open('compressedDepth_sample_msg.txt', 'r') as outfile: sample_depth = outfile.read() with open('compressedRGB_sample_msg.txt', 'r') as outfile: sample_rgb = outfile.read() from rosbridge_library.rosbridge_protocol import RosbridgeProtocol test_protocol = RosbridgeProtocol(88) test_protocol.incoming(sample_depth) test_protocol.incoming(sample_rgb)
def test_advertise_service(self): from rosbridge_library.rosbridge_protocol import RosbridgeProtocol test_protocol=RosbridgeProtocol(88) def outgoing(message): print("ROSBRidge Outgoing: {}".format(message)) test_protocol.incoming('{"op": "service_response", "service": "/test_service", "values": {"value": "88"}, "result": true, "id": "service_request:/test_service:1"}') test_protocol.outgoing=outgoing service = Service("/test_service",type="rosapi/GetParam") test_protocol.incoming(service.advertise_command())
def test_rosbridge_false_subscription(self): from std_msgs.msg import String pub = rospy.Publisher("/test", String, queue_size=10,latch=True) pub.publish("dada") from rosbridge_library.rosbridge_protocol import RosbridgeProtocol test_protocol=RosbridgeProtocol(88) def outgoing(message): print("ROSBRidge Outgoing: {}".format(message)) test_protocol.outgoing=outgoing topic=Topic("/test","sensor_msgs/Image") test_protocol.incoming(topic.advertise_command())
def test_rosbridgeprotocol(self): from rosbridge_library.rosbridge_protocol import RosbridgeProtocol test_protocol=RosbridgeProtocol(88) def outgoing(message): print("ROSBRidge Outgoing: {}".format(message)) test_protocol.outgoing=outgoing with open('sample_config.json', 'r') as outfile: sample_config = outfile.read() parser = ConfigParser(sample_config) commands = parser.parse() test_protocol.incoming(commands[1])
def test_scan_topic(self): with open('sample_scan.txt', 'r') as outfile: sample_scan = outfile.read() from rosbridge_library.rosbridge_protocol import RosbridgeProtocol test_protocol = RosbridgeProtocol(88) def outgoing(message): print("ROSBRidge Outgoing: {}".format(message)) test_protocol.outgoing = outgoing topic = Topic("/scan", "sensor_msgs/LaserScan", latch=False) test_protocol.incoming(topic.advertise_command()) test_protocol.incoming(sample_scan)
class RosbridgePushClient(): def __init__(self, url): self.url = url parameters = { "fragment_timeout": 600, "delay_between_messages": 0, "max_message_size": None, "unregister_timeout": 10, "bson_only_mode": False } self.ws = None self.protocol = RosbridgeProtocol("main", parameters=parameters) self.protocol.outgoing = self.send def connect(self): try: self.ws = websocket.create_connection(self.url) rospy.loginfo("Connected to: %s", self.url) except Exception as exc: rospy.logerr("Unable to connect to server. Reason: %s", str(exc)) def isConnected(self): if self.ws is None: return False else: return self.ws.connected def send(self, message): if self.ws.connected: rospy.loginfo("Sending message!") self.ws.send(message) def recv(self): try: result = self.ws.recv() rospy.loginfo(result) self.protocol.incoming(result) except websocket.WebSocketConnectionClosedException as exc: rospy.logerr("Server is not responding. Error: %s", str(exc)) def close(self): # TODO: should we rebuild the Rosbridge Protocol after a disconnect? rospy.loginfo("Shutting down rosbridge_push") if self.isConnected(): rospy.loginfo("Closed websocket to: %s", self.url) self.ws.close() self.ws = None
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 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 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 ClientSession(ApplicationSession): """ """ def __init__(self, config): ApplicationSession.__init__(self, config) self.init() def init(self): self.ros_protocol = RosbridgeProtocol(0) self.producer = OutgoingValve(self) self.ros_protocol.outgoing = self.producer.relay def onConnect(self): self._transport.registerProducer(self.producer, True) self.producer.resumeProducing() self.join(self.config.realm) @inlineCallbacks def onJoin(self, details): print("session attached") #Subscribe #yield self.subscribe(self.on_data, data_domain) #Register command procedure try: yield self.register(self.on_command, command_domain) except Exception as e: print("failed to register procedure: {}".format(e)) else: print("command procedure registered") #Fake: Call command procedure ''' topic= Topic("dadatopic","std_msgs/String") self.ros_protocol.incoming(topic.advertise_command()) try: yield self.call(command_domain,topic.subscribe_command()) except Exception as e: print("Error: {}".format(e)) else: print("command sent") ''' def on_data(self,message): self.ros_protocol.incoming(command) def on_command(self, command): print("recieved command is {}".format(command)) self.ros_protocol.incoming(command) def outgoing(self, message): self.publish(data_domain, message) #print("outgoing message from rosbridgeprotocole") def onDisconnect(self): print("disconnected")
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 ros_protocol(): """ Rosbridge protocol wrapper to pre-process incoming messages, post-process outgoing messages, and store the settings and configs of each topic and service Methods: initialize_service initialize_topic incoming outgoing """ def __init__(self, id): self._protocol = RosbridgeProtocol(id) self._protocol.outgoing = self.rosbridge_outgoing #Store whether to compress/decompress topics self.incoming_topics_compression_state = {} self.outgoing_topics_compression_state = {} #Store whether to serialize/deserialize topics self.topics_serialization_state = {} #Store whether to remap topics/services self.topics_remap_state = {} self.services_remap_state = {} self.inv_services_remap_state = {} def initialize_service(self, wrapper=None, message=None): """ Initialize ROS service settings Args: wrapper/message : Either one of these arguments must be assigned as service information """ if wrapper is not None: name = wrapper.name remap = wrapper.remap elif message is not None: name = message.get("service") remap = message.get("_remap") self.setup_service_remaps(name, remap) def setup_service_remaps(self, name, remap): self.services_remap_state[name] = remap self.inv_services_remap_state = { v: k for k, v in self.services_remap_state.items() } def remap_service(self, service): return self.services_remap_state.get(service) def inv_remap_service(self, service): """ Re-remap the service """ return self.inv_services_remap_state.get(service) def initialize_topic(self, wrapper=None, message=None, isIncoming=None): """ Initialize ROS topic settings Args: wrapper/message : Either one of these arguments must be assigned as topic information isIncoming : If true, the topic messages are to be received from external source, so setup remap and store the compression state for decompressing If False, the topic messages are generated on this machine, so no remapping is required and the compression state is stored for compressing """ if wrapper is not None: topic = wrapper.name compression = wrapper.compression serialization = wrapper.serialization remap = wrapper.remap elif message is not None: topic = message["topic"] compression = message.get("_compression", "none") serialization = message.get("_serialization", "json") remap = message.get("_remap") if isIncoming: self.setup_topic_remaps(topic, remap) self.update_compression(topic, compression, isIncoming) self.update_serialization(topic, serialization) def setup_topic_remaps(self, name, remap): self.topics_remap_state[name] = remap def remap_topic(self, topic): return self.topics_remap_state.get(topic) def update_compression(self, topic, value, isIncoming): """ Store the topic's compression status Args: topic : topic name value : compression state isIncoming : If True, the compression state is stored for decompressing If False, the compression state is stored for compressing """ if isIncoming: if topic not in self.incoming_topics_compression_state: self.incoming_topics_compression_state[topic] = value else: if topic not in self.outgoing_topics_compression_state: self.outgoing_topics_compression_state[topic] = value def update_serialization(self, topic, value): if topic not in self.topics_serialization_state: self.topics_serialization_state[topic] = value def decompress(self, topic, data): if self.incoming_topics_compression_state.get(topic) == "png": data = pngcompression.decode(data) return json.loads(data) else: return data def compress(self, topic, data): if self.outgoing_topics_compression_state.get(topic) == "png": data = json.dumps(data) return pngcompression.encode(data) else: return data def serialize(self, topic, data): if self.topics_serialization_state[topic] == "msgpack": data = umsgpack.packb(data) return data else: return json.dumps(data) def deserialize(self, data): data = umsgpack.unpackb(data) return data def incoming(self, message): """ Intermediary incoming method to pre-process the message Args: message : rosbridge message """ #Convert to Dictionary, Whatever the input is if isinstance(message, str): message = json.loads(message) elif isinstance(message, bytes): message = self.deserialize(message) op = message.get("op") if op == "publish": message["msg"] = self.decompress(message["topic"], message.get("msg")) message["topic"] = self.remap_topic(message["topic"]) elif op == "advertise": message["topic"] = self.remap_topic(message["topic"]) elif op == "advertise_service" or op == "service_response": message["service"] = self.remap_service(message["service"]) message = json.dumps(message) #-------- #replace JSON Null values in float32 types with infinity datatype (changed according to the error for LaserScan values) message = message.replace("null", "Infinity") #-------- self._protocol.incoming(message) def rosbridge_outgoing(self, message): """ Overriden method of rosbridge protocol's Outgoing """ message = json.loads(message) op = message.get("op") if op == "publish": topic_name = message["topic"] message["msg"] = self.compress(topic_name, message.get("msg")) message = self.serialize(topic_name, message) elif op == "call_service": message["service"] = self.inv_remap_service(message["service"]) if isinstance(message, bytes): self.outgoing(message, isBinary=True, identifier=topic_name) elif isinstance(message, str): self.outgoing(message) else: message = json.dumps(message) self.outgoing(message) def outgoing(self, message, isBinary=False, identifier=None): """ Pass an outgoing message. This method should be overridden. Args: message : rosbridge protocol wrapper's outgoing message isBinary : If true, the message is of `publish` type and is binary serialized identifier : If isBinary=True , topic name will be aligned as identifier to manage thread occupancy status """ pass
class RosbridgeTcpSocket(SocketServer.BaseRequestHandler): """ TCP Socket server for rosbridge """ busy = False queue = [] client_id_seed = 0 clients_connected = 0 # list of possible parameters ( with internal default values port = 9090 # integer (portnumber) host = "" # hostname / IP as string incoming_buffer = 65536 # bytes socket_timeout = 10 # seconds retry_startup_delay = 5 # seconds # advertise_service.py: service_request_timeout = 600 # seconds check_response_delay = 0.01 # seconds wait_for_busy_service_provider = 0.01 # seconds max_service_requests = 1000000 # requests # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0.01 # seconds max_message_size = None # bytes def setup(self): cls = self.__class__ parameters = { "port": cls.port, "host": cls.host, "incoming_buffer": cls.incoming_buffer, "socket_timeout": cls.socket_timeout, "retry_startup_delay": cls.retry_startup_delay, "service_request_timeout": cls.service_request_timeout, "check_response_delay": cls.check_response_delay, "wait_for_busy_service_provider": cls.wait_for_busy_service_provider, "max_service_requests": cls.max_service_requests, "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 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 handle(self): """ Listen for TCP messages """ cls = self.__class__ self.request.settimeout(cls.socket_timeout) while 1: try: 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, e: pass self.protocol.log( "debug", "socket connection timed out! (ignore warning if client is only listening..)" )
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 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 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 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 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 RosbridgeTcpSocket(SocketServer.BaseRequestHandler): """ TCP Socket server for rosbridge """ busy = False queue = [] client_id_seed = 0 clients_connected = 0 # list of possible parameters ( with internal default values port = 9090 # integer (portnumber) host = "" # hostname / IP as string incoming_buffer = 65536 # bytes socket_timeout = 10 # seconds retry_startup_delay = 5 # seconds # advertise_service.py: service_request_timeout = 600 # seconds check_response_delay = 0.01 # seconds wait_for_busy_service_provider = 0.01 # seconds max_service_requests = 1000000 # requests # defragmentation.py: fragment_timeout = 600 # seconds # protocol.py: delay_between_messages = 0.01 # seconds max_message_size = None # bytes def setup(self): cls = self.__class__ parameters = { "port": cls.port, "host": cls.host, "incoming_buffer": cls.incoming_buffer, "socket_timeout": cls.socket_timeout, "retry_startup_delay": cls.retry_startup_delay, "service_request_timeout": cls.service_request_timeout, "check_response_delay": cls.check_response_delay, "wait_for_busy_service_provider": cls.wait_for_busy_service_provider, "max_service_requests": cls.max_service_requests, "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 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 handle(self): """ Listen for TCP messages """ cls = self.__class__ self.request.settimeout(cls.socket_timeout) while 1: try: 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, e: pass self.protocol.log("debug", "socket connection timed out! (ignore warning if client is only listening..)")
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 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 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 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)