class K9dApp: #Compile RE for iwconfig ouput parsing brre = re.compile(r"Bit Rate=([\d.]+ \S+)", re.M) lqre = re.compile(r"Link Quality=(\d+/\d+) Signal level=(-\d+ dBm)", re.M) def __init__(self): #Kind a config here #arduino tracks bridge pinout self.tracks = Tracks(pin.port.PB5, pin.port.PB6, 28, pin.port.PB7, pin.port.PB10, 29, 20) #ip self.addr = "192.168.0.22" self.UDP_IP = "0.0.0.0" self.UDP_PORT = 9999 #sonar self.sonaron = True self.sonarfailsafe = 0. #mjpg_streamer related self.videomode = 'OFF' #init self.faststate = {} self.faststate["proxalert"] = False self.sonardist = 0 self.sonarfailsafe_active = False self.festivalup = False self.log_init() #==============UDP INIT==================== self.log.info("spinal interface daemon for K-9 starting") self.servsock = socket.socket( socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP self.servsock.bind((self.UDP_IP, self.UDP_PORT)) self.respsock = socket.socket( socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP #============SERIAL INIT=================== try: self.spinal = serial.Serial('/dev/ttyS2', 115200, timeout=0.5) self.spinal_enabled = True except: self.spinal_enabled = False self.log.info( "Could not open port for arduino -- connection disabled ") try: self.sonar = serial.Serial('/dev/ttyS1', 9600, timeout=0.5) self.sonar_enabled = True except: self.log.error("Could not open port for sonar -- sonar disabled") self.sonar_enabled = False #===========THREADING INIT================= self.faststate['S1'] = -1 self.faststate['S2'] = -1 self.faststate['A1'] = -1 self.faststate['A2'] = -1 self.faststate['A3'] = -1 if self.spinal_enabled: self.tSerialRead = threading.Thread(target=self.sreader) self.tSerialRead.setDaemon(1) self.tSerialRead.start() self.tTrackWatchdog = threading.Thread(target=self.cmdtimeout) self.tTrackWatchdog.setDaemon(1) self.tTrackWatchdog.start() self.tStateUpload = threading.Thread(target=self.tstateup) self.tStateUpload.setDaemon(1) self.tStateUpload.start() if self.sonar_enabled: self.tSonar = threading.Thread(target=self.tsonar) self.tSonar.setDaemon(1) self.tSonar.start() self.log.info("ready to play") def __del__(self): self.log.info("Shutting down daemon") self.gsens.__del__() self.gsens = None def gsens_update(self): if hasattr(self.gsens, 'yaw'): self.faststate['yaw'] = self.gsens.yaw self.faststate['pitch'] = self.gsens.pitch self.faststate['roll'] = self.gsens.roll self.faststate['ax'] = self.gsens.ax self.faststate['ay'] = self.gsens.ay self.faststate['az'] = self.gsens.az self.stateupload() def log_init(self): if hasattr(self, 'log'): self.log.removeHandler(self.stdoutloghandler) self.log.removeHandler(self.udploghandler) else: self.log = logging.getLogger('k9d') self.stdoutloghandler = logging.StreamHandler(sys.stdout) self.stdoutloghandler.setFormatter( logging.Formatter( "%(asctime)s [%(name)s#%(levelname)s]: %(message)s")) self.stdoutloghandler.setLevel(logging.DEBUG) self.udploghandler = logging.handlers.DatagramHandler( self.addr, self.UDP_PORT) self.udploghandler.setFormatter( logging.Formatter( "%(asctime)s [%(name)s#%(levelname)s]: %(message)s")) self.udploghandler.setLevel(logging.DEBUG) self.log.setLevel(logging.DEBUG) self.log.addHandler(self.stdoutloghandler) self.log.addHandler(self.udploghandler) def mainloop(self): while True: data, (addr, self.port) = self.servsock.recvfrom( 1024) # buffer size is 1024 bytes if addr != self.addr: self.log.warn("New client ip: {}".format(addr)) self.addr = addr self.log_init() out = "{}>{} &".format(str(self.addr), data) request = data.split(' ') CMD = request.pop(0) CMD = CMD.strip("\n") if CMD == "STOP": self.tracks.neutral() self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() self.log.debug("Tracks vector set to neutral") elif CMD == "SPINAL": emsg = ' '.join(request) self.log.info("Sending user packet to spinal: {}".format(emsg)) self.spinal_write(emsg) elif CMD == "PING": self.log.info("PING {} got".format(request[0])) self.send("PONG {} {}".format(time.time(), request[0]), self.addr) elif CMD == "TRACKS": self.tracks.vector(request[0], request[1]) self.log.debug("Tracks vector set to {},{}".format( request[0], request[1])) self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() elif CMD == "SERVO": self.servo(request[0], request[1]) elif CMD == "SON": if request[0] == 'Failsafe': self.sonaron = True if request[1] == 'off': self.sonarfailsafe = 0 else: self.sonarfailsafe = float(request[1]) elif request[0] == 'Sonar' and request[1] == 'off': self.sonaron = False self.sonarfailsafe = 0 elif CMD == "CAM": if request[0] == 'RES': if hasattr(self, 'pmjpg'): if self.pmjpg.poll() == None: pid = self.pmjpg.pid self.pmjpg.kill() self.videomode = "OFF" t = threading.Timer(2.0, self.run_mjpg, [request[1]]) t.start() self.log.info( "Killed mjpg with pid {} and deferred run with {}" .format(pid, request[1])) else: self.run_mjpg(request[1]) else: self.run_mjpg(request[1]) elif request[0] == 'OFF': if hasattr(self, 'pmjpg') and self.pmjpg.poll() == None: self.pmjpg.kill() self.videomode = "OFF" self.log.info("Killed mjpg.") elif request[0] == 'ZOOM' and request[1] == 'OFF': status = subprocess.call( ["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=1"]) self.log.info("Zoom OFF") elif request[0] == 'ZOOM' and request[1] == 'ON': status = subprocess.call( ["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=5"]) self.log.info("Zoom ON") elif CMD == "FESTIVAL": if request[0] == 'ON': if hasattr(self, 'pfestival'): if self.pfestival.poll() == None: pid = self.pfestival.pid self.pfestival.kill() self.festivalup = False t = threading.Timer(2.0, self.run_festival) t.start() self.log.info( "Killed festival with pid {} and deferred run". format(pid)) else: self.run_festival() else: self.run_festival() elif request[0] == 'OFF': if hasattr(self, 'pfestival') and self.pfestival.poll() == None: self.pfestival.kill() self.festivalup = False self.log.info("Killed festival") elif CMD == "SAY": tts = cPickle.loads(data[4:]) if self.festivalup: self.pfestival.stdin.write("(SayText \"{}\")".format( tts.encode("utf-8"))) self.pfestival.stdin.flush() self.log.info("Pronouncing '{}'".format( tts.encode("utf-8"))) else: self.log.err("Can't SAY, festival down") elif CMD == 'EVAL': ecmd = " ".join(request) try: output = eval("pprint.pformat({})".format(ecmd)) self.log.info("{} = {}".format(ecmd, output)) except Exception, e: self.log.error( "eval \"{}\" raised {} Exception: {}".format( ecmd, type(e).__name__, e)) elif CMD == "GSENSOR": if request[0] == 'ON': self.gsens = GSens(0, self.gsens_update, self.log.error, self.log.info, self.log.debug) elif request[0] == 'OFF': self.gsens.__del__() self.gsens = None else:
class K9dApp: #Compile RE for iwconfig ouput parsing brre = re.compile (r"Bit Rate=([\d.]+ \S+)",re.M) lqre = re.compile (r"Link Quality=(\d+/\d+) Signal level=(-\d+ dBm)",re.M) def __init__ (self): #Kind a config here #arduino tracks bridge pinout self.tracks = Tracks (pin.port.PB5, pin.port.PB6, 28, pin.port.PB7, pin.port.PB10, 29, 20) #ip self.addr = "192.168.0.22" self.UDP_IP = "0.0.0.0" self.UDP_PORT = 9999 #sonar self.sonaron = True self.sonarfailsafe = 0. #mjpg_streamer related self.videomode = 'OFF' #init self.faststate = {} self.faststate["proxalert"] = False self.sonardist = 0 self.sonarfailsafe_active = False self.festivalup = False self.log_init() #==============UDP INIT==================== self.log.info ("spinal interface daemon for K-9 starting"); self.servsock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP self.servsock.bind((self.UDP_IP, self.UDP_PORT)) self.respsock = socket.socket(socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP #============SERIAL INIT=================== try: self.spinal = serial.Serial('/dev/ttyS2', 115200, timeout=0.5) self.spinal_enabled = True except: self.spinal_enabled = False self.log.info("Could not open port for arduino -- connection disabled ") try: self.sonar = serial.Serial('/dev/ttyS1', 9600, timeout=0.5) self.sonar_enabled = True except: self.log.error("Could not open port for sonar -- sonar disabled") self.sonar_enabled = False #===========THREADING INIT================= self.faststate['S1']=-1 self.faststate['S2']=-1 self.faststate['A1']=-1 self.faststate['A2']=-1 self.faststate['A3']=-1 if self.spinal_enabled: self.tSerialRead = threading.Thread(target=self.sreader) self.tSerialRead.setDaemon (1) self.tSerialRead.start() self.tTrackWatchdog = threading.Thread(target=self.cmdtimeout) self.tTrackWatchdog.setDaemon (1) self.tTrackWatchdog.start() self.tStateUpload = threading.Thread(target=self.tstateup) self.tStateUpload.setDaemon (1) self.tStateUpload.start() if self.sonar_enabled: self.tSonar = threading.Thread(target=self.tsonar) self.tSonar.setDaemon (1) self.tSonar.start() self.log.info ("ready to play"); def __del__ (self): self.log.info("Shutting down daemon") self.gsens.__del__() self.gsens = None def gsens_update (self): if hasattr(self.gsens,'yaw'): self.faststate['yaw'] = self.gsens.yaw self.faststate['pitch'] = self.gsens.pitch self.faststate['roll'] = self.gsens.roll self.faststate['ax'] = self.gsens.ax self.faststate['ay'] = self.gsens.ay self.faststate['az'] = self.gsens.az self.stateupload() def log_init(self): if hasattr(self, 'log'): self.log.removeHandler(self.stdoutloghandler) self.log.removeHandler(self.udploghandler) else: self.log = logging.getLogger('k9d') self.stdoutloghandler = logging.StreamHandler(sys.stdout) self.stdoutloghandler.setFormatter(logging.Formatter("%(asctime)s [%(name)s#%(levelname)s]: %(message)s")) self.stdoutloghandler.setLevel(logging.DEBUG) self.udploghandler = logging.handlers.DatagramHandler(self.addr, self.UDP_PORT) self.udploghandler.setFormatter(logging.Formatter("%(asctime)s [%(name)s#%(levelname)s]: %(message)s")) self.udploghandler.setLevel(logging.DEBUG) self.log.setLevel(logging.DEBUG) self.log.addHandler(self.stdoutloghandler) self.log.addHandler(self.udploghandler) def mainloop (self): while True: data,(addr,self.port) = self.servsock.recvfrom(1024) # buffer size is 1024 bytes if addr != self.addr: self.log.warn ("New client ip: {}".format(addr)) self.addr = addr self.log_init() out = "{}>{} &".format(str(self.addr),data) request = data.split(' '); CMD = request.pop(0) CMD = CMD.strip("\n") if CMD == "STOP": self.tracks.neutral() self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() self.log.debug ("Tracks vector set to neutral") elif CMD == "SPINAL": emsg = ' '.join(request) self.log.info ("Sending user packet to spinal: {}".format(emsg)) self.spinal_write (emsg) elif CMD == "PING": self.log.info ("PING {} got".format(request[0])) self.send ("PONG {} {}".format(time.time(),request[0]),self.addr) elif CMD == "TRACKS": self.tracks.vector (request[0],request[1]); self.log.debug ("Tracks vector set to {},{}".format(request[0],request[1])) self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() elif CMD == "SERVO": self.servo (request[0],request[1]); elif CMD == "SON": if request[0] == 'Failsafe': self.sonaron = True if request[1] == 'off': self.sonarfailsafe = 0 else: self.sonarfailsafe = float(request[1]) elif request[0] == 'Sonar' and request[1] == 'off' : self.sonaron = False self.sonarfailsafe = 0 elif CMD == "CAM": if request[0] == 'RES': if hasattr(self, 'pmjpg'): if self.pmjpg.poll() == None: pid = self.pmjpg.pid self.pmjpg.kill() self.videomode = "OFF" t = threading.Timer(2.0,self.run_mjpg,[request[1]]) t.start() self.log.info ("Killed mjpg with pid {} and deferred run with {}".format(pid,request[1])) else: self.run_mjpg(request[1]) else: self.run_mjpg(request[1]) elif request[0] == 'OFF': if hasattr(self, 'pmjpg') and self.pmjpg.poll() == None: self.pmjpg.kill() self.videomode = "OFF" self.log.info ("Killed mjpg.") elif request[0] == 'ZOOM' and request[1] == 'OFF' : status = subprocess.call(["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=1"]) self.log.info ("Zoom OFF") elif request[0] == 'ZOOM' and request[1] == 'ON' : status = subprocess.call(["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=5"]) self.log.info ("Zoom ON") elif CMD == "FESTIVAL": if request[0] == 'ON': if hasattr(self, 'pfestival'): if self.pfestival.poll() == None: pid = self.pfestival.pid self.pfestival.kill() self.festivalup = False t = threading.Timer(2.0,self.run_festival) t.start() self.log.info ("Killed festival with pid {} and deferred run".format(pid)) else: self.run_festival() else: self.run_festival() elif request[0] == 'OFF': if hasattr(self, 'pfestival') and self.pfestival.poll() == None: self.pfestival.kill() self.festivalup = False self.log.info ("Killed festival") elif CMD == "SAY": tts = cPickle.loads(data[4:]) if self.festivalup: self.pfestival.stdin.write ("(SayText \"{}\")".format( tts.encode("utf-8"))) self.pfestival.stdin.flush() self.log.info ("Pronouncing '{}'".format(tts.encode("utf-8"))) else: self.log.err ("Can't SAY, festival down") elif CMD == 'EVAL': ecmd = " ".join(request) try: output = eval("pprint.pformat({})".format(ecmd)) self.log.info ("{} = {}".format(ecmd, output)) except Exception, e: self.log.error("eval \"{}\" raised {} Exception: {}".format(ecmd,type(e).__name__ ,e)) elif CMD == "GSENSOR": if request[0] == 'ON': self.gsens = GSens (0, self.gsens_update, self.log.error, self.log.info, self.log.debug) elif request[0] == 'OFF': self.gsens.__del__() self.gsens = None else:
def mainloop(self): while True: data, (addr, self.port) = self.servsock.recvfrom( 1024) # buffer size is 1024 bytes if addr != self.addr: self.log.warn("New client ip: {}".format(addr)) self.addr = addr self.log_init() out = "{}>{} &".format(str(self.addr), data) request = data.split(' ') CMD = request.pop(0) CMD = CMD.strip("\n") if CMD == "STOP": self.tracks.neutral() self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() self.log.debug("Tracks vector set to neutral") elif CMD == "SPINAL": emsg = ' '.join(request) self.log.info("Sending user packet to spinal: {}".format(emsg)) self.spinal_write(emsg) elif CMD == "PING": self.log.info("PING {} got".format(request[0])) self.send("PONG {} {}".format(time.time(), request[0]), self.addr) elif CMD == "TRACKS": self.tracks.vector(request[0], request[1]) self.log.debug("Tracks vector set to {},{}".format( request[0], request[1])) self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() elif CMD == "SERVO": self.servo(request[0], request[1]) elif CMD == "SON": if request[0] == 'Failsafe': self.sonaron = True if request[1] == 'off': self.sonarfailsafe = 0 else: self.sonarfailsafe = float(request[1]) elif request[0] == 'Sonar' and request[1] == 'off': self.sonaron = False self.sonarfailsafe = 0 elif CMD == "CAM": if request[0] == 'RES': if hasattr(self, 'pmjpg'): if self.pmjpg.poll() == None: pid = self.pmjpg.pid self.pmjpg.kill() self.videomode = "OFF" t = threading.Timer(2.0, self.run_mjpg, [request[1]]) t.start() self.log.info( "Killed mjpg with pid {} and deferred run with {}" .format(pid, request[1])) else: self.run_mjpg(request[1]) else: self.run_mjpg(request[1]) elif request[0] == 'OFF': if hasattr(self, 'pmjpg') and self.pmjpg.poll() == None: self.pmjpg.kill() self.videomode = "OFF" self.log.info("Killed mjpg.") elif request[0] == 'ZOOM' and request[1] == 'OFF': status = subprocess.call( ["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=1"]) self.log.info("Zoom OFF") elif request[0] == 'ZOOM' and request[1] == 'ON': status = subprocess.call( ["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=5"]) self.log.info("Zoom ON") elif CMD == "FESTIVAL": if request[0] == 'ON': if hasattr(self, 'pfestival'): if self.pfestival.poll() == None: pid = self.pfestival.pid self.pfestival.kill() self.festivalup = False t = threading.Timer(2.0, self.run_festival) t.start() self.log.info( "Killed festival with pid {} and deferred run". format(pid)) else: self.run_festival() else: self.run_festival() elif request[0] == 'OFF': if hasattr(self, 'pfestival') and self.pfestival.poll() == None: self.pfestival.kill() self.festivalup = False self.log.info("Killed festival") elif CMD == "SAY": tts = cPickle.loads(data[4:]) if self.festivalup: self.pfestival.stdin.write("(SayText \"{}\")".format( tts.encode("utf-8"))) self.pfestival.stdin.flush() self.log.info("Pronouncing '{}'".format( tts.encode("utf-8"))) else: self.log.err("Can't SAY, festival down") elif CMD == 'EVAL': ecmd = " ".join(request) try: output = eval("pprint.pformat({})".format(ecmd)) self.log.info("{} = {}".format(ecmd, output)) except Exception, e: self.log.error( "eval \"{}\" raised {} Exception: {}".format( ecmd, type(e).__name__, e)) elif CMD == "GSENSOR": if request[0] == 'ON': self.gsens = GSens(0, self.gsens_update, self.log.error, self.log.info, self.log.debug) elif request[0] == 'OFF': self.gsens.__del__() self.gsens = None
def mainloop (self): while True: data,(addr,self.port) = self.servsock.recvfrom(1024) # buffer size is 1024 bytes if addr != self.addr: self.log.warn ("New client ip: {}".format(addr)) self.addr = addr self.log_init() out = "{}>{} &".format(str(self.addr),data) request = data.split(' '); CMD = request.pop(0) CMD = CMD.strip("\n") if CMD == "STOP": self.tracks.neutral() self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() self.log.debug ("Tracks vector set to neutral") elif CMD == "SPINAL": emsg = ' '.join(request) self.log.info ("Sending user packet to spinal: {}".format(emsg)) self.spinal_write (emsg) elif CMD == "PING": self.log.info ("PING {} got".format(request[0])) self.send ("PONG {} {}".format(time.time(),request[0]),self.addr) elif CMD == "TRACKS": self.tracks.vector (request[0],request[1]); self.log.debug ("Tracks vector set to {},{}".format(request[0],request[1])) self.faststate['trackr'] = self.tracks.trackr self.faststate['trackl'] = self.tracks.trackl self.stateupload() elif CMD == "SERVO": self.servo (request[0],request[1]); elif CMD == "SON": if request[0] == 'Failsafe': self.sonaron = True if request[1] == 'off': self.sonarfailsafe = 0 else: self.sonarfailsafe = float(request[1]) elif request[0] == 'Sonar' and request[1] == 'off' : self.sonaron = False self.sonarfailsafe = 0 elif CMD == "CAM": if request[0] == 'RES': if hasattr(self, 'pmjpg'): if self.pmjpg.poll() == None: pid = self.pmjpg.pid self.pmjpg.kill() self.videomode = "OFF" t = threading.Timer(2.0,self.run_mjpg,[request[1]]) t.start() self.log.info ("Killed mjpg with pid {} and deferred run with {}".format(pid,request[1])) else: self.run_mjpg(request[1]) else: self.run_mjpg(request[1]) elif request[0] == 'OFF': if hasattr(self, 'pmjpg') and self.pmjpg.poll() == None: self.pmjpg.kill() self.videomode = "OFF" self.log.info ("Killed mjpg.") elif request[0] == 'ZOOM' and request[1] == 'OFF' : status = subprocess.call(["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=1"]) self.log.info ("Zoom OFF") elif request[0] == 'ZOOM' and request[1] == 'ON' : status = subprocess.call(["/usr/bin/v4l2-ctl", "--set-ctrl=zoom_absolute=5"]) self.log.info ("Zoom ON") elif CMD == "FESTIVAL": if request[0] == 'ON': if hasattr(self, 'pfestival'): if self.pfestival.poll() == None: pid = self.pfestival.pid self.pfestival.kill() self.festivalup = False t = threading.Timer(2.0,self.run_festival) t.start() self.log.info ("Killed festival with pid {} and deferred run".format(pid)) else: self.run_festival() else: self.run_festival() elif request[0] == 'OFF': if hasattr(self, 'pfestival') and self.pfestival.poll() == None: self.pfestival.kill() self.festivalup = False self.log.info ("Killed festival") elif CMD == "SAY": tts = cPickle.loads(data[4:]) if self.festivalup: self.pfestival.stdin.write ("(SayText \"{}\")".format( tts.encode("utf-8"))) self.pfestival.stdin.flush() self.log.info ("Pronouncing '{}'".format(tts.encode("utf-8"))) else: self.log.err ("Can't SAY, festival down") elif CMD == 'EVAL': ecmd = " ".join(request) try: output = eval("pprint.pformat({})".format(ecmd)) self.log.info ("{} = {}".format(ecmd, output)) except Exception, e: self.log.error("eval \"{}\" raised {} Exception: {}".format(ecmd,type(e).__name__ ,e)) elif CMD == "GSENSOR": if request[0] == 'ON': self.gsens = GSens (0, self.gsens_update, self.log.error, self.log.info, self.log.debug) elif request[0] == 'OFF': self.gsens.__del__() self.gsens = None