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)