listener.py 1.76 KB
Newer Older
Nicholas Shindler's avatar
Nicholas Shindler committed
1 2 3 4 5 6 7 8 9 10 11 12
import socket
import select
import struct
import code
import threading
import Queue
import time
import os
import sys
import subprocess
import rospy

13
class listen(threading.Thread):
14 15
    # listener class for getting data from robot
    def __init__(self, q, client):
16 17
        threading.Thread.__init__(self)
        self.q = q
Nicholas Shindler's avatar
Nicholas Shindler committed
18
        self.client = client
19
        self.callback = None
20
        self.setDaemon(True)
21

Nicholas Shindler's avatar
Nicholas Shindler committed
22
    def addMsgCallback(self, callbackFcn):
23 24
        # adds a function that is called whenever a message is recived by the queue
        self.callback = callbackFcn
25 26 27

    def run(self):
        # runs a listener thread, reading in data and placing it in a queue
28 29 30 31
        try:
            while True:
                # Wait for incoming data from the RCP or for the listener
                # pipe to close, indicating that we are
32 33 34
                (r, w, x) = select.select(
                    (self.client.s, self.client.listener_pipe[0]), (), ()
                )
35 36 37 38 39 40 41 42 43 44 45
                if self.client.listener_pipe[0] in r:
                    # listener pipe signaled the thread to exit
                    os.close(self.client.listener_pipe[0])
                    return
                if self.client.s in r:
                    (t, l, v) = self.client.receive_next_message()
                    if not self.client.do_async_message(t, l, v):
                        self.q.put((t, l, v))
                        try:
                            if self.callback == None:
                                raise NameError("No Callback has been defined")
46
                            self.callback(t, v)
47 48 49 50
                        except NameError:
                            pass
                    # time.sleep(0.1)
        except Exception as e:
Nicholas Shindler's avatar
Nicholas Shindler committed
51
            rospy.logwarn(e)
52
            raise e