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)
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