예제 #1
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
  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