コード例 #1
0
ファイル: client.py プロジェクト: pombredanne/ipython
    def resubmit(self, indices_or_msg_ids=None, subheader=None, block=None):
        """Resubmit one or more tasks.

        in-flight tasks may not be resubmitted.

        Parameters
        ----------

        indices_or_msg_ids : integer history index, str msg_id, or list of either
            The indices or msg_ids of indices to be retrieved

        block : bool
            Whether to wait for the result to be done

        Returns
        -------

        AsyncHubResult
            A subclass of AsyncResult that retrieves results from the Hub

        """
        block = self.block if block is None else block
        if indices_or_msg_ids is None:
            indices_or_msg_ids = -1

        if not isinstance(indices_or_msg_ids, (list,tuple)):
            indices_or_msg_ids = [indices_or_msg_ids]

        theids = []
        for id in indices_or_msg_ids:
            if isinstance(id, int):
                id = self.history[id]
            if not isinstance(id, str):
                raise TypeError("indices must be str or int, not %r"%id)
            theids.append(id)

        for msg_id in theids:
            self.outstanding.discard(msg_id)
            if msg_id in self.history:
                self.history.remove(msg_id)
            self.results.pop(msg_id, None)
            self.metadata.pop(msg_id, None)
        content = dict(msg_ids = theids)

        self.session.send(self._query_socket, 'resubmit_request', content)

        zmq.select([self._query_socket], [], [])
        idents,msg = self.session.recv(self._query_socket, zmq.NOBLOCK)
        if self.debug:
            pprint(msg)
        content = msg['content']
        if content['status'] != 'ok':
            raise self._unwrap_exception(content)

        ar = AsyncHubResult(self, msg_ids=theids)

        if block:
            ar.wait()

        return ar
コード例 #2
0
ファイル: test_poll.py プロジェクト: AndreaCrotti/pyzmq
 def test_timeout(self):
     """make sure select timeout has the right units (seconds)."""
     s1, s2 = self.create_bound_pair(zmq.PAIR, zmq.PAIR)
     tic = time.time()
     r,w,x = zmq.select([s1,s2],[],[],.005)
     toc = time.time()
     self.assertTrue(toc-tic < 1)
     self.assertTrue(toc-tic > 0.001)
     tic = time.time()
     r,w,x = zmq.select([s1,s2],[],[],.25)
     toc = time.time()
     self.assertTrue(toc-tic < 1)
     self.assertTrue(toc-tic > 0.1)
コード例 #3
0
ファイル: zmqsub.py プロジェクト: eastein/zmqfan
    def last_msg(self):
        r = [self.s]
        msg = None
        while r:
            r, w, x = zmq.select([self.s], [], [], 0.0)
            if r:
                msg = self.s.recv()

        r, w, x = zmq.select([self.s], [], [], 0.05)
        if r:
            msg = self.s.recv()

        if msg is not None:
            self._last = decode_utf8_json(msg)

        return self._last
コード例 #4
0
def read_write_nontty(socket):
    EOF_reached = False
    try:
        r, w, x  = select.select([sys.stdin], [sys.stdout], [], 0.0)
    except select.error:
        pass
    zr,zw,zx = zmq.select([socket],[socket],[], timeout = 0.0)

    if (sys.stdin in r) and (socket in zw):
        x = sys.stdin.readline()
        if not EOF_reached:
            socket.send_json({'p':x})
        if x == '':
            EOF_reached = True

    if (socket in zr) and (sys.stdout in w):
        x = socket.recv_json()
        try:
            plain = x['p']
            sys.stdout.write(plain)
            while True:
                r, w, x  = select.select([], [sys.stdout], [], 0.0)
                if sys.stdout in w:
                    sys.stdout.flush()
                    break
        except KeyError:
            if 'ctrl' in x:
                ctrlmsg = x['ctrl']
                if 'terminated' in ctrlmsg:
                    sys.stdout.write('\r\nexiting... \r\n')
                    socket.send_json({'ctrl':'terminated'})
                    return 1
    return 0
コード例 #5
0
ファイル: client.py プロジェクト: APSL/pyzmq-mdp
def mdp_request(socket, service, msg, timeout=None):
    """Synchronous MDP request.

    This function sends a request to the given service and
    waits for a reply.

    If timeout is set and no reply received in the given time
    the function will return `None`.

    :param socket:    zmq REQ socket to use.
    :type socket:     zmq.Socket
    :param service:   service id to send the msg to.
    :type service:    str
    :param msg:       list of message parts to send.
    :type msg:        list of str
    :param timeout:   time to wait for answer in seconds.
    :type timeout:    float

    :rtype list of str:
    """
    if not timeout or timeout < 0.0:
        timeout = None
    if type(msg) in (bytes, unicode):
        msg = [msg]
    to_send = [PROTO_VERSION, service]
    to_send.extend(msg)
    socket.send_multipart(to_send)
    ret = None
    rlist, _, _ = select([socket], [], [], timeout)
    if rlist and rlist[0] == socket:
        ret = socket.recv_multipart()
        ret.pop(0) # remove service from reply
    return ret
コード例 #6
0
ファイル: simple.py プロジェクト: EnTeQuAk/zerogw
 def backend_send(self, *args, backend='echo'):
     sock = getattr(self, backend)
     self.assertEqual(([], [sock], []),
         zmq.select([], [sock], [], timeout=self.timeout))
     sock.send_multipart([
         a if isinstance(a, bytes) else a.encode('utf-8')
         for a in args], zmq.NOBLOCK)
コード例 #7
0
ファイル: zmq_ex.py プロジェクト: shvar/redfs
    def send_json(self, msg):
        """
        Send a JSON-encoded message. If the connection is not established yet,
        establish it before sending.

        @raises ZMQReqConnection.NoConnection:
            if the connection could not be established.
        """
        with self.__zmq_lock:
            if self._socket is None:
                self.__connect()

            if self._socket is not None:
                sockets = [self._socket]
                # Is the socket immediately available for writing?
                rlist, wlist, xlist = zmq.select(rlist=sockets,
                                                 wlist=sockets,
                                                 xlist=sockets,
                                                 timeout=0)

                try:
                    if self._socket in wlist:
                        self._socket.send_json(msg)
                    else:
                        raise ZMQReqConnection.NoConnection()
                except (zmq.ZMQError, ZMQReqConnection.NoConnection) as e:
                    logger.error('ZMQ error on send: %s', e)
                    self.__disconnect()
                    raise ZMQReqConnection.NoConnection(
                              'No connection to Trusted Host')
コード例 #8
0
ファイル: simple.py プロジェクト: EnTeQuAk/zerogw
 def backend_recv(self, backend='echo'):
     sock = getattr(self, backend)
     if (([sock], [], []) !=
         zmq.select([sock], [], [], timeout=self.timeout)):
         raise TimeoutError()
     val = sock.recv_multipart(zmq.NOBLOCK)
     return val
コード例 #9
0
def chat_room(kernel,output_box,socket_address):
    context = zmq.Context()
    subscribe_socket = context.socket(zmq.SUB)
    subscribe_socket.connect(socket_address)
    subscribe_socket.setsockopt(zmq.SUBSCRIBE, '')
    print '>>> welcome to the zmq chatroom'
    while True:
        # print 'loop'
        zr,zw,zx = zmq.select([subscribe_socket],[],[],timeout = 0.0)
        if subscribe_socket in zr:
            # print 'ok got message'
            message = subscribe_socket.recv_json()
            if 'plain_message' in message:
                plain_message = message['plain_message']
                output_box.value += '{}: {}\n'.format(plain_message['nickname'],plain_message['message'])
            elif 'object_transfer' in message:
                # print 'aqcuiring lock on shared state'
                ipythonchat_state.shared_state_lock.acquire()
                #ok now we no, nobody else is manipulating the shared_state object
                sender = message['object_transfer']['sender']
                import pickle
                from_sender = pickle.loads(message['object_transfer']['payload'])
                output_box.value += '<< got data object from {}>>\n'.format(sender)
                ipythonchat_state.shared_state = from_sender
                ipythonchat_state.shared_state_lock.release()
                # print 'released lock, shared state is: ', ipythonchat_state.shared_state
                kernel.do_one_iteration()
        time.sleep(0.01)
コード例 #10
0
ファイル: __init__.py プロジェクト: karolaug/psychopy
 def run(self):
     context = zmq.Context().instance()
     sub_socket = context.socket(zmq.SUB)
     sub_socket.connect("tcp://" + self.server_address + ":34234")
     sub_socket.setsockopt(zmq.SUBSCRIBE, "")
     
     listener = context.socket(zmq.PAIR)
     print(str(id(self)))
     listener.bind("inproc://" + str(id(self)))
     
     readable = []
     while listener not in readable:
         readable, _, _ = zmq.select([sub_socket, listener], [], [])
         if sub_socket in readable:
             published = json.loads(sub_socket.recv())
             if published.get("uuid") == self.uuid:
                 if not self.failed and published.get("status_name") == "failed":
                     self.failed = True
                     self.status_handler(self.alive, self.failed)
                 elif not self.alive and published.get("peers") and "mx" in published['peers']:
                     self.alive = True
                     self.status_handler(self.alive, self.failed)
     listener.recv()
     listener.close()
     sub_socket.close()
コード例 #11
0
ファイル: backend.py プロジェクト: nolanw/acquire-server
 def _runloop(self):
     """A single run-through of all sockets handled by this backend."""
     inputs = [self.pull_socket, sys.stdin]
     outputs = [] if self.pub_queue.empty() else [self.pub_socket]
     exceptionals = inputs + [self.pub_socket]
     read, write, error = zmq.select(inputs, outputs, exceptionals)
     for fileno in read:
         if fileno == self.pull_socket:
             self.route_message(self.pull_socket.recv_json())
         elif fileno == sys.stdin.fileno():
             for _ in sys.stdin:
                 pass
             sys.exit(0)
     for fileno in write:
         if fileno == self.pub_socket:
             try:
                 self.pub_socket.send_json(self.pub_queue.get_nowait())
             except Queue.Empty:
                 pass
     for fileno in error:
         if fileno == self.pub_socket:
             raise Exception('PUB socket in exceptional state')
         elif fileno == self.pull_socket:
             raise Exception('PULL socket in exceptional state')
         elif fileno == sys.stdin.fileno():
             raise Exception('stdin in exceptional state')
コード例 #12
0
ファイル: server.py プロジェクト: marceln-gh/webhook-server
def run_loop():
    while True:
        http_loop()

        rlist, _, _ = zmq.select([httpd, zmq_socket], [], [])

        if zmq_socket in rlist:
            new_subscriber()
コード例 #13
0
def trySend(sock, message):
    reply = None
    for _ in range(3):
        sock.send_json(message)
        ready = zmq.select([sock],[],[],10.0)
        if ready:
            return sock.recv_json()
        print "Awaiting response from server"
    return reply
コード例 #14
0
ファイル: client.py プロジェクト: jhmsd1981/ipython
    def _connect(self, sshserver, ssh_kwargs, timeout):
        """setup all our socket connections to the cluster. This is called from
        __init__."""
        
        # Maybe allow reconnecting?
        if self._connected:
            return
        self._connected=True

        def connect_socket(s, url):
            url = util.disambiguate_url(url, self._config['location'])
            if self._ssh:
                return tunnel.tunnel_connection(s, url, sshserver, **ssh_kwargs)
            else:
                return s.connect(url)
            
        self.session.send(self._query_socket, 'connection_request')
        r,w,x = zmq.select([self._query_socket],[],[], timeout)
        if not r:
            raise error.TimeoutError("Hub connection request timed out")
        idents,msg = self.session.recv(self._query_socket,mode=0)
        if self.debug:
            pprint(msg)
        msg = ss.Message(msg)
        content = msg.content
        self._config['registration'] = dict(content)
        if content.status == 'ok':
            if content.mux:
                self._mux_socket = self._context.socket(zmq.XREQ)
                self._mux_socket.setsockopt(zmq.IDENTITY, self.session.session)
                connect_socket(self._mux_socket, content.mux)
            if content.task:
                self._task_scheme, task_addr = content.task
                self._task_socket = self._context.socket(zmq.XREQ)
                self._task_socket.setsockopt(zmq.IDENTITY, self.session.session)
                connect_socket(self._task_socket, task_addr)
            if content.notification:
                self._notification_socket = self._context.socket(zmq.SUB)
                connect_socket(self._notification_socket, content.notification)
                self._notification_socket.setsockopt(zmq.SUBSCRIBE, b'')
            # if content.query:
            #     self._query_socket = self._context.socket(zmq.XREQ)
            #     self._query_socket.setsockopt(zmq.IDENTITY, self.session.session)
            #     connect_socket(self._query_socket, content.query)
            if content.control:
                self._control_socket = self._context.socket(zmq.XREQ)
                self._control_socket.setsockopt(zmq.IDENTITY, self.session.session)
                connect_socket(self._control_socket, content.control)
            if content.iopub:
                self._iopub_socket = self._context.socket(zmq.SUB)
                self._iopub_socket.setsockopt(zmq.SUBSCRIBE, b'')
                self._iopub_socket.setsockopt(zmq.IDENTITY, self.session.session)
                connect_socket(self._iopub_socket, content.iopub)
            self._update_engines(dict(content.engines))
        else:
            self._connected = False
            raise Exception("Failed to connect!")
コード例 #15
0
def handle_tty(cmd,cid,socket):
    print 'handling tty docker session'

    import pty
    import shlex
    master, slave = pty.openpty()

    p = subprocess.Popen(shlex.split(cmd), stdin = slave, stdout = slave, stderr = slave)
    print 'started container with pid: {}'.format(p.pid)

    term_size = socket.recv_json()['ctrl']['term_size']
    set_winsize(master,term_size['rows'],term_size['cols'],p.pid)

    while True:
        #print "polling"
        r, w, x = select.select([master],[master],[master], 0.0)
        #print "master poll: r: {} w: {} x: {}".format(r,w,x)

        zr,zw,zx = zmq.select([socket], [socket],[socket], timeout = 0.0)
        #print "ZMQ poll: r: {} w: {} x: {}".format(zr,zw,zx)

        procpoll = p.poll()
        #print 'process: {}'.format(procpoll)

        if (procpoll is not None) and (socket in zw):
	    print "ending session because process ended"
            socket.send_json({'ctrl':'terminated'})
            print 'wait for ack from client'    
	    ack = socket.recv_json()
            print "return"
            return
        
        if (master in r) and (socket in zw):
            #print "reading!"
            fromprocess = os.read(master,1024)
            #print 'sending {}'.format(fromprocess)
            socket.send_json({'p':fromprocess})

        if (master in w) and (socket in zr):
            #  Wait for next request from client
            #print "recv"
            message = socket.recv_json()
            #print("Received request: {} length: {}".format(message,len(message)))
	    try:
                os.write(master,message['p'])
            except KeyError:
		if 'ctrl' in message:
		    ctrlmsg = message['ctrl']
        	    if 'term_size' in ctrlmsg:
		        set_winsize(master,ctrlmsg['term_size']['rows'],ctrlmsg['term_size']['cols'],p.pid)
		    if 'signal' in ctrlmsg:
 		        print 'got signal: {}'.format(ctrlmsg['signal']) 
                        os.kill(p.pid,ctrlmsg['signal'])
			if ctrlmsg['signal'] in [signal.SIGHUP,signal.SIGTERM,signal.SIGKILL]:
                            stop_container(get_container_id(cid))
                            return
コード例 #16
0
ファイル: zmqsub.py プロジェクト: eastein/zmqfan
 def recv(self, timeout=0.0):
     r, w, x = zmq.select([self.s], [], [], timeout)
     if r:
         try:
             self._last = decode_utf8_json(self.s.recv())
             return self._last
         except ValueError:
             raise NoMessagesException
     else:
         raise NoMessagesException
コード例 #17
0
ファイル: simple.py プロジェクト: asvetlov/zerogw
 def control(self, *args):
     sock = self.zmq.socket(zmq.REQ)
     try:
         sock.connect(CONTROL_ADDR)
         sock.send_multipart([a.encode('utf-8') for a in args])
         self.assertEqual(([sock], [], []),
             zmq.select([sock], [], [], timeout=self.timeout))
         return sock.recv_multipart()
     finally:
         sock.close()
コード例 #18
0
ファイル: zmqsub.py プロジェクト: eastein/zmqfan
def select(rlist, wlist, xlist, timeout):
    i, rlist = _mkindex(rlist)
    wi, wlist = _mkindex(wlist)
    xi, xlist = _mkindex(xlist)

    i.update(wi)
    i.update(xi)

    r, w, x = zmq.select(rlist, wlist, xlist, timeout)

    return _useindex(r, i),  _useindex(w, i), _useindex(x, i)
コード例 #19
0
ファイル: test_poll.py プロジェクト: AndreaCrotti/pyzmq
    def test_pair(self):
        s1, s2 = self.create_bound_pair(zmq.PAIR, zmq.PAIR)

        # Sleep to allow sockets to connect.
        wait()

        rlist, wlist, xlist = zmq.select([s1, s2], [s1, s2], [s1, s2])
        self.assert_(s1 in wlist)
        self.assert_(s2 in wlist)
        self.assert_(s1 not in rlist)
        self.assert_(s2 not in rlist)
コード例 #20
0
ファイル: tworoutes.py プロジェクト: sguzwf/zerogw
 def backend_recv(self, backend=None):
     if backend is None:
         sock = self.chatfw
     else:
         raise NotImplementedError(backend)
     if ([sock], [], []) != zmq.select([sock], [], [], timeout=self.timeout):
         raise TimeoutError()
     val = sock.recv_multipart()
     if val[1] == b"heartbeat":
         return self.backend_recv(backend=backend)
     return val
コード例 #21
0
def zmq_monitor(sio, socketname):
    context = zmq.Context()
    socket = context.socket(zmq.PULL)
    socket.bind(socketname)

    while True:
        zr, zw, zx = zmq.select([socket], [socket], [socket], timeout=0.0)
        if socket in zr:
            jsondata = socket.recv_json()
            senddata = json.dumps(jsondata)
            sio.emit(jsondata["to"], senddata, room=jsondata["compid"])
        time.sleep(0.01)
コード例 #22
0
ファイル: wlimit.py プロジェクト: EnTeQuAk/zerogw
 def backend_recv(self, backend=None):
     if backend is None:
         sock = self.chatfw
     else:
         sock = self.minigame
     if (([sock], [], []) !=
         zmq.select([sock], [], [], timeout=self.timeout)):
         raise TimeoutError()
     val =  sock.recv_multipart()
     if val[1] == b'heartbeat':
         return self.backend_recv(backend=backend)
     return val
コード例 #23
0
def read_write_nontty(socket,writer,outfile):
    try:
        r, w, x  = select.select([], [outfile], [], 0.0)
    except select.error:
        pass
    zr,zw,zx = zmq.select([socket],[socket],[], timeout = 0.0)

    if (socket in zr) and (outfile in w):
        x = socket.recv_json()
        result = handle_message(x,socket,writer,outfile)
        if result == 1: return 1

    return 0
コード例 #24
0
def trySend(sock, message):
    reply = None
    for _ in range(4):
        try:
            sock.send_json(message)
        except zmq.ZMQError:
            # try resetting the socket
            sock.close()
            sock.connect("tcp://brazil.vampire:5556")
        ready,_,_ = zmq.select([sock],[],[],10.0)
        if ready:
            return sock.recv_json()
        print "Awaiting response from server"
    return reply
コード例 #25
0
ファイル: __init__.py プロジェクト: AndreaCrotti/pyzmq
 def _select_recv(self, multipart, socket, **kwargs):
     """call recv[_multipart] in a way that raises if there is nothing to receive"""
     if zmq.zmq_version_info() >= (3,1,0):
         # zmq 3.1 has a bug, where poll can return false positives,
         # so we wait a little bit just in case
         # See LIBZMQ-280 on JIRA
         time.sleep(0.1)
     
     r,w,x = zmq.select([socket], [], [], timeout=5)
     assert len(r) > 0, "Should have received a message"
     kwargs['flags'] = zmq.DONTWAIT | kwargs.get('flags', 0)
     
     recv = socket.recv_multipart if multipart else socket.recv
     return recv(**kwargs)
コード例 #26
0
ファイル: zmqclass.py プロジェクト: dtbinh/soccer2
	def recv(self):
		# check to see if there is read, write, or erros
		r, w, e = zmq.select([self.socket], [], [], self.poll_time)

		topic = ''
		msg = {}

		# should this be a for loop? I don't think so???
		if len(r) > 0:
			topic, jmsg = r[0].recv_multipart()
			msg = json.loads(jmsg)

		# topic, jmsg = self.socket.recv_multipart()
		# msg = json.loads(jmsg)
		return topic, msg
コード例 #27
0
ファイル: client.py プロジェクト: MagnetonBora/zmq
    def rawrequest(self, service, msg, timeout=None):
        if not timeout or timeout < 0.0:
            timeout = None
	if type(msg) in (bytes, str):
            msg = [msg]
	to_send = [self._proto_version, service]
	to_send.extend(msg)
	self.socket.send_multipart(to_send)
	ret = None
        rlist, _, _ = select([self.socket], [], [], timeout)
        if rlist and rlist[0] == self.socket:
            ret = self.socket.recv_multipart()
            ret.pop(0) # remove service from reply
            ret.pop(0)
	return ret
コード例 #28
0
def start_docker(cmd, cid, socket, logfile, msgstub):
    cmd = cmd + "| cat"  # make sure there is not interactive updating
    writelog = open(logfile, "w")
    readlog = open(logfile, "r")
    p = subprocess.Popen(cmd, shell=True, stdout=writelog, stderr=writelog)
    while True:
        r, w, x = select.select([readlog], [], [], 0.0)
        zr, zw, zx = zmq.select([socket], [socket], [socket], timeout=0.0)
        procpoll = p.poll()
        if procpoll is not None:
            print "ending session because process ended"
            break
        if (readlog in r) and (socket in zw):
            fromprocess = os.read(readlog.fileno(), 1024)
            if fromprocess:
                socket.send_json(dict(p=fromprocess, **msgstub))
コード例 #29
0
ファイル: event_collector.py プロジェクト: aretha-hep/mprtect
def eventcollector(elastic_ip):
    context = zmq.Context()
    pull_socket = context.socket(zmq.PULL)
    pull_socket.bind("tcp://*:3141")

    nevents = 0
    put_event = es_put.get_putter(elastic_ip)
    click.echo('start collection of events with forwarding to {}'.format(elastic_ip))
    while True:
        zr,zw,zx = zmq.select([pull_socket], [],[], timeout = 0.0)
        if pull_socket in zr:
            message = pull_socket.recv_json()
            nevents = nevents+1
            print '{} events received'.format(nevents)
            put_event(message)
        time.sleep(0.001)
コード例 #30
0
def handle_nontty(cmd,cid,socket):
    print 'handling non tty docker session'

    import shlex
    p = subprocess.Popen(shlex.split(cmd), stdin = subprocess.PIPE, stdout = subprocess.PIPE, stderr = subprocess.STDOUT)
    print 'started container with pid: {}'.format(p.pid)

    while True:
        if p.poll() is not None:
            print "ending session because process ended"
            socket.send_json({'ctrl':'terminated'})
            print 'wait for ack from client'
            ack = socket.recv_json()
            print "return"
            return

        writefds = []
        if not p.stdin.closed:
            writefds.append(p.stdin)

        r,w,x = select.select([p.stdout],writefds,[],0.0)
        zr,zw,zx = zmq.select([socket], [socket],[socket], timeout = 0.0)
        if (socket in zr) and (p.stdin in w):
            message = socket.recv_json()
            try:
                if message['p'] == '':
                    print 'EOF of input'
                    p.stdin.close()
                else:
                    p.stdin.write(message['p'])
            except KeyError:
		if 'ctrl' in message:
		    ctrlmsg = message['ctrl']
                    if 'signal' in ctrlmsg:
                        print 'got signal: {}'.format(ctrlmsg['signal'])
                        os.kill(p.pid,ctrlmsg['signal'])
			if ctrlmsg['signal'] in [signal.SIGHUP,signal.SIGTERM,signal.SIGKILL]:
			    stop_container(get_container_id())
                            return
        if (p.stdout in r) and (socket in zw):
            x = os.read(p.stdout.fileno(),1024)
            socket.send_json({'p':x})

    print 'read reamining stdout buffer'
    for x in  p.stdout.readlines():
        socket.send_json({'p':x}) 
    return
コード例 #31
0

if __name__ == '__main__':
    zmqctx = zmq.Context()

    sock_txt = zmqctx.socket(zmq.SUB)
    sock_txt.connect("tcp://localhost:13373")
    sock_txt.setsockopt(zmq.SUBSCRIBE, "")

    sock_rych = zmqctx.socket(zmq.PUB)
    sock_rych.setsockopt(zmq.HWM, 100)
    sock_rych.bind('tcp://*:13374')

    try:
        while True:
            while not zmq.select([sock_txt], [], [], 3)[0]:
                time.sleep(.1)
            article = sock_txt.recv_pyobj()

            if is_bloomberg_scorable(article):
                print 'processing %r' % (article['id'], )
                add_bloomberg_score(article)
            else:
                print '(%s %s)' % (article['id'], article['lang'])

            sock_rych.send_pyobj(article)
    except:
        traceback.print_exc()
    finally:
        sock_txt.close()
        sock_rych.close()
コード例 #32
0
ファイル: recorder.py プロジェクト: ohad-i/RovVision2
def recorder():
    global telemFile, videoFile, videoQFile, doRec

    cnt = 0
    imgCnt = 0
    imgDilution = 4  #save full frame each
    hasHighQuality = False
    while True:
        socks = zmq.select(subs_socks, [], [], 0.001)[0]
        ts = time.time()

        cnt += 1
        if cnt % 200 == 0:
            total, used, free = shutil.disk_usage("/")
            if free // 2**30 < 5:
                doRec = False
                print("***Low disk space - record Stopped! ******" * 5)

        for sock in socks:
            ret = sock.recv_multipart()
            #topic,data=ret[0],pickle.loads(ret[1])
            topic = ret[0]
            if topic in mpsDict.keys():
                mpsDict[topic].calcMPS()
                if topic == zmq_topics.topic_system_state:
                    newDoRec = pickle.loads(ret[1])['record']
                    if (newDoRec) and (not doRec):
                        recPath = initRec()
                        print('record started, %s' % recPath)
                        doRec = True
                    elif (not newDoRec) and doRec:
                        print('Stop recording...')
                        doRec = False

                if doRec:
                    if topic == zmq_topics.topic_stereo_camera:
                        imgCnt += 1
                        imMetaData = pickle.loads(ret[-3])

                        hasHighQuality = imMetaData[-1]
                        if hasHighQuality:
                            with open(videoFile, 'ab') as fid:
                                # write image raw data
                                fid.write(ret[-1])
                        '''
                        imShape = pickle.loads(ret[1])[1]
                        imRaw = np.frombuffer(ret[-1], dtype='uint8').reshape(imShape)
                        
                        low = cv2.resize(imRaw, (imShape[1]//2, imShape[0]//2))
                        '''
                        low = ret[-2]
                        with open(videoQFile, 'ab') as fid:
                            # write image raw data
                            fid.write(low)  #.tobytes())
                        with open(telemFile, 'ab') as fid:
                            # write image metadata
                            recImMetadata = ret[:-2]
                            recImMetadata.append(hasHighQuality)
                            pickle.dump([ts, recImMetadata], fid)
                    else:
                        with open(telemFile, 'ab') as fid:
                            pickle.dump([ts, ret], fid)
コード例 #33
0
ファイル: ue4_bridge.py プロジェクト: MorS25/DroneSimLab
def listener():

    rospy.init_node('ue4_bridge', anonymous=True, log_level=rospy.DEBUG)
    rate=rospy.Rate(1000)
    rospy.loginfo('starting...')
    publishers={}
    start=rospy.get_time()
    while not rospy.is_shutdown():
        while len(zmq.select([zmq_sub],[],[],0)[0])>0:
            topic, shape, data = zmq_sub.recv_multipart()
            #rospy.loginfo(rospy.get_caller_id() + ' got topic %s', topic)
            topic=topic.decode()
            #rospy.loginfo('topic is..%s',topic)
            if topic not in publishers:
                if 'depth' not in topic:
                    publishers[topic]=rospy.Publisher(topic,Image,queue_size=2)
                else:
                    publishers[topic+'_rgb8']=rospy.Publisher(topic+'_rgb8',Image,queue_size=2)
                    publishers[topic+'_range_mm']=rospy.Publisher(topic+'_range_mm',Image,queue_size=2)
                    publishers[topic+'_cam_info']=rospy.Publisher(topic+'_cam_info',CameraInfo,queue_size=2)

            #import pdb;pdb.set_trace()
            shape=struct.unpack('lll',shape)
            if 'depth' not in topic:
                img=np.fromstring(data,'uint8').reshape(shape)
                publishers[topic].publish(cvbridge.cv2_to_imgmsg(img,'bgr8'))
            else:
                #img is in the format of RGBA A store depth in cm and all in float16 format
                tstamp=rospy.get_rostime() 
                img=np.fromstring(data,'float16').reshape(shape)
                img_max_val = 5.0 #assuming I right
                depth_camera_img_rgb = (img[:,:,[2,1,0]].clip(0,img_max_val)/img_max_val*255).astype('uint8')
                img_rgb=cvbridge.cv2_to_imgmsg(depth_camera_img_rgb,'bgr8')
                img_rgb.header.stamp=tstamp
                publishers[topic+'_rgb8'].publish(img_rgb)

                #conversions to be compatible with intel realsense camera
                depth_err_val=65504
                depth_img_f16=img[:,:,3]
                #max_distance=64#m
                depth_img_f16[depth_img_f16==depth_err_val]=0
                depth_final_mm=(depth_img_f16*10).astype('uint16') #cm to mm
                img_depth=cvbridge.cv2_to_imgmsg(depth_final_mm,'mono16')
                img_depth.header.stamp=tstamp
                publishers[topic+'_range_mm'].publish(img_depth)
                cam_info = CameraInfo()
                cam_info.header.stamp=tstamp
                cam_info.distortion_model='plumb_bob' 
                cam_info.D=[0.0]*6

                cam_info.K=[    160.0,  0.0,    160.0, 
                                0.0,    160.0,  120.0,
                                0.0,    0.0,    1.0]

                cam_info.R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]

                cam_info.P=[    160.0,  0.0,    160.0,  0,
                                0.0,    160.0,  120.0,  0,
                                0.0,    0.0,    1.0  ,  0]
                cam_info.header.frame_id='camera_depth_optical_frame'
                cam_info.height = 240
                cam_info.width = 320
                cam_info.roi.height = 240
                cam_info.roi.width = 320
                cam_info.roi.do_rectify = False
                publishers[topic+'_cam_info'].publish(cam_info)



            if cvshow:
                if 'depth' in topic:
                    clip_dist_cm=30*100
                    aaa=img.max()
                    inds2=img==aaa
                    img=img.clip(0,clip_dist_cm)
                    #img=np.fromstring(data,'uint8').reshape(shape)
                    rospy.loginfo('maxmin %f %f'%(img.max(),img.min()))
                    img=img.astype('float')/clip_dist_cm*255
                    img[inds2]=255
                    #import pdb;pdb.set_trace()
                    cv2.imshow(topic,img.astype('uint8'))
                else:
                    cv2.imshow(topic,cv2.resize(cv2.resize(img,(1024,1024)),(512,512)))
                cv2.waitKey(1)
        rate.sleep()
コード例 #34
0
ファイル: unreal_proxy.py プロジェクト: zivzone/DroneSimLab
def main_loop(gworld):
    print('-- actors --')
    for p in ph.GetActorsNames(gworld):
        print(p)
    print('-- textures --')
    drone_textures=[]
    for tn in drone_texture_names:
        drone_textures.append(ph.GetTextureByName(tn))
    drone_textures_down=[]
    for tn in drone_textures_down_names:
        drone_textures_down.append(ph.GetTextureByName(tn))
    drone_textures_depth=[]
    for tn in drone_textures_depth_names:
        drone_textures_depth.append(ph.GetTextureByName(tn))

    if not all(drone_textures):
        print("Error, Could not find all textures")
        while 1:
            yield
    drone_actors=[]
    for drn in drone_actors_names:
        drone_actors.append(ph.FindActorByName(gworld,drn))

    if not all(drone_actors):
        print("Error, Could not find all drone actors")
        while 1:
            yield

    for _ in range(10): #need to send it a few time don't know why.
        print('sending state main loop')
        socket_pub.send_multipart([config.topic_unreal_state,b'main_loop'])
        yield
    drone_start_positions=[np.array(ph.GetActorLocation(drone_actor)) for drone_actor in drone_actors]
    positions=[None for _ in range(config.n_drones)]
    while 1:
        for drone_index in range(config.n_drones):
            socket_sub=drone_subs[drone_index]
            drone_actor=drone_actors[drone_index]
            while len(zmq.select([socket_sub],[],[],0)[0])>0:
                topic, msg = socket_sub.recv_multipart()
                positions[drone_index]=pickle.loads(msg)
                #print('-----',positions[drone_index])

            position=positions[drone_index]
            if position is not None:
                new_pos=drone_start_positions[drone_index]+np.array([position['posx'],position['posy'],position['posz']])*100 #turn to cm
                #print('-----',drone_index,new_pos)
                ph.SetActorLocation(drone_actor,new_pos)
                ph.SetActorRotation(drone_actor,(position['roll'],position['pitch'],position['yaw']))
                positions[drone_index]=None
        yield
        for drone_index in range(config.n_drones):
            #img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
            topics=[]
            imgs=[]

            img=ph.GetTextureData(drone_textures[drone_index])
            topics.append(config.topic_unreal_drone_rgb_camera%drone_index)
            imgs.append(img)

            if drone_index<len(drone_textures_down):
                img_down=ph.GetTextureData(drone_textures_down[drone_index])
                topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'down')
                imgs.append(img_down)

            if drone_index<len(drone_textures_depth):
                img_depth=ph.GetTextureData16f(drone_textures_depth[drone_index],channels=[0,1,2,3]) #depth data will be in A componnent
                #img_depth=ph.GetTextureData(drone_textures_depth[drone_index],channels=[2]) #depth data will be in red componnent
                topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'depth')
                imgs.append(img_depth)

            #topics=[config.topic_unreal_drone_rgb_camera%drone_index,
            #        config.topic_unreal_drone_rgb_camera%drone_index+b'down',
            #        config.topic_unreal_drone_rgb_camera%drone_index+b'depth']
            #imgs=[  ph.GetTextureData(drone_textures[drone_index]),
            #        ph.GetTextureData(drone_textures_down[drone_index]),
            #        ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
            if pub_cv:
                for topic,img in zip(topics,imgs):
                    #socket_pub.send_multipart([topic,pickle.dumps(img,2)])
                    #print('--->',img.shape)
                    socket_pub.send_multipart([topic,struct.pack('lll',*img.shape),img.tostring()])
                    #socket_pub.send_multipart([topic,pickle.dumps(img,-1)])

            if show_cv:
                cv2.imshow('drone camera %d'%drone_index,img)
                cv2.waitKey(1)
コード例 #35
0
ファイル: __init__.py プロジェクト: qsnake/pyzmq
 def recv_multipart(self, socket, *args, **kwargs):
     """call recv_multipart in a way that raises if there is nothing to receive"""
     r,w,x = zmq.select([socket], [], [], timeout=5)
     assert len(r) > 0, "Should have received a message"
     return socket.recv_multipart(*args, **kwargs)
コード例 #36
0
 def backend_send(self, *args):
     self.assertEqual(([], [self.chatout], []),
                      zmq.select([], [self.chatout], [],
                                 timeout=self.timeout))
     self.chatout.send_multipart(
         [a if isinstance(a, bytes) else a.encode('utf-8') for a in args])
コード例 #37
0
ファイル: laptop.py プロジェクト: fabubaker/Hive
    socket.connect("tcp://%s" % ip)
    socket.setsockopt(zmq.SUBSCRIBE, '')
    sockets.append(socket)

time.sleep(1)
count = 1

f = open("results.dat", "w+")
f.write("time \t pi1 \t pi2 \t pi3 \t pi4 \t hive \n")

final = ""
try:
    while True:
        amps = {}
        slab = {}
        readable, writable, exceptional = zmq.select(sockets, [], [])
        for socket in readable:
            topic = socket.recv()
            data = socket.recv()
            print "-----"
            print get_amplitude(data)

            amps[socket] = 20 * math.log10(get_amplitude(data) / float(2**15))
            slab[socket] = data

        print "============="
        print ">>>>>" + str(count)
        print amps.values()
        maxamp = max(amps.values())
        print maxamp
コード例 #38
0
def main():
    flock = FlockManager(
        match_found,
        config["BLEET_TIMEOUT"],
        config["SERVICE_TIMEOUT"]
    )

    logger.info("Shepherd starting.")

    while True:
        # Wait until either the public or sheep socket has messages waiting
        zmq.select([public, sheep], [], [], timeout = 5)

        # Will grab all of the outstanding messages from the outside and place them
        # in the request queue
        while public.getsockopt(zmq.EVENTS) & zmq.POLLIN != 0:
            request = public.recv_json()
            logger.debug("Raw test request: %s", str(request))

            request = TestRequest.from_dict(request)
            try:
                submission = \
                    Submission.objects.get(id = ObjectId(request.submission_id))
            except Submission.DoesNotExist as e:
                logger.warning(
                    "Received test request for non-existant submission [%s].",
                    str(request.submission_id)
                )
                continue
            except bson.errors.InvalidId as e:
                logger.warning("Received malformed test request. %s", str(e))
                continue

            try:
                assignment = Assignment.objects.get(id = submission.assignment)
            except Assignment.DoesNotExist as e:
                logger.error(
                    "Received test request for a submission [%s] referencing "
                    "an invalid assignment [%s].",
                    str(submission.id),
                    str(submission.assignment)
                )
                continue

            if not assignment.test_harness:
                logger.warning(
                    "Received test request for a submission [%s] referencing "
                    "an assignment [%s] that does not have a test harness "
                    "associated with it.",
                    str(submission.id),
                    str(submission.assignment)
                )
                continue

            try:
                test_harness = \
                    TestHarness.objects.get(id = assignment.test_harness)
            except TestHarness.DoesNotExit as e:
                logger.error(
                    "Received test request for a submission [%s] referencing "
                    "an assignment [%s] that references a non-existant test "
                    "harness [%s].",
                    str(submission.id),
                    str(submission.assignment),
                    str(assignment.test_harness)
                )
                continue

            # Gather all the necessary information from the test request
            # received from the outside.
            processed_request = InternalTestRequest(
                submission.id,
                test_harness.config.get("galah/timeout",
                    config["BLEET_TIMEOUT"].seconds),
                test_harness.config.get("galah/environment", {})
            )

            logger.info("Received test request.")

            flock.received_request(processed_request)


        # Will grab all of the outstanding messages from the sheep and process them
        while sheep.getsockopt(zmq.EVENTS) & zmq.POLLIN != 0:
            try:
                sheep_identity, sheep_message = router_recv_json(sheep)
                sheep_message = FlockMessage.from_dict(sheep_message)
                logger.debug(
                    "Received message from sheep: %s",
                    str(sheep_message)
                )
            except ValueError as e:
                logger.error("Could not decode sheep's message: %s", str(e))
                logger.debug(
                    "Exception thrown while decoding sheep's message...",
                    exc_info = sys.exc_info()
                )
                continue

            if sheep_message.type == "distress":
                logger.warn("Received distress message. Sending bloot.")
                router_send_json(
                    sheep, sheep_identity, FlockMessage("bloot", "").to_dict()
                )

            elif sheep_message.type == "bleet":
                logger.debug(
                    "Sheep [%s] bleeted. Sending bloot.",
                    repr(sheep_identity)
                )

                result = flock.sheep_bleeted(sheep_identity)

                # Under certain circumstances we want to completely ignore a
                # bleet (see FlockManager.sheep_bleeted() for more details)
                if result is FlockManager.IGNORE:
                    logger.debug("Ignoring bleet.")
                    continue

                if not result:
                    router_send_json(
                        sheep,
                        sheep_identity,
                        FlockMessage("identify", "").to_dict()
                    )

                    logger.info(
                        "Unrecognized sheep [%s] connected, identify sent.",
                        repr(sheep_identity)
                    )

                    continue

                router_send_json(
                    sheep,
                    sheep_identity,
                    FlockMessage("bloot", "").to_dict()
                )
            elif sheep_message.type == "environment":
                if not flock.manage_sheep(sheep_identity, sheep_message.body):
                    logger.warn(
                        "Received environment from an already-recognized sheep."
                    )
            elif sheep_message.type == "result":
                logger.info("Received test result from sheep.")
                logger.debug(
                    "Received test result from sheep: %s",
                    str(sheep_message.body)
                )

                try:
                    submission_id = ObjectId(sheep_message.body["id"])

                    submission = Submission.objects.get(id = submission_id)

                    test_result = TestResult.from_dict(sheep_message.body)
                    try:
                        test_result.save()
                    except InvalidDocument:
                        logger.warn(
                            "Test result is too large for the database.",
                            exc_info = True
                        )
                        test_result = TestResult(failed = True)
                        test_result.save()

                    submission.test_results = test_result.id
                    submission.save()
                except (InvalidId, Submission.DoesNotExist) as e:
                    logger.warn(
                        "Could not retrieve submission [%s] for test result "
                        "received from sheep [%s].",
                        str(submission_id),
                        repr(sheep_identity)
                    )

                    continue

                router_send_json(
                    sheep,
                    sheep_identity,
                    FlockMessage(
                        "bloot", sheep_message.body["id"]
                    ).to_dict()
                )

                if not flock.sheep_finished(sheep_identity):
                    logger.info(
                        "Got result from sheep [%s] who was not processing "
                        "a test request.",
                        repr(sheep_identity)
                    )

        # Let the flock manager get rid of any dead or killed sheep.
        lost_sheep, killed_sheep = flock.cleanup()

        if lost_sheep:
            logger.warn(
                "%d sheep lost due to bleet timeout: %s",
                len(lost_sheep),
                str([repr(i) for i in lost_sheep])
            )

        if killed_sheep:
            logger.warn(
                "%d sheep lost due to request timeout: %s",
                len(killed_sheep),
                str([repr(i) for i in killed_sheep])
            )
コード例 #39
0
def update_graph(axes):
    global hdl_pos, hdl_arrow, ch, sh
    tic = time.time()
    new_data = False
    while 1:
        socks = zmq.select(subs_socks, [], [], 0.001)[0]
        if time.time() - tic >= 0.09:
            print('too much time break', time.time() - tic())
            break
        if len(socks) == 0:
            break
        else:
            for sock in socks:
                ret = sock.recv_multipart()
                topic, data = ret
                #print('--- topic ---',topic)
                data = pickle.loads(ret[1])
                if topic == zmq_topics.topic_tracker:
                    #new_data=True
                    new_ref = False
                    keys = ['valid', 'pt_l', 'pt_r', 'range', 'ref_cnt']
                    #print([(i,data[i]) for i in keys])
                    new_ref = data['ref_cnt'] != tin_data.get('ref_cnt', -1)
                    for k in keys:
                        tin_data[k] = data[k]
                    pickle.dump(tin_data, fd)
                    #print(tin_data)
                    new_data = True
                    ypr = (tin_data['yaw'], tin_data['pitch'],
                           tin_data['roll'])
                    xy = tin_data['pt_l']
                    ret = tracer.feed(tin_data['sonar'][0], new_ref, ypr,
                                      xy[0], xy[1])
                    ret = (ret[1], -ret[0])
                    gdata.pos_hist.add(ret)
                    gdata.trace_hist.add(ret)
                    gdata.curr_pos = ret
                    gdata.heading_rot = tin_data['yaw']
                    print('---', ret)

                if topic == zmq_topics.topic_imu:
                    for k in ['yaw', 'pitch', 'roll']:
                        tin_data[k] = data[k]
                if topic == zmq_topics.topic_sonar:
                    tin_data['sonar'] = (data[0] / 100.0, data[1] / 100.0)
                    gdata.range_arr.add(tin_data['sonar'][0])
                    #toprint=['valid','pt_l','pt_r','range']
                    #print('--imu--',data)

    if not pause_satus and new_data:
        xs = np.arange(len(gdata.trace_hist))
        pos_arr = gdata.pos_hist()
        hdl_pos[0].set_ydata(pos_arr[:, 1])
        hdl_pos[0].set_xdata(pos_arr[:, 0])
        #hdl_last_pos
        for i in [0, 1]:
            hdl_trace[i][0].set_xdata(xs)
            hdl_trace[i][0].set_ydata(gdata.trace_hist()[:, i])
        ax2.set_xlim(len(gdata.trace_hist) - 100, len(gdata.trace_hist))
        ax2.set_ylim(-0.2 * 4, 0.2 * 4)
        hdl_arrow.remove()
        hdl_arrow = ax1.arrow(gdata.curr_pos[0],
                              gdata.curr_pos[1],
                              -ch * 0.1,
                              -sh * 0.1,
                              width=0.3)

        cx, cy = gdata.map_center[:2]
        ax1.set_xlim(-rad + cx, rad + cx)
        ax1.set_ylim(-rad + cy, rad + cy)

        xs = np.arange(len(gdata.range_arr))
        hdl_range[0][0].set_xdata(xs)
        #print(pos_arr[:,2][-3:])
        hdl_range[0][0].set_ydata(gdata.range_arr.buf)
        ax3.set_xlim(len(xs) - 100, len(xs))
        ax3.set_ylim(0, 2)

        axes.figure.canvas.draw()
コード例 #40
0
ファイル: client_example.py プロジェクト: uc-csse/landrov
control_socket = context.socket(zmq.PUB)
control_socket.connect("tcp://192.168.8.106:5556")
print('connected to landrov server')

############      main loop        ##################

start_time = time.time()
while 1:
    k = cv2.waitKey(20)
    #if sensor_socket.poll(0.001):

    if k != -1:
        if k == 27 or k == ord('q'):
            break
    if len(zmq.select([sensor_socket], [], [], 0)[0]):
        topic, buf = sensor_socket.recv_multipart()
        #print('got topic',topic)
        if topic == b'rgbimage':
            img = cv2.imdecode(np.fromstring(buf, dtype=np.uint8),
                               cv2.IMREAD_COLOR)
            cv2.imshow('img', img)
        if topic == b'depthimage':
            depth = cv2.imdecode(np.fromstring(buf, dtype=np.uint8),
                                 cv2.IMREAD_GRAYSCALE)
            color_depth = cv2.applyColorMap(depth, cv2.COLORMAP_JET)
            cv2.imshow('depth', color_depth)

    tdiff = time.time() - start_time

    fact = 0.6
コード例 #41
0
 def tick(self):
     for sock in zmq.select([self.result_pull, self.router], [], [])[0]:
         if sock is self.router:
             self.recv_request()
         elif sock is self.result_pull:
             self.recv_chunk_result()
コード例 #42
0
ファイル: execute.py プロジェクト: mayankv03/mayankhms
    def run_cell(self, cell, cell_index=0, store_history=False):
        parent_msg_id = self.kc.execute(cell.source,
                                        store_history=store_history,
                                        stop_on_error=not self.allow_errors)
        self.log.debug("Executing cell:\n%s", cell.source)
        exec_timeout = self._get_timeout(cell)
        deadline = None
        if exec_timeout is not None:
            deadline = monotonic() + exec_timeout

        cell.outputs = []
        self.clear_before_next_output = False

        # we need to have a reply, and return to idle before we can consider the cell executed
        idle = False
        execute_reply = None

        deadline_passed = 0
        deadline_passed_max = 5
        while not idle or execute_reply is None:
            # we want to timeout regularly, to see if the kernel is still alive
            # this is tested in preprocessors/test/test_execute.py#test_kernel_death
            # this actually fakes the kernel death, and we might be able to use the xlist
            # to detect a disconnected kernel
            timeout = min(1, deadline - monotonic())
            # if we interrupt on timeout, we allow 1 seconds to pass till deadline
            # to make sure we get the interrupt message
            if timeout >= 0.0:
                # we include 0, which simply is a poll to see if we have messages left
                rlist = zmq.select([
                    self.kc.iopub_channel.socket, self.kc.shell_channel.socket
                ], [], [], timeout)[0]
                if not rlist:
                    self._check_alive()
                    if monotonic() > deadline:
                        self._handle_timeout(exec_timeout)
                        deadline_passed += 1
                        assert self.interrupt_on_timeout
                        if deadline_passed <= deadline_passed_max:
                            # when we interrupt, we want to do this multiple times, so we give some
                            # extra time to handle the interrupt message
                            deadline += self.iopub_timeout
                if self.kc.shell_channel.socket in rlist:
                    msg = self.kc.shell_channel.get_msg(block=False)
                    if msg['parent_header'].get('msg_id') == parent_msg_id:
                        execute_reply = msg
                if self.kc.iopub_channel.socket in rlist:
                    msg = self.kc.iopub_channel.get_msg(block=False)
                    if msg['parent_header'].get('msg_id') == parent_msg_id:
                        if msg['msg_type'] == 'status' and msg['content'][
                                'execution_state'] == 'idle':
                            idle = True
                        else:
                            self.process_message(msg, cell, cell_index)
                    else:
                        self.log.debug(
                            "Received message for which we were not the parent: %s",
                            msg)
            else:
                self._handle_timeout(exec_timeout)
                break

        return execute_reply, cell.outputs
コード例 #43
0
async def recv_and_process():
    keep_running = True
    pitch, roll = 0, 0
    target_range = 0
    pid = None
    ab = None
    rate = 0
    system_state = {'mode': []}
    jm = Joy_map()

    while keep_running:
        socks = zmq.select(subs_socks, [], [], 0.005)[0]
        for sock in socks:
            ret = sock.recv_multipart()
            topic, data = ret[0], pickle.loads(ret[1])

            if topic == zmq_topics.topic_sonar:
                new_sonar_ts, range = data[
                    'ts'], data['sonar'][0] / 1000  # Convert to m
                if ab is None:
                    ab = ab_filt([range, 0])
                else:
                    depth, rate = ab(range, new_sonar_ts - sonar_ts)
                sonar_ts = new_sonar_ts

                if 'SONAR_HOLD' in system_state['mode']:
                    if pid is None:
                        pid = PID(**sonar_pid)
                    else:
                        ud_command = -pid(range, target_range, rate, 0)
                        debug_pid = {
                            'P': pid.p,
                            'I': pid.i,
                            'D': pid.d,
                            'C': ud_command,
                            'T': target_range,
                            'N': range,
                            'TS': new_sonar_ts
                        }
                        pub_sock.send_multipart([
                            zmq_topics.topic_sonar_hold_pid,
                            pickle.dumps(debug_pid, -1)
                        ])
                        thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
                                                 pitch, roll)
                        thrusters_source.send_pyobj(
                            ['sonar', time.time(), thruster_cmd])
                else:
                    if pid is not None:
                        pid.reset()
                    thrusters_source.send_pyobj(
                        ['sonar', time.time(),
                         mixer.zero_cmd()])
                    target_range = range

            if topic == zmq_topics.topic_axes:
                jm.update_axis(data)
                target_range += -jm.joy_mix()['ud'] / 25.0

            if topic == zmq_topics.topic_imu:
                pitch, roll = data['pitch'], data['roll']

            if topic == zmq_topics.topic_system_state:
                _, system_state = data

        await asyncio.sleep(0.001)
コード例 #44
0
def main():
    ############ connecting to landrov ##################

    context = zmq.Context()
    # sensor_socket = context.socket(zmq.SUB)
    # sensor_socket.setsockopt(zmq.CONFLATE, 1)
    # sensor_socket.connect("tcp://192.168.8.106:5557")
    # sensor_socket.setsockopt(zmq.SUBSCRIBE,b'pointcloud')
    # sensor_socket.setsockopt(zmq.SUBSCRIBE,b'rgbimage')

    control_socket = context.socket(zmq.PUB)
    control_socket.connect("tcp://192.168.8.106:5556")
    print('connected to landrov server')

    ############      main loop        ##################

    #initialize Kernal
    kernel = np.ones((5, 5), np.uint8)

    # Initialize the PCL visualizer.
    # visual_2 = pcl.pcl_visualization.CloudViewing()

    visual = pcl.pcl_visualization.PCLVisualizering(
        '3D Viewer')  # Initialize the visualizer.

    depth_scale = 0.0010000000474974513 + 0.03
    # depth_scale = 0.03

    #Create PointCloud
    pc = rs.pointcloud()

    clipping_distance_in_meters = 2
    clipping_distance = clipping_distance_in_meters / depth_scale

    found_cloud = False
    updated_cloud = False

    cnt = 0
    left_turns = 0
    right_turns = 0

    while 1:

        # k=cv2.waitKey(20)
        #     #if sensor_socket.poll(0.001):
        #
        # if k!=-1:
        #     if k  == 27 or k == ord('q'):
        #         break
        #     # import pdb; pdb.set_trace()
        #         # cv2.imshow('img', color)

        if not found_cloud:
            time.sleep(0.5)
            sensor_socket = context.socket(zmq.SUB)
            # sensor_socket.setsockopt(zmq.CONFLATE, 1)
            sensor_socket.connect("tcp://192.168.8.106:5557")
            sensor_socket.setsockopt(zmq.SUBSCRIBE, b'pointcloud')

            while not found_cloud:
                if len(zmq.select([sensor_socket], [], [], 0)[0]):
                    topic, buf = sensor_socket.recv_multipart()
                    # print('got topic',topic)
                    if topic == b'pointcloud':
                        cloud_vtx = np.fromstring(buf,
                                                  dtype=[('f0', '<f4'),
                                                         ('f1', '<f4'),
                                                         ('f2', '<f4')])
                        found_cloud = True
            # print("Recieved PC")
            sensor_socket.close()

        else:

            time_start = time.time()
            print("Original Point Cloud Size:", cloud.size, "points")
            # import pdb; pdb.set_trace()
            cloud = pcl.PointCloud()
            cloud.from_list(cloud_vtx)

            # Removes points outside of the range 0.1 to 1.5 in the Z axis.
            cloud_filtered = cloud_filter(cloud, "z", 0.1, 1.2)

            #Applies a voxel grid to the cloud filter
            voxel_cloud = voxel_filter(cloud_filtered, 0.04, 0.04, 0.04)

            print("Voxel filter Cloud Size:", voxel_cloud.size, "points")

            # Remove the ground plane inrfont of the robot
            ground_removed = remove_ground(voxel_cloud)

            # objects = cluster_extraction(ground_removed, 0.05, 20, 1000)
            #
            # closest_object_index = find_closest(objects)
            #
            # closest_object = objects[closest_object_index]

            # Create a passthrough filter shows points infront of the robot
            path_objects = cloud_filter(ground_removed, "x", -0.3, 0.3)

            # Create a passthrough filter shows points left of the robot
            left_objects = cloud_filter(ground_removed, "x", -2, -0.3)

            # Create a passthrough filter shows points right of the robot
            right_objects = cloud_filter(ground_removed, "x", 0.3, 2)

            if path_objects.size > 10:
                if left_objects.size > right_objects.size:
                    command = "right"
                else:
                    command = "left"
            else:
                command = "forward"

            visual.RemoveAllPointClouds(0)

            template = "Points left {},Points infront {}, points right {}"
            print(
                template.format(left_objects.size, path_objects.size,
                                right_objects.size))

            path_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
                path_objects, 0, 0, 255)
            left_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
                left_objects, 255, 0, 0)
            right_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(
                right_objects, 0, 255, 0)

            visual.AddPointCloud_ColorHandler(left_objects, left_color,
                                              b'outliers')
            visual.AddPointCloud_ColorHandler(right_objects, right_color,
                                              b'inliers')
            visual.AddPointCloud_ColorHandler(path_objects, path_color,
                                              b'original')

            print(command)

            # # Provide a colour for the point cloud.
            # cloud_color = pcl.pcl_visualization.PointCloudColorHandleringCustom(closest_object, 255, 255, 255)
            # # Display the point cloud.
            # visual.AddPointCloud_ColorHandler(closest_object, cloud_color, b'ground_removed')

            print("Time taken", time.time() - time_start)

            cnt += 1
            updated_cloud = True
            # import pdb; pdb.set_trace()

        # import pdb; pdb.set_trace()
        # Navigate Robot
        if updated_cloud:

            fact = 0.6

            if command == "forward":
                cmd = (fact, fact)  # straight
                travel_time = 3.0
                print("Going Forward")
            elif command == "right":
                cmd = (fact, -fact)  # turn right
                travel_time = 0.5
                print("Turn Right")
            elif command == "left":
                cmd = (-fact, fact)  # turn left
                travel_time = 0.5
                print("Turn Left")
            else:
                print("No Command")
            send_command(cmd, travel_time)
            found_cloud = False
            updated_cloud = False
コード例 #45
0
def listener():
    cnt = 0
    rgb = None
    while 1:
        while len(zmq.select([zmq_sub], [], [], 0.001)[0]) > 0:
            data = zmq_sub.recv_multipart()
            topic = data[0]
            frame_cnt, shape = pickle.loads(data[1])
            if topic == topic_stereo:
                imgl = np.frombuffer(data[2], 'uint8').reshape(shape)
                imgr = np.frombuffer(data[3], 'uint8').reshape(shape)

                ch, cw, cl = shape
                ch = ch // 2
                cw = cw // 2

                if config.rov_type == 2:  #resizing for prev cameras
                    nh, nw = config.cam_res_rgby, config.cam_res_rgbx
                    imgl = imgl[ch - nh // 2:ch + nh // 2,
                                cw - nw // 2:cw + nw // 2, :]
                    imgr = imgr[ch - nh // 2:ch + nh // 2,
                                cw - nw // 2:cw + nw // 2, :]
                    shape = imgl.shape

                if 0 and cvshow:
                    #if 'depth' in topic:
                    #    cv2.imshow(topic,img)
                    #else:
                    #cv2.imshow(topic,cv2.resize(cv2.resize(img,(1920/2,1080/2)),(512,512)))
                    imgls = imgl[::2, ::2]
                    imgrs = imgr[::2, ::2]
                    cv2.imshow(topic.decode() + 'l', imgls)
                    cv2.imshow(topic.decode() + 'r', imgrs)
                    cv2.waitKey(1)
                zmq_pub.send_multipart([
                    zmq_topics.topic_stereo_camera,
                    pickle.dumps([frame_cnt, shape]),
                    imgl.tostring(),
                    imgr.tostring()
                ])
                time.sleep(0.001)
                zmq_pub.send_multipart([
                    zmq_topics.topic_stereo_camera_ts,
                    pickle.dumps((frame_cnt, time.time()))
                ])  #for sync

            if topic == topic_depth:
                img = np.frombuffer(data[2], 'float16').reshape(shape)
                min_range = img.min()
                #import pdb;pdb.set_trace()
                img = np.squeeze(img).copy()
                #img.byteswap()

                img_show = (img / 10.0).clip(0, 255).astype('uint8')
                img[img > 5000] = np.nan
                max_range = np.nanmax(img)
                print('sonar::', min_range, max_range)
                pub_sonar.send_multipart([
                    zmq_topics.topic_sonar,
                    pickle.dumps([min_range, max_range])
                ])

                if cvshow:
                    cv2.imshow(topic.decode(), img_show)
                    cv2.waitKey(1)

            ### test
        time.sleep(0.010)
        if cnt % 20 == 0 and rgb is not None:
            print('send...', cnt, rgb.shape)
        cnt += 1
コード例 #46
0
ファイル: oiTracker_plugin.py プロジェクト: ohad-i/RovVision2
async def recv_and_process():
    keep_running = True
    tracker = sTracker.tracker()
    tracker.init()
    imBuffer = {}
    maxImBuffer = 20

    system_state = {'mode': []}
    trackerInitiated = False

    curYaw, curPitch, curRoll = 0.0, 0.0, 0.0
    rates = [0] * 3

    ## IDS
    fovX = 40.58  # deg.
    fovY = 30.23  # deg.

    pidY = None
    pidP = None
    pidR = None
    pidX = None

    tPitch = 0.0
    tRoll = 0.0
    tYaw = 0.0
    tX = 0.0

    maxAllowedPitch = 15  # deg

    while keep_running:
        socks = zmq.select(subs_socks, [], [], 0.005)[0]
        for sock in socks:
            ret = sock.recv_multipart()
            topic, data = ret[0], pickle.loads(ret[1])

            if topic == zmq_topics.topic_stereo_camera:
                frameCnt, shape, ts, curExp, hasHighRes = pickle.loads(ret[1])
                frame = np.frombuffer(ret[-2], 'uint8').reshape(
                    (shape[0] // 2, shape[1] // 2, 3)).copy()

                if 'IM_TRACKER_MODE' in system_state[
                        'mode'] and trackerInitiated:
                    trackRes = tracker.track(frame)
                    #print('--->', trackRes)

                    if tracker.trackerInit:
                        if trackRes is not None:
                            msg = [
                                zmq_topics.topic_tracker_result,
                                pickle.dumps([frameCnt, trackRes])
                            ]
                            sock_pub.send_multipart(msg)
                            centerX = frame.shape[1] // 2
                            centerY = frame.shape[0] // 2

                            iFovX = fovX / frame.shape[1]
                            iFovY = fovY / frame.shape[0]

                            #import ipdb; ipdb.set_trace()
                            trckResCenteredX = (trackRes[0] - centerX) * iFovX
                            trckResCenteredY = -(trackRes[1] - centerY) * iFovY

                            #print('--> dx=%f, dy=%f'%(trckResCenteredX-curYaw, trckResCenteredY+curPitch) )

                            if pidX == None:
                                tYaw = curYaw
                                pidX = PID(**pos_pid_x)
                                pidY = PID(**yaw_pid)
                                pidP = PID(**pitch_im_pid)
                                pidR = PID(**roll_pid)

                            tPitch = curPitch + trckResCenteredY
                            print('-1-> tP: %.2f' % tPitch)

                            tPitch = min(max(tPitch, -maxAllowedPitch),
                                         maxAllowedPitch)
                            print('-2-> tP: %.2f' % tPitch)

                            tX = centerX - trackRes[0]

                            #print('--> dx=%f, dp=%f'%(trackRes[0]-centerX, tPitch-curPitch) )
                            pitchCmd = pidP(curPitch, tPitch, 0, 0)
                            rollCmd = 0  #pidR(curRoll, tRoll, 0, 0)
                            yawCmd = pidY(curYaw, tYaw, 0, 0)

                            ts = time.time()
                            debug_pid = {
                                'P': pidR.p,
                                'I': pidR.i,
                                'D': pidR.d,
                                'C': rollCmd,
                                'T': tRoll,
                                'N': curRoll,
                                'R': rates[0],
                                'TS': ts
                            }
                            pub_sock.send_multipart([
                                zmq_topics.topic_att_hold_roll_pid,
                                pickle.dumps(debug_pid, -1)
                            ])
                            debug_pid = {
                                'P': pidP.p,
                                'I': pidP.i,
                                'D': pidP.d,
                                'C': pitchCmd,
                                'T': tPitch,
                                'N': curPitch,
                                'R': rates[1],
                                'TS': ts
                            }
                            pub_sock.send_multipart([
                                zmq_topics.topic_att_hold_pitch_pid,
                                pickle.dumps(debug_pid, -1)
                            ])
                            debug_pid = {
                                'P': pidY.p,
                                'I': pidY.i,
                                'D': pidY.d,
                                'C': yawCmd,
                                'T': tYaw,
                                'N': curYaw,
                                'R': rates[2],
                                'TS': ts
                            }
                            pub_sock.send_multipart([
                                zmq_topics.topic_att_hold_yaw_pid,
                                pickle.dumps(debug_pid, -1)
                            ])

                            xCmd = pidX(tX, 0)
                            #print('xCmd: %f tx: %f'%(xCmd, tX), pidX.p, pidX.i, pidX.d)

                            thrusterCmd = np.array(
                                mixer.mix(0, xCmd, 0, rollCmd, pitchCmd,
                                          yawCmd, curPitch, curRoll))
                            #print( {'P':pidX.p,'I':pidX.i,'D':pidX.d,'C':xCmd,'T':tX,'N':0, 'TS':ts} )

                            thrusterCmd += mixer.mix(0, 0, 0, 0,
                                                     -rates[1] * pidP.D,
                                                     -rates[2] * pidY.D, 0, 0)

                            thrusters_source.send_pyobj(
                                ['trck', time.time(), thrusterCmd])

                    else:
                        print('oitracker break...')
                        trackerInitiated = False
                        sock_pub.send_multipart([
                            zmq_topics.topic_tracker_result,
                            pickle.dumps([frameCnt, (-1, -1)])
                        ])

                        if pidX is not None:
                            print('kill controllers...')
                            pidY.reset(), pidP.reset(), pidX.reset()

                            pidY = None
                            pidP = None
                            pidX = None

                            thrusters_source.send_pyobj(
                                ['trck', time.time(),
                                 mixer.zero_cmd()])

            if topic == zmq_topics.topic_imu:
                curYaw, curPitch, curRoll = data['yaw'], data['pitch'], data[
                    'roll']
                rates = [x / np.pi * 180 for x in data['rates']]

            elif topic == zmq_topics.topic_tracker_cmd:
                #print('-->', data)
                if data['frameId'] == -1:
                    tracker.stopTracker()
                    trackerInitiated = False
                    sock_pub.send_multipart([
                        zmq_topics.topic_tracker_result,
                        pickle.dumps([-1, [-1, -1]])
                    ])

                    if pidX is not None:
                        pidY.reset(), pidP.reset(), pidX.reset()

                        pidY = None
                        pidP = None
                        pidX = None

                        thrusters_source.send_pyobj(
                            ['trck', time.time(),
                             mixer.zero_cmd()])

                    print('got kill tracker...')
                else:
                    trackerInitiated = tracker.initTracker(data['trackPnt'])
                    trackRes = tracker.track(frame)
                    print('--->', trackerInitiated, data['trackPnt'])
            if topic == zmq_topics.topic_system_state:
                system_state = data

        await asyncio.sleep(0.001)
コード例 #47
0
ファイル: jupyter.py プロジェクト: paxtonfitzpatrick/davos
def prompt_restart_rerun_buttons(pkgs):
    """
    Jupyter-specific implementation of `prompt_restart_rerun_buttons`.

    Displays a warning that the notebook kernel must be restarted in
    order to use the just-smuggled version of one or more previously
    imported `pkgs`, and displays a pair of buttons (via
    `JS_FUNCTIONS.jupyter.displayButtonPrompt`) that prompt the user to
    either (a) restart the kernel and rerun all cells up to the current
    point, or (b) ignore the warning and continue running. Then, polls
    the kernel's stdin socket until it receives a reply from the
    notebook frontend, or the kernel is restarted.

    Parameters
    ----------
    pkgs : list of str
        Packages that could not be reloaded without restarting the
        kernel.

    Returns
    -------
    None
        If the user clicks the "Continue Running" button, returns
        `None`. Otherwise, restarts the kernel and therefore never
        returns.

    See Also
    --------
    JS_FUNCTIONS.jupyter.displayButtonPrompt :
        JavaScript function for prompting user input with buttons.
    ipykernel.kernelbase.Kernel._input_request :
        Kernel method that replaces the built-in `input` in notebooks.

    Notes
    -----
    1. This method of blocking and waiting for user input is based on
       `ipykernel`'s replacement for the built-in `input` function used
       in notebook environments.

    """
    # UI: could remove warning message when "continue" button is clicked
    msg = (
        "WARNING: The following packages were previously imported by the "
        "interpreter and could not be reloaded because their compiled modules "
        f"have changed:\n\t[{', '.join(pkgs)}]\nRestart the kernel to use "
        "the newly installed version.")

    # noinspection JSUnusedLocalSymbols,JSUnresolvedFunction
    button_args = dedent("""
        const buttonArgs = [
            {
                text: 'Restart Kernel and Rerun Cells',
                onClick: () => {restartRunCellsAbove();},
            },
            {
                text: 'Continue Running',
                result: null,
            },
        ]
    """)
    display_button_prompt_full = dedent(f"""
        {JS_FUNCTIONS.jupyter.restartRunCellsAbove};
        console.log('restartRunCellsAbove defined');
        
        {JS_FUNCTIONS.jupyter.displayButtonPrompt};
        console.log('displayButtonPrompt defined');
        
        {button_args};
        console.warn(`{msg}`);
        displayButtonPrompt(buttonArgs, true);
    """)

    # get_ipython() exists globally when imported into IPython context
    kernel = get_ipython().kernel
    stdin_sock = kernel.stdin_socket

    print(f"\033[0;31;1m{msg}\033[0m")

    # flush output before creating button display
    sys.stdout.flush()
    sys.stderr.flush()

    # flush ipykernel stdin socket to purge stale replies
    while True:
        try:
            # noinspection PyUnresolvedReferences
            # (dynamically imported names not included in stub files)
            stdin_sock.recv_multipart(zmq.NOBLOCK)
        except zmq.ZMQError as e:
            # noinspection PyUnresolvedReferences
            # (dynamically imported names not included in stub files)
            if e.errno == zmq.EAGAIN:
                break
            raise

    # noinspection PyTypeChecker
    display(Javascript(display_button_prompt_full))

    while True:
        try:
            # zmq.select (zmq.sugar.poll.select) args:
            #   - list of sockets/FDs to be polled for read events
            #   - list of sockets/FDs to be polled for write events
            #   - list of sockets/FDs to be polled for error events
            #   - timeout (in seconds; None implies no timeout)
            rlist, _, xlist = zmq.select([stdin_sock], [], [stdin_sock], 0.01)
            if rlist or xlist:
                ident, reply = kernel.session.recv(stdin_sock)
                if ident is not None or reply is not None:
                    break
        except Exception as e:  # pylint: disable=broad-except
            if isinstance(e, KeyboardInterrupt):
                # re-raise KeyboardInterrupt with simplified traceback
                # (excludes some convoluted calls to internal
                # IPython/zmq machinery)
                raise KeyboardInterrupt("Interrupted by user") from None
            kernel.log.warning("Invalid Message:", exc_info=True)

    # noinspection PyBroadException
    try:
        value = reply['content']['value']
    except Exception:  # pylint: disable=broad-except
        if ipykernel.version_info[0] >= 6:
            _parent_header = kernel._parent_ident['shell']
        else:
            _parent_header = kernel._parent_ident
        kernel.log.error(f"Bad input_reply: {_parent_header}")
        value = ''

    if value == '\x04':
        # end of transmission
        raise EOFError
    return value
コード例 #48
0
 def send(self, msg, timeout=10.0):
     r, w, x = zmq.select([], [self.s], [], timeout)
     if w:
         self.s.send(codecs.encode(json.dumps(msg), 'utf8'))
コード例 #49
0
ファイル: unreal_proxy.py プロジェクト: zivzone/DroneSimLab
def main_loop(gworld):
    frame_cnt = 0
    print('-- actors list --', gworld)
    i = 0
    for p in ph.GetActorsNames(gworld, 1024 * 1000):
        p_wc = p[1::2]
        a = ph.FindActorByName(gworld, p_wc)
        print(i, p,
              ph.GetActorLocation(a)
              if a else a)  #,'pos',ph.GetActorLocation(a))
        if 0:
            import pdb
            pdb.set_trace()
        i += 1
        #print(p,'pos',ph.GetActorLocation(a))
    print('-- textures --')
    print('-- starting openrov simulation --')
    drone_textures = []
    for tn in drone_texture_names:
        drone_textures.append(ph.GetTextureByName(tn))
    drone_textures_depth = []
    for tn in drone_textures_depth_names:
        drone_textures_depth.append(ph.GetTextureByName(tn))

    if not all(drone_textures):
        print("Error, Could not find all textures")
        while 1:
            yield
    drone_actors = []
    for drn in drone_actors_names:
        drone_actors.append(ph.FindActorByName(gworld, drn))

    if not all(drone_actors):
        print("Error, Could not find all drone actors")
        while 1:
            yield

    #change cameras angle
    for cam_name in ['SceneCaptureBROV1left', 'SceneCaptureBROV1right']:
        #ca=ph.FindActorByName(gworld,'SceneCaptureBROV1left')
        ca = ph.FindActorByName(gworld, cam_name)
        #ph.SetActorRotation(ca,(1,-1,1)) #left
        #ph.SetActorRotation(ca,(1,-1,89)) #forward
        #ph.SetActorRotation(ca,(1,-89,89)) #down (pitch -90)
        #pitch range >=1  <=89
        #pitch=45
        #pitch=1
        ph.SetActorRotation(ca, (1, -pitch, 89))

        yield
        #ph.SetActorRotation(caml,(1,1,89)) #facing down
        print('camera ' + cam_name + ' rotation ---', ph.GetActorRotation(ca))

    #print('--- ',caml)

    for _ in range(10):  #need to send it a few time don't know why.
        print('sending state main loop')
        socket_pub.send_multipart([config.topic_unreal_state, b'main_loop'])
        yield
    print('initial_pos is ', initial_pos)
    drone_start_positions = [
        np.array(
            ph.GetActorLocation(drone_actor
                                ) if initial_pos is None else initial_pos)
        for drone_actor in drone_actors
    ]
    positions = [None for _ in range(config.n_drones)]
    while 1:
        for drone_index in range(config.n_drones):
            socket_sub = drone_subs[drone_index]
            drone_actor = drone_actors[drone_index]
            while len(zmq.select([socket_sub], [], [], 0)[0]) > 0:
                topic, msg = socket_sub.recv_multipart()
                positions[drone_index] = pickle.loads(msg)
                #print('-----',positions[drone_index])

            position = positions[drone_index]
            if position is not None:
                new_pos = drone_start_positions[drone_index] + np.array([
                    position['posx'], position['posy'], position['posz']
                ]) * 100  #turn to cm
                #print('-----',drone_index,position)
                ph.SetActorLocation(drone_actor, new_pos)
                ph.SetActorRotation(
                    drone_actor,
                    (position['roll'], position['pitch'], position['yaw']))
                positions[drone_index] = None
        yield
        #for drone_index in range(config.n_drones):
        #img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
        topics = []
        imgs = []

        img = ph.GetTextureData(drone_textures[0])
        imgr = ph.GetTextureData(drone_textures[1])
        imgl = ph.GetTextureData(drone_textures[2])
        topics.append(config.topic_unreal_drone_rgb_camera % 0)
        topics.append(config.topic_unreal_drone_rgb_camera % 0 + b'l')
        topics.append(config.topic_unreal_drone_rgb_camera % 0 + b'r')
        imgs = [img, imgl, imgr]

        #if drone_index<len(drone_textures_depth):
        #    img_depth=ph.GetTextureData16f(drone_textures_depth[drone_index],channels=[0,1,2,3]) #depth data will be in A componnent
        #img_depth=ph.GetTextureData(drone_textures_depth[drone_index],channels=[2]) #depth data will be in red componnent
        #    topics.append(config.topic_unreal_drone_rgb_camera%drone_index+b'depth')
        #    imgs.append(img_depth)

        if pub_cv:
            for topic, img in zip(topics, imgs):
                #socket_pub.send_multipart([topic,pickle.dumps(img,2)])
                #print('--->',img[:].max(),img[:].min())
                #rgb=img[...,::-1]
                socket_pub.send_multipart([
                    topic,
                    struct.pack('llll', *img.shape, frame_cnt),
                    img.tostring()
                ])
                #socket_pub.send_multipart([topic,pickle.dumps(img,-1)])

        if show_cv:
            cv2.imshow('drone camera %d' % drone_index, img)
            cv2.waitKey(1)

        frame_cnt += 1
コード例 #50
0
ファイル: controller.py プロジェクト: ohad-i/RovVision2
async def recv_and_process():
    keep_running = True
    thruster_cmd = np.zeros(8)
    timer10hz = time.time() + 1 / 10.0
    timer20hz = time.time() + 1 / 20.0

    thrustersRate = 50.0
    thrustersTimerHz = time.time() + 1 / thrustersRate

    timer0_1hz = time.time() + 10

    initPwmFocus = 1400
    preFocusFileName = '../hw/focusState.bin'
    if os.path.exists(preFocusFileName):
        with open(preFocusFileName, 'r') as fid:
            initPwmFocus = int(fid.read(4))
            print('reset focus to:', initPwmFocus)

    gainCtl = -1
    expCtl = -1
    camStateFile = '../hw/camSate.pkl'
    if os.path.exists(camStateFile):
        try:
            with open(camStateFile, 'rb') as fid:
                camState = pickle.load(fid)
                if 'aGain' in camState.keys():
                    gainCtl = camState['aGain']
                if 'aExp' in camState.keys():
                    expCtl = camState['aExp']
        except:
            print('no cam state...')

    diskUsage = int(
        os.popen('df -h / | tail -n 1').readline().strip().split()[-2][:-1])
    system_state = {
        'ts': time.time(),
        'arm': False,
        'mode': [],
        'lights': 0,
        'focus': initPwmFocus,
        'record': False,
        'diskUsage': diskUsage,
        'autoGain': gainCtl,
        'autoExp': expCtl
    }  #lights 0-5

    thrusters_dict = {}

    jm = Joy_map()

    def togle_mode(m):
        s = system_state
        if m in s['mode']:
            s['mode'].remove(m)
        else:
            s['mode'].append(m)

    def test_togle(b):
        return new_joy_buttons[b] == 1 and joy_buttons[b] == 0

    while keep_running:
        socks = zmq.select(subs_socks, [], [], 0.002)[0]
        for sock in socks:
            if sock == thruster_sink:
                source, _, thruster_src_cmd = sock.recv_pyobj()
                thrusters_dict[source] = thruster_src_cmd
            else:
                ret = sock.recv_multipart()
                topic, data = ret[0], pickle.loads(ret[1])

                if topic == zmq_topics.topic_button or topic == zmq_topics.topic_gui_diveModes:

                    jm.update_buttons(data)
                    if jm.depth_hold_event():
                        print('Toggle depth hold...')
                        if ('IM_TRACKER_MODE' in system_state['mode']) and (
                                'DEPTH_HOLD' in system_state['mode']):
                            print(
                                'failed to toggle to DEPTH_HOLD (needs to stay on DEPTH_HOLD) while IM_TRACKER_MODE'
                            )
                        else:
                            togle_mode('DEPTH_HOLD')
                    if jm.att_hold_event():
                        print('Toggle attitude hold...')
                        #if ('IM_TRACKER_MODE' in system_state['mode']) and ('ATT_HOLD' in system_state['mode']):
                        #    print('failed to toggle to ATT_HOLD (needs to stay on ATT_HOLD) while IM_TRACKER_MODE')
                        #else:
                        #    togle_mode('ATT_HOLD')
                        togle_mode('ATT_HOLD')
                    if jm.Rx_hold_event():
                        togle_mode('RX_HOLD')
                    if jm.Ry_hold_event():
                        togle_mode('RY_HOLD')
                    if jm.Rz_hold_event():
                        togle_mode('RZ_HOLD')
                    if jm.record_event():
                        system_state['record'] = not system_state['record']
                        print('record state', system_state['record'])
                    if jm.arm_event():
                        system_state['arm'] = not system_state['arm']
                        print('state arm:', system_state['arm'])
                        if not system_state['arm']:
                            system_state['mode'] = []

                elif topic == zmq_topics.topic_axes or topic == zmq_topics.topic_gui_controller:
                    jm.update_axis(data)
                    if jm.inc_lights_event():
                        system_state['lights'] = min(
                            5, system_state['lights'] + 1)
                        pub_sock.send_multipart([
                            zmq_topics.topic_lights,
                            pickle.dumps(system_state['lights'])
                        ])
                        print('lights set to ', system_state['lights'])
                    if jm.dec_lights_event():
                        system_state['lights'] = max(
                            0, system_state['lights'] - 1)
                        pub_sock.send_multipart([
                            zmq_topics.topic_lights,
                            pickle.dumps(system_state['lights'])
                        ])
                        print('lights set to ', system_state['lights'])
                    if jm.inc_focus_event():
                        system_state['focus'] = min(
                            2250, system_state['focus'] + focusResolution)
                        pub_sock.send_multipart([
                            zmq_topics.topic_focus,
                            pickle.dumps(system_state['focus'])
                        ])
                        print('focus set to ', system_state['focus'])
                    if jm.dec_focus_event():
                        system_state['focus'] = max(
                            850, system_state['focus'] - focusResolution)
                        pub_sock.send_multipart([
                            zmq_topics.topic_focus,
                            pickle.dumps(system_state['focus'])
                        ])
                        print('focus set to ', system_state['focus'])
                elif topic == zmq_topics.topic_gui_focus_controller or topic == zmq_topics.topic_autoFocus:
                    pwm = data
                    system_state['focus'] = pwm
                    pub_sock.send_multipart([
                        zmq_topics.topic_focus,
                        pickle.dumps(system_state['focus'])
                    ])
                    print('focus set to value ', system_state['focus'])

                elif topic == zmq_topics.topic_gui_autoFocus:
                    os.system('../scripts/runAutofocus.sh')

                elif topic == zmq_topics.topic_gui_start_stop_track:
                    print('start/stop tracker... ', data)
                    '''
                    if 'ATT_HOLD' not in system_state['mode']:
                        togle_mode('ATT_HOLD')
                    '''
                    if 'DEPTH_HOLD' not in system_state['mode']:
                        togle_mode('DEPTH_HOLD')

                    togle_mode('IM_TRACKER_MODE')
                    if 'IM_TRACKER_MODE' in system_state['mode']:
                        pub_sock.send_multipart(
                            [zmq_topics.topic_tracker_cmd,
                             pickle.dumps(data)])
                    else:
                        pub_sock.send_multipart([
                            zmq_topics.topic_tracker_cmd,
                            pickle.dumps({
                                'frameId': -1,
                                'trackPnt': (-1, -1)
                            })
                        ])

                elif topic == zmq_topics.topic_tracker_result:
                    #print('--->', data)
                    if data[1][0] < 0:
                        if 'IM_TRACKER_MODE' in system_state['mode']:
                            print('Tracker ended...')
                            togle_mode('IM_TRACKER_MODE')
                elif topic == zmq_topics.topic_gui_toggle_auto_exp:
                    print('got auto exposure command')
                    if expCtl == 0:
                        expCtl = 1
                    else:
                        expCtl = 0
                    system_state['autoExp'] = expCtl
                    camPubSock.send_multipart([
                        zmq_topics.topic_cam_toggle_auto_exp,
                        pickle.dumps(expCtl)
                    ])

                elif topic == zmq_topics.topic_gui_toggle_auto_gain:
                    print('got auto gain command')
                    if gainCtl == 0:
                        gainCtl = 1
                    else:
                        gainCtl = 0
                    system_state['autoGain'] = gainCtl
                    camPubSock.send_multipart([
                        zmq_topics.topic_cam_toggle_auto_gain,
                        pickle.dumps(gainCtl)
                    ])

                elif topic == zmq_topics.topic_gui_inc_exp:
                    print('got camera inc exp.')
                    camPubSock.send_multipart(
                        [zmq_topics.topic_cam_inc_exp,
                         pickle.dumps([0])])

                elif topic == zmq_topics.topic_gui_dec_exp:
                    print('got camera dec exp.')
                    camPubSock.send_multipart(
                        [zmq_topics.topic_cam_dec_exp,
                         pickle.dumps([0])])

                elif topic == zmq_topics.topic_gui_exposureVal:
                    print('got camera exp. value:', data)
                    camPubSock.send_multipart(
                        [zmq_topics.topic_cam_exp_val,
                         pickle.dumps(data)])

        tic = time.time()
        if tic - timer10hz > 0:
            timer10hz = tic + 1 / 10.0
            system_state['ts'] = tic
            pub_sock.send_multipart(
                [zmq_topics.topic_system_state,
                 pickle.dumps(system_state)])
        if tic - thrustersTimerHz > 0:
            thrustersTimerHz = tic + 1 / thrustersRate
            if not system_state['arm']:
                thruster_cmd = np.zeros(8)
            else:
                for k in thrusters_dict:
                    thruster_cmd += thrusters_dict[k]
            pub_sock.send_multipart([
                zmq_topics.topic_thrusters_comand,
                pickle.dumps((tic, list(thruster_cmd)))
            ])
            thruster_cmd = np.zeros(8)
        if tic - timer0_1hz > 0:
            timer0_1hz = tic + 10
            system_state['diskUsage'] = int(
                os.popen('df -h / | tail -n 1').readline().strip().split()[-2]
                [:-1])

            #print('botton',ret)

        await asyncio.sleep(0.001)
コード例 #51
0
ファイル: att_hold_plugin.py プロジェクト: uc-csse/RovVision2
async def recv_and_process():
    keep_running=True
    target_att=np.zeros(3)
    pid_y,pid_p,pid_r=[None]*3
    system_state={'mode':[]}

    jm=Joy_map()
    joy=None

    while keep_running:
        socks=zmq.select(subs_socks,[],[],0.005)[0]
        for sock in socks:
            ret=sock.recv_multipart()
            topic,data=ret[0],pickle.loads(ret[1])

            if topic==zmq_topics.topic_imu:
                yaw,pitch,roll=data['yaw'],data['pitch'],data['roll']
                if 0 and 'yawr' in data:
                    ans = (data['yawr'],data['pitchr'],data['rollr'])
                    yawr,pitchr,rollr=ans
                else:
                    #ans=#mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
                    #if ans is not None:
                    #    yawr,pitchr,rollr=mixer.from_ang_rates_to_euler_rates(yaw,pitch,roll,data['rates'])
                    rates = [x/np.pi*180 for x in data['rates']]

                joy = jm.joy_mix()

                if 'ATT_HOLD' in system_state['mode']:# and ans is not None:
                    if pid_y is None:
                        pid_y=PID(**yaw_pid)
                        pid_p=PID(**pitch_pid)
                        pid_r=PID(**roll_pid)
                    else:
                        #if joy and joy['inertial'] and abs(joy['yaw'])<0.05:
                        if joy and abs(joy['yaw'])<0.05:
                            yaw_cmd = pid_y(yaw,target_att[0],0,0)
                        else:
                            target_att[0]=yaw
                            yaw_cmd=0
                        #print('R{:06.3f} Y{:06.3f} YT{:06.3f} C{:06.3f}'.format(yawr,yaw,target_att[0],yaw_cmd))

                        if joy and abs(joy['pitch'])<0.1:
                            pitch_cmd = pid_p(pitch,target_att[1],0,0)
                        else:
                            target_att[1]=pitch
                            pitch_cmd=0
                        #print('R{:06.3f} P{:06.3f} PT{:06.3f} C{:06.3f}'.format(pitchr,pitch,target_att[1],pitch_cmd))
                        roll_cmd = pid_r(roll,0 if roll_target_0 else target_att[2],0,0)
                        #print('RR{:06.3f} R{:06.3f} RT{:06.3f} C{:06.3f}'.format(rollr,roll,target_att[2],roll_cmd))
                        ts=time.time()
                        debug_pid = {'P':pid_r.p,'I':pid_r.i,'D':pid_r.d,'C':roll_cmd,'T':0,'N':roll, 'R':rates[0], 'TS':ts}
                        pub_sock.send_multipart([zmq_topics.topic_att_hold_roll_pid, pickle.dumps(debug_pid,-1)])
                        debug_pid = {'P':pid_p.p,'I':pid_p.i,'D':pid_p.d,'C':pitch_cmd,'T':target_att[1],'N':pitch, 'R':rates[1],'TS':ts}
                        pub_sock.send_multipart([zmq_topics.topic_att_hold_pitch_pid, pickle.dumps(debug_pid,-1)])
                        debug_pid = {'P':pid_y.p,'I':pid_y.i,'D':pid_y.d,'C':yaw_cmd,'T':target_att[0],'N':yaw, 'R':rates[2], 'TS':ts}
                        pub_sock.send_multipart([zmq_topics.topic_att_hold_yaw_pid, pickle.dumps(debug_pid,-1)])

                        thruster_cmd = np.array(mixer.mix(0,0,0,roll_cmd,pitch_cmd,yaw_cmd,pitch,roll))
                        thruster_cmd += mixer.mix(0,0,0,-rates[0]*pid_r.D,-rates[1]*pid_p.D,-rates[2]*pid_y.D,0,0)
                        thrusters_source.send_pyobj(['att',time.time(),thruster_cmd])
                else:
                    if pid_y is not None:
                        pid_y.reset(),pid_r.reset(),pid_y.reset()
                    target_att=[yaw,0,0]
                    thrusters_source.send_pyobj(['att',time.time(),mixer.zero_cmd()])


            if topic==zmq_topics.topic_axes:
                jm.update_axis(data)


            if topic==zmq_topics.topic_button:
                jm.update_buttons(data)
                #target_depth+=data[jm.ud]


            if topic==zmq_topics.topic_system_state:
                _,system_state=data

        await asyncio.sleep(0.001)
コード例 #52
0
ファイル: unreal_proxy.py プロジェクト: zivzone/DroneSimLab
            #        ph.GetTextureData(drone_textures_down[drone_index]),
            #        ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
            if pub_cv:
                for topic,img in zip(topics,imgs):
                    #socket_pub.send_multipart([topic,pickle.dumps(img,2)])
                    #print('--->',img.shape)
                    socket_pub.send_multipart([topic,struct.pack('lll',*img.shape),img.tostring()])
                    #socket_pub.send_multipart([topic,pickle.dumps(img,-1)])

            if show_cv:
                cv2.imshow('drone camera %d'%drone_index,img)
                cv2.waitKey(1)

def kill():
    print('done!')
    socket_pub.send_multipart([config.topic_unreal_state,b'kill'])
    if show_cv:
        cv2.destroyAllWindows()
        for _ in range(10):
            cv2.waitKey(10)



if __name__=="__main__":
    while 1:
        for drone_index in range(config.n_drones):
            socket_sub=drone_subs[drone_index]
            while len(zmq.select([socket_sub],[],[],0)[0])>0:
                topic, msg = socket_sub.recv_multipart()
                print("got ",topic)
コード例 #53
0
ファイル: client.py プロジェクト: shimizukawa/ipython
    def result_status(self, msg_ids, status_only=True):
        """Check on the status of the result(s) of the apply request with `msg_ids`.
        
        If status_only is False, then the actual results will be retrieved, else
        only the status of the results will be checked.
        
        Parameters
        ----------
        
        msg_ids : list of msg_ids
            if int:
                Passed as index to self.history for convenience.
        status_only : bool (default: True)
            if False:
                Retrieve the actual results of completed tasks.
        
        Returns
        -------
        
        results : dict
            There will always be the keys 'pending' and 'completed', which will
            be lists of msg_ids that are incomplete or complete. If `status_only`
            is False, then completed results will be keyed by their `msg_id`.
        """
        if not isinstance(msg_ids, (list, tuple)):
            msg_ids = [msg_ids]

        theids = []
        for msg_id in msg_ids:
            if isinstance(msg_id, int):
                msg_id = self.history[msg_id]
            if not isinstance(msg_id, basestring):
                raise TypeError("msg_ids must be str, not %r" % msg_id)
            theids.append(msg_id)

        completed = []
        local_results = {}

        # comment this block out to temporarily disable local shortcut:
        for msg_id in theids:
            if msg_id in self.results:
                completed.append(msg_id)
                local_results[msg_id] = self.results[msg_id]
                theids.remove(msg_id)

        if theids:  # some not locally cached
            content = dict(msg_ids=theids, status_only=status_only)
            msg = self.session.send(self._query_socket,
                                    "result_request",
                                    content=content)
            zmq.select([self._query_socket], [], [])
            idents, msg = self.session.recv(self._query_socket, zmq.NOBLOCK)
            if self.debug:
                pprint(msg)
            content = msg['content']
            if content['status'] != 'ok':
                raise self._unwrap_exception(content)
            buffers = msg['buffers']
        else:
            content = dict(completed=[], pending=[])

        content['completed'].extend(completed)

        if status_only:
            return content

        failures = []
        # load cached results into result:
        content.update(local_results)
        # update cache with results:
        for msg_id in sorted(theids):
            if msg_id in content['completed']:
                rec = content[msg_id]
                parent = rec['header']
                header = rec['result_header']
                rcontent = rec['result_content']
                iodict = rec['io']
                if isinstance(rcontent, str):
                    rcontent = self.session.unpack(rcontent)

                md = self.metadata[msg_id]
                md.update(self._extract_metadata(header, parent, rcontent))
                md.update(iodict)

                if rcontent['status'] == 'ok':
                    res, buffers = util.unserialize_object(buffers)
                else:
                    print rcontent
                    res = self._unwrap_exception(rcontent)
                    failures.append(res)

                self.results[msg_id] = res
                content[msg_id] = res

        if len(theids) == 1 and failures:
            raise failures[0]

        error.collect_exceptions(failures, "result_status")
        return content
コード例 #54
0
def main_loop(gworld):
    global file_dump
    if dump_name:
        print('openning dump file dump name is ', dump_name)
        file_dump = open(dump_name, 'wb')
        os.chmod(dump_name, 0o777)
    print('-- actors 1--')
    for p in ph.GetActorsNames(gworld, 100 * 1024):
        print(p)
    print('-- textures --')
    drone_textures = []
    for tn in drone_texture_names:
        drone_textures.append(ph.GetTextureByName(tn))

    if not all(drone_textures):
        print("Error, Could not find all textures")
        while 1:
            yield
    drone_actors = []
    for drn in drone_actors_names:
        drone_actors.append(ph.FindActorByName(gworld, drn))
    reference_actor = ph.FindActorByName(gworld, reference_actor_name)
    if not all(drone_actors):
        print("Error, Could not find all drone actors")
        while 1:
            yield

    for _ in range(10):  #need to send it a few time don't know why.
        print('sending state main loop')
        socket_pub.send_multipart([config.topic_unreal_state, b'main_loop'])
        yield
    drone_start_positions = [
        np.array(ph.GetActorLocation(drone_actor))
        for drone_actor in drone_actors
    ]
    positions = [None for _ in range(config.n_drones)]
    reference_start = np.array(ph.GetActorLocation(reference_actor))
    reference_rotation = np.array(ph.GetActorRotation(reference_actor))
    while 1:
        for drone_index in range(config.n_drones):
            socket_sub = drone_subs[drone_index]
            drone_actor = drone_actors[drone_index]
            while len(zmq.select([socket_sub], [], [], 0)[0]) > 0:
                topic, msg = socket_sub.recv_multipart()
                positions[drone_index] = pickle.loads(msg)
                #print('-----',positions[drone_index])

            #position=positions[drone_index]
            #if position is not None:
            #    new_pos=drone_start_positions[drone_index]+np.array([position['posx'],position['posy'],position['posz']])*100 #turn to cm
            #    #print('-----',drone_index,new_pos)
            #    ph.SetActorLocation(drone_actor,new_pos)
            #    ph.SetActorRotation(drone_actor,(position['roll'],position['pitch'],position['yaw']))
            #    positions[drone_index]=None
            #new_pos=reference_start-ph.GetActorLocation(reference_actor)+drone_start_positions[drone_index]
            #reference_rotation = reference_rotation*0.9+np.array(ph.GetActorRotation(reference_actor))*0.1
            #ph.SetActorRotation(drone_actor,reference_rotation)
        # ph.SetActorLocation(drone_actor,new_pos)
            reference_rotation = ph.GetActorRotation(reference_actor)
            reference_location = ph.GetActorLocation(reference_actor)


#            print('-----',reference_location,reference_rotation)

        yield
        for drone_index in range(config.n_drones):
            #img=cv2.resize(ph.GetTextureData(drone_textures[drone_index]),(1024,1024),cv2.INTER_LINEAR)
            topics = []
            imgs = []

            img = ph.GetTextureData(drone_textures[drone_index])
            #print('--==--',img.shape)
            topics.append(config.topic_unreal_drone_rgb_camera % drone_index)
            imgs.append(img)

            #topics=[config.topic_unreal_drone_rgb_camera%drone_index,
            #        config.topic_unreal_drone_rgb_camera%drone_index+b'down',
            #        config.topic_unreal_drone_rgb_camera%drone_index+b'depth']
            #imgs=[  ph.GetTextureData(drone_textures[drone_index]),
            #        ph.GetTextureData(drone_textures_down[drone_index]),
            #        ph.GetTextureData(drone_textures_depth[drone_index],channels=[2])]
            if pub_cv:
                for topic, img in zip(topics, imgs):
                    #socket_pub.send_multipart([topic,pickle.dumps(img,2)])
                    #print('--->',img.shape)
                    socket_pub.send_multipart([
                        topic,
                        struct.pack('lll', *img.shape),
                        img.tostring()
                    ])
                    #socket_pub.send_multipart([topic,pickle.dumps(img,-1)])

            if show_cv:
                #img=cv2.blur(cv2.resize(img,(512,512)),(3,3))
                cv2.imshow('drone camera %d' % drone_index, img)
                cv2.waitKey(1)

            if file_dump is not None:
                pickle.dump((img, reference_rotation, reference_location),
                            file_dump, -1)
コード例 #55
0
ファイル: periph_gate.py プロジェクト: uc-csse/RovVision2
print('connected to ', detect_usb.devmap['PERI_USB'])
subs_socks=[]
subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_lights],zmq_topics.topic_controller_port))
subs_socks.append(zmq_wrapper.subscribe([zmq_topics.topic_record_state ],zmq_topics.topic_record_state_port))

#start triggering at config fps
ser.write(b'\x01')
ser.write(b'%c'%(config.fps+10))
#ser.flush()
print('trigger sent')

cur_lights_cmd = 0
prev_rec_state = 0

while True:
    socks=zmq.select(subs_socks,[],[],0.005)[0]
    for sock in socks:
        ret=sock.recv_multipart()
        topic,data=ret[0],pickle.loads(ret[1])
        if topic == zmq_topics.topic_record_state and data != prev_rec_state:
            print('Record start/stop sync light blink')
            ser.write(b'%c' % (0+2))
            time.sleep(0.5)
            ser.write(b'%c' % (LIGHTS_MAX+2))
            time.sleep(0.5)
            ser.write(b'%c' % (cur_lights_cmd+2))
            prev_rec_state = data

        if topic==zmq_topics.topic_lights:
            print('got lights command',data)
            ser.write(b'%c'%(data+2))
コード例 #56
0
ファイル: client.py プロジェクト: shimizukawa/ipython
    def _connect(self, sshserver, ssh_kwargs, timeout):
        """setup all our socket connections to the cluster. This is called from
        __init__."""

        # Maybe allow reconnecting?
        if self._connected:
            return
        self._connected = True

        def connect_socket(s, url):
            url = util.disambiguate_url(url, self._config['location'])
            if self._ssh:
                return tunnel.tunnel_connection(s, url, sshserver,
                                                **ssh_kwargs)
            else:
                return s.connect(url)

        self.session.send(self._query_socket, 'connection_request')
        r, w, x = zmq.select([self._query_socket], [], [], timeout)
        if not r:
            raise error.TimeoutError("Hub connection request timed out")
        idents, msg = self.session.recv(self._query_socket, mode=0)
        if self.debug:
            pprint(msg)
        msg = ss.Message(msg)
        content = msg.content
        self._config['registration'] = dict(content)
        if content.status == 'ok':
            if content.mux:
                self._mux_socket = self._context.socket(zmq.XREQ)
                self._mux_socket.setsockopt(zmq.IDENTITY, self.session.session)
                connect_socket(self._mux_socket, content.mux)
            if content.task:
                self._task_scheme, task_addr = content.task
                self._task_socket = self._context.socket(zmq.XREQ)
                self._task_socket.setsockopt(zmq.IDENTITY,
                                             self.session.session)
                connect_socket(self._task_socket, task_addr)
            if content.notification:
                self._notification_socket = self._context.socket(zmq.SUB)
                connect_socket(self._notification_socket, content.notification)
                self._notification_socket.setsockopt(zmq.SUBSCRIBE, b'')
            # if content.query:
            #     self._query_socket = self._context.socket(zmq.XREQ)
            #     self._query_socket.setsockopt(zmq.IDENTITY, self.session.session)
            #     connect_socket(self._query_socket, content.query)
            if content.control:
                self._control_socket = self._context.socket(zmq.XREQ)
                self._control_socket.setsockopt(zmq.IDENTITY,
                                                self.session.session)
                connect_socket(self._control_socket, content.control)
            if content.iopub:
                self._iopub_socket = self._context.socket(zmq.SUB)
                self._iopub_socket.setsockopt(zmq.SUBSCRIBE, b'')
                self._iopub_socket.setsockopt(zmq.IDENTITY,
                                              self.session.session)
                connect_socket(self._iopub_socket, content.iopub)
            self._update_engines(dict(content.engines))
        else:
            self._connected = False
            raise Exception("Failed to connect!")
コード例 #57
0
    omnirobot_manager.robot.resetOdom(0, 0, 0)
    omnirobot_manager.robot.reset()

    omnirobot_manager.robot.pubPosCmd()
    r = rospy.Rate(RL_CONTROL_FREQ)
    while not rospy.is_shutdown():
        print("wait for new command")
        msg = socket.recv_json()

        print("msg: {}".format(msg))
        omnirobot_manager.processMsg(msg)

        # wait for robot to finish the action, timeout 30s
        timeout = 30  # second
        for i in range(timeout):
            readable_list, _, _ = zmq.select([socket], [], [], 0)

            if len(readable_list) > 0:
                print("New command incomes, ignore the current command")
                continue
            if omnirobot_manager.robot.move_finished:
                print("action done")
                break

            elif i == timeout - 1:
                print("Error: timeout for action finished signal")
                exit()
            time.sleep(1)

        if IMAGE_TOPIC is not None:
            # Retrieve last image from image topic
コード例 #58
0
def listener():
    fmt_cnt_l = -1
    fmt_cnt_r = -2
    if doplot:
        plot = ploter()
        plot.__next__()
    keep_running = True
    while keep_running:
        while len(zmq.select([zmq_sub], [], [], 0.001)[0]) > 0:
            topic, info, data = zmq_sub.recv_multipart()
            #topic=topic.decode()
            info = struct.unpack('llll', info)
            shape = info[:3]
            img = np.fromstring(data, 'uint8').reshape(shape)
            if topic == topicl:
                fmt_cnt_l = info[3]
                imgl = img
            if topic == topicr:
                fmt_cnt_r = info[3]
                imgr = img

            if cvshow:
                #if 'depth' in topic:
                #    cv2.imshow(topic,img)
                #else:
                #cv2.imshow(topic,cv2.resize(cv2.resize(img,(1920/2,1080/2)),(512,512)))
                img_shrk = img[::2, ::2]

                centx = None
                centy = None
                wx = 100
                wy = 100

                if fmt_cnt_r == fmt_cnt_l:
                    disparity = stereo.compute(preisterproc(imgl),
                                               preisterproc(imgr))
                    disparityu8 = np.clip(disparity, 0, 255).astype('uint8')

                    centx = disparityu8.shape[1] // 2
                    centy = disparityu8.shape[0] // 2
                    #print('--->',disparity.max(),disparity.min(),type(disparity),imgr.shape)
                    #disparityu8[centy-wy//2:centy+wy//2,centx-wx//2:centx+wx//2]=255
                    cv2.rectangle(disparityu8,
                                  (centx - wx // 2, centy - wy // 2),
                                  (centx + wx // 2, centy + wy // 2), 255)
                    cv2.imshow('disparity', disparityu8)
                    avg_disp, min_d, max_d, cnt_d = avg_disp_win(disparity,
                                                                 centx,
                                                                 centy,
                                                                 wx,
                                                                 wy,
                                                                 tresh=10)
                    print('D {:05.2f}, {:05.2f} ,{:05.2f} ,{:05d} R {:5.2f}'.
                          format(avg_disp, min_d, max_d, cnt_d,
                                 disp2range(avg_disp)))
                    if doplot:
                        plot.send({
                            'tstemp': time.time() - start,
                            'disp': avg_disp
                        })
                centx_full = img.shape[1] // 2
                centy_full = img.shape[0] // 2
                cv2.rectangle(img,
                              (centx_full - wx // 2, centy_full - wy // 2),
                              (centx_full + wx // 2, centy_full + wy // 2),
                              (0, 0, 255))
                cv2.imshow(topic.decode(), img)

                #import pdb;pdb.set_trace()
                k = cv2.waitKey(1)
                if k == ord('q'):
                    keep_running = False
                    plot.send('stop')
                    break

            ### test
        time.sleep(0.010)
コード例 #59
0
async def recv_and_process():
    keep_running = True
    pitch, roll = 0, 0
    target_depth = 0
    pid = None
    ab = None
    rate = 0
    system_state = {'mode': []}
    jm = Joy_map()

    while keep_running:
        socks = zmq.select(subs_socks, [], [], 0.005)[0]
        for sock in socks:
            ret = sock.recv_multipart()
            topic, data = ret[0], pickle.loads(ret[1])

            if topic == zmq_topics.topic_depth:
                new_depth_ts, depth = data['ts'], data['depth']
                if ab is None:
                    ab = ab_filt([depth, 0])
                else:
                    depth, rate = ab(depth, new_depth_ts - depth_ts)
                depth_ts = new_depth_ts

                if 'DEPTH_HOLD' in system_state['mode']:
                    if pid is None:
                        pid = PID(**depth_pid)
                    else:
                        ud_command = pid(depth, target_depth, rate, 0)
                        debug_pid = {
                            'P': pid.p,
                            'I': pid.i,
                            'D': pid.d,
                            'C': ud_command,
                            'T': target_depth,
                            'N': depth,
                            'TS': new_depth_ts
                        }
                        pub_sock.send_multipart([
                            zmq_topics.topic_depth_hold_pid,
                            pickle.dumps(debug_pid, -1)
                        ])
                        thruster_cmd = mixer.mix(ud_command, 0, 0, 0, 0, 0,
                                                 pitch, roll)
                        thrusters_source.send_pyobj(
                            ['depth', time.time(), thruster_cmd])
                else:
                    if pid is not None:
                        pid.reset()
                    thrusters_source.send_pyobj(
                        ['depth', time.time(),
                         mixer.zero_cmd()])
                    target_depth = depth

            if topic == zmq_topics.topic_axes:
                jm.update_axis(data)
                target_depth += jm.joy_mix()['ud'] / 250.0
            if topic == zmq_topics.topic_gui_depthAtt:
                if data['dDepth'] is not None:
                    target_depth = data['dDepth']
                    print('set new depth: %.2f' % target_depth)

            if topic == zmq_topics.topic_imu:
                pitch, roll = data['pitch'], data['roll']

            if topic == zmq_topics.topic_system_state:
                system_state = data

        await asyncio.sleep(0.001)
コード例 #60
0
ファイル: pos_hold_plugin.py プロジェクト: uc-csse/RovVision2
async def recv_and_process():
    keep_running = True
    target_pos = np.zeros(3)
    pids = [None] * 3
    system_state = {'mode': []}

    jm = Joy_map()
    joy = None

    while keep_running:
        socks = zmq.select(subs_socks, [], [], 0.005)[0]
        for sock in socks:
            ret = sock.recv_multipart()
            topic, data = ret[0], pickle.loads(ret[1])

            if topic == zmq_topics.topic_imu:
                yaw, pitch, roll = data['yaw'], data['pitch'], data['roll']

            if topic == zmq_topics.topic_tracker:
                td = data
                cmds = [0] * 3

                for ind in range(len(pids)):
                    #mapping from track image coords to world coords
                    imap = ['dx_f', 'dy_f', 'dz_f'][ind]
                    pid_states = ['RX_HOLD', 'RY_HOLD', 'RZ_HOLD']
                    if pid_states[ind] not in system_state['mode'] \
                            or pids[ind] is None:
                        pids[ind] = PID(**pos_pids[ind])
                        if td['valid'] and imap in td:
                            x, _ = td[imap]
                            #we want it to return to 0 for the y lock
                            target_pos[ind] = 0 if ind == 1 else x
                    else:
                        if td['valid'] and imap in td:
                            x, v = td[imap]
                            cmds[ind] = -pids[ind](x, target_pos[ind], v)
                            #print('pid=',ind,x,v,str(pids[ind]))
                            ts = time.time()
                            debug_pid = {
                                'P': pids[ind].p,
                                'I': pids[ind].i,
                                'D': pids[ind].d,
                                'C': cmds[ind],
                                'T': target_pos[ind],
                                'N': x,
                                'R': v,
                                'TS': ts
                            }
                            pub_sock.send_multipart([
                                zmq_topics.topic_pos_hold_pid_fmt % ind,
                                pickle.dumps(debug_pid, -1)
                            ])

                thruster_cmd = mixer.mix(cmds[2], cmds[1], cmds[0], 0, 0, 0, 0,
                                         0)
                thrusters_source.send_pyobj(['pos', time.time(), thruster_cmd])

            if topic == zmq_topics.topic_axes:
                jm.update_axis(data)

            if topic == zmq_topics.topic_button:
                jm.update_buttons(data)
                #target_depth+=data[jm.ud]

            if topic == zmq_topics.topic_system_state:
                _, system_state = data

        await asyncio.sleep(0.001)