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)
Exemplo n.º 2
0
  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