def StartCommunicator(self,topic,messagetype):
"""Start the communication through gztopic"""
# Listen for Subscribers
s_sub = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s_sub.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s_sub.bind((self.NODE_TCP_IP, self.NODE_TCP_PORT))
s_sub.listen(5)
# Register as a Publisher with Gazebo
pk = Packet()
pk.stamp.sec = int(time.time())
pk.stamp.nsec = datetime.now().microsecond
pk.type = "advertise"
pub = Publish()
pub.topic = topic #"/gazebo/Configuration/configSubscriber"
pub.msg_type = messagetype #'config_message.msgs.ConfigMessage'
pub.host = self.NODE_TCP_IP
pub.port = self.NODE_TCP_PORT
pk.serialized_data = pub.SerializeToString()
self.s_reg = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.s_reg.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.s_reg.connect((self.MASTER_TCP_IP, self.MASTER_TCP_PORT))
self.s_reg.send(hex(pk.ByteSize()).rjust(8))
self.s_reg.send(pk.SerializeToString())
print "wating for reply"
# Respond to a subscriber
try:
self.conn, address = s_sub.accept()
data = self.conn.recv(self.TCP_BUFFER_SIZE)
except Exception as e:
print "Cannot connect to the server."
print e
print
else:
print "wating for reply"
# Decode Incomming Packet
pk_sub = Packet()
pk_sub.ParseFromString(data[8:])
print "Packet:\n", pk_sub
# Decode Subscription Request
sub = Subscribe()
sub.ParseFromString(pk_sub.serialized_data)
print "Sub:\n", sub
print
def CreatePulisher(self, topic, messagetype):
# print "Dictionary len: ",len(self.clientmap)
# Register as a Publisher with Gazebo
pk = Packet()
pk.stamp.sec = int(time.time())
pk.stamp.nsec = datetime.now().microsecond
pk.type = "advertise"
pub = Publish()
pub.topic = topic #"/gazebo/Configuration/configSubscriber"
pub.msg_type = messagetype #'config_message.msgs.ConfigMessage'
pub.host = self.NODE_TCP_IP
pub.port = self.NODE_TCP_PORT
pk.serialized_data = pub.SerializeToString()
s_reg = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s_reg.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
timeout = time.time() + 60
has_waited = False
while time.time() < timeout:
try:
s_reg.connect((self.MASTER_TCP_IP, self.MASTER_TCP_PORT))
except Exception, e:
print 'Cannot connect to master, retrying ...'
has_waited = True
else:
if has_waited:
time.sleep(1)
s_reg.sendall(hex(pk.ByteSize()).rjust(8)+pk.SerializeToString())
print "Finish sending"
break
time.sleep(1)
class GzCommunicator(threading.Thread):
## Constructor
# @param self Object pointer
# @param masterip Master ip address, default is 127.0.0.1
# @param masterport Master port number, defult 11345
# @param selfport Socket local machine port number
def __init__(self,masterip = '127.0.0.1',masterport = 11345,selfport = 11451):
threading.Thread.__init__(self)
"""Initiate server"""
## Master ip address
self.MASTER_TCP_IP = masterip
## Master port number
self.MASTER_TCP_PORT = masterport
## Node ip address
self.NODE_TCP_IP = ''
## Node port number
self.NODE_TCP_PORT = selfport
## Socket buffer size
self.TCP_BUFFER_SIZE = 4096
## List of output sockets
self.outputs = []
## Dictionary of clients using gztopic string as key
self.clientmap = {}
# self.subscribers = []
## Flag of running server
self.runserver = 1
## A socket that builds communication with simulator plugin
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.server.setblocking(0)
self.server.bind((self.NODE_TCP_IP, self.NODE_TCP_PORT))
self.server.listen(5)
## Creates a thread and starts a socket
# @param self Object pointer
def run(self):
inputs = [self.server]
while self.runserver:
try:
# Setting 1 second timeout
inputready,outputready,exceptready = select.select(inputs, self.outputs, [], 1)
except select.error, e:
print "Suffering a select error"
break
except socket.error, e:
print "Suffering a socket error"
break
# Running comunication management
for s in inputready:
if s == self.server:
client, address = s.accept()
message = client.recv(self.TCP_BUFFER_SIZE)
pk_sub = Packet()
pk_sub.ParseFromString(message[8:])
print "Packet:\n", pk_sub
sub = Subscribe()
sub.ParseFromString(pk_sub.serialized_data)
print "Sub:\n", sub
client.setblocking(0)
self.outputs.append(client)
inputs.append(client)
self.clientmap[sub.topic] = client
else:
print "Receive information from others"
# handle all other sockets
try:
message = s.recv(self.TCP_BUFFER_SIZE)
if not message:
print 'Client with topic "',self.WhichCLient(self.clientmap,s),'" hung up'
s.close()
inputs.remove(s)
self.outputs.remove(s)
break
else:
print "Other message: ", message
except socket.error, e:
print 'Client with topic "',self.WhichCLient(self.clientmap,s), \
'" Suffering socket error'
# Remove
inputs.remove(s)
self.outputs.remove(s)
break