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)
