class OdroidPerson: def __init__(self, config, ident): self.ident = ident self.dying = False self.config = config self.send_id = 0 print "ODRIOD launching as '%s'" % ident self.modules = ModuleHandler(self, 'odroidperson') self.bind_ip = self.config.get_my("my-ip") self.server_id = self.config.get_network("server") self.server_ip = self.config.get_foreign(self.server_id, "my-ip") self.cc_local_port = self.config.get_my("cc-local-port") self.cc_server_port = self.config.get_my("cc-server-port") self.sbp_server_port = self.config.get_my("sbp-server-port") self.sbp_bind_port = self.config.get_my("sbp-bind-port") self.mav_server_port = self.config.get_my("mav-server-port") logger.info("Launching with Config:") logger.info(self.config) self.init_death() def init_death(self): self.last_death_attempt = 0 '''Setup Graceful Death''' def quit_handler(signum=None, frame=None): #print 'Signal handler called with signal', signum if time.time() - self.last_death_attempt < 4.0: print "Still within 4s of last death attempt. CHILL!" return self.last_death_attempt = time.time() if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self): logger.info("Shutting down!") self.dying = True self.modules.unload_all_modules() def send_cc(self, msgtype, payload=None): try: msg = {'msgtype': msgtype} if payload is not None: msg['payload'] = payload msg['__ID__'] = self.send_id self.send_id += 1 print "sending message %s to %s, %s" % (msgtype, self.server_ip, self.cc_server_port) self.cc_udp.sendto(json.dumps(msg), (self.server_ip, self.cc_server_port)) except socket.error as e: if e.errno == 65: print "SEND_CC: No route to %s:%d" % (self.server_ip, self.cc_server_port) else: traceback.print_exc() def handle_cc(self, cc_data, cc_addr): msg = json.loads(cc_data) msg_handler = { 'ACK': self.cc_ack, 'NACK': self.cc_nack, 'malformed': self.cc_unrecognized, 'unsupported': self.cc_unsupported, 'simulator': self.cc_simulator, 'reset_piksi': self.cc_reset_piksi, 'shutdown': self.cc_shutdown, 'restart': self.cc_restart, 'update': self.cc_update, } if not 'msgtype' in msg: self.send_cc('malformed', {'msg': 'message contains to \'msgtype\' field.'}) return if not msg['msgtype'] in msg_handler: self.send_cc( 'unsupported', {'msg': 'message type \'%s\' not supported.' % msg['msgtype']}) return #print "Handling message type %s: %s" % (msg['msgtype'], str(msg)) success = msg_handler[msg['msgtype']](msg) if not ('ACK' in msg['msgtype'] or 'NACK' in msg['msgtype']): if success: self.send_cc('ACK', {'__ACK_ID__': msg['__ID__']}) else: self.send_cc('NACK', {'__ACK_ID__': msg['__ID__']}) def cc_ack(self, msg): print "ACK RECEIVED for %s" % msg['payload'] def cc_nack(self, msg): print "NACK RECEIVED for %s" % msg['payload'] def cc_shutdown(self, msg): os.system("shutdown now -h") return True def cc_restart(self, msg): os.system("reboot") return True def cc_simulator(self, msg): try: payload = msg['payload'] if payload == 'True': print 'ENABLING PIKSI' self.modules.trigger('enable_piksi_sim') else: print 'DISABLING PIKSI' self.modules.trigger('disable_piksi_sim') pass return True except: traceback.print_exc() return False def cc_reset_piksi(self, msg): try: self.modules.trigger('reset_piksi') return True except: traceback.print_exc() return False def cc_update(self, msg): print "Updating now..." os.system("git pull") self.send_cc('restarting') os.system("systemctl restart spooky.service") def cc_unrecognized(self, msg): pass def cc_unsupported(self, msg): pass def send_heartbeat(self): self.send_cc('heartbeat', payload={ 'git-describe': spooky.get_version(), 'UID': os.getuid() }) def mainloop(self): if self.config.get_my("be-the-basestation"): print "I AM THE BASE STATION" self.modules.load_module('SBPUDPBroadcast') else: print "I AM NOT THE BASE STATION" piksi = self.modules.load_module('piksihandler') bcastmodule = self.modules.load_module('sbpbroadcastlistener') bcastmodule.set_data_callback(piksi.send_to_piksi) pixhawk = self.modules.load_module('pixhawkhandler', waitTimeout=10.0) try: # Here we do UDP command and control with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as cc_udp: cc_udp.setblocking(1) cc_udp.settimeout(0.1) # run this at 10hz cc_udp.bind((self.bind_ip, self.cc_local_port)) self.cc_udp = cc_udp print "CC bound to %s : %d" % (self.bind_ip, self.cc_local_port) heartbeat = spooky.DoPeriodically( lambda: self.send_heartbeat(), 1.0) while not self.dying: try: # For command and control, we're going to use JSON over UDP # UDP *already* has a simple checksum and delivers a complete packet at a time. # It also returns exactly one datagram per recv() call, so it's all good! # Our only requirement is, a cc message consists of at the very least: # {'msgtype': 'something', 'payload': {}} heartbeat.tick() cc_data, cc_addr = cc_udp.recvfrom(4096) self.handle_cc(cc_data, cc_addr) except (socket.error, socket.timeout) as e: pass except KeyboardInterrupt: self.stop()
class OdroidPerson(CommandLineHandlerShim): def __init__(self, config, ident): self.ident = ident self.dying = False self.config = config self.send_id = 0 print "ODRIOD launching as '%s'" % ident self.modules = ModuleHandler(self, 'odroidperson') self.bind_ip = self.config.get_my("my-ip") self.server_id = self.config.get_network("server") self.server_ip = self.config.get_foreign(self.server_id, "my-ip") self.cc_local_port = self.config.get_my("cc-local-port") self.cc_server_port = self.config.get_my("cc-server-port") self.sbp_server_port = self.config.get_my("sbp-server-port") self.sbp_bind_port = self.config.get_my("sbp-bind-port") self.mav_server_port = self.config.get_my("mav-server-port") logger.info("Launching with Config:") logger.info(self.config) self.init_death() def init_death(self): self.last_death_attempt = 0 '''Setup Graceful Death''' def quit_handler(signum = None, frame = None): #print 'Signal handler called with signal', signum if time.time() - self.last_death_attempt < 4.0: print "Still within 4s of last death attempt. CHILL!" return self.last_death_attempt = time.time() if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self): logger.info("Shutting down!") self.dying = True self.modules.unload_all_modules() def send_cc(self, msgtype, payload=None): try: msg = {'msgtype':msgtype} if payload is not None: msg['payload'] = payload msg['__ID__'] = self.send_id self.send_id += 1 print "sending message %s to %s, %s" % (msgtype, self.server_ip, self.cc_server_port) self.cc_udp.sendto(json.dumps(msg), (self.server_ip, self.cc_server_port)) except socket.error as e: if e.errno == 65: print "SEND_CC: No route to %s:%d" % (self.server_ip, self.cc_server_port) else: traceback.print_exc() traceback.print_stack() def handle_cc(self, cc_data, cc_addr): msg = json.loads(cc_data) msg_handler = { 'ACK': self.cc_ack, 'NACK': self.cc_nack, 'malformed': self.cc_unrecognized, 'unsupported': self.cc_unsupported, 'simulator': self.cc_simulator, 'reset_piksi': self.cc_reset_piksi, 'shutdown': self.cc_shutdown, 'restart': self.cc_restart, 'update': self.cc_update, } if not 'msgtype' in msg: self.send_cc('malformed', {'msg': 'message contains to \'msgtype\' field.'}) return if not msg['msgtype'] in msg_handler: self.send_cc('unsupported', {'msg': 'message type \'%s\' not supported.' % msg['msgtype']}) return #print "Handling message type %s: %s" % (msg['msgtype'], str(msg)) success = msg_handler[msg['msgtype']](msg) if not ('ACK' in msg['msgtype'] or 'NACK' in msg['msgtype']): if success: self.send_cc('ACK', {'__ACK_ID__': msg['__ID__']}) else: self.send_cc('NACK', {'__ACK_ID__': msg['__ID__']}) def cc_ack(self, msg): print "ACK RECEIVED for %s" % msg['payload'] def cc_nack(self, msg): print "NACK RECEIVED for %s" % msg['payload'] def cc_shutdown(self, msg): os.system("shutdown now -h") return True def cc_restart(self, msg): os.system("reboot") return True def cc_simulator(self, msg): try: payload = msg['payload'] if payload == 'True': print 'ENABLING PIKSI' self.modules.trigger('enable_piksi_sim') else: print 'DISABLING PIKSI' self.modules.trigger('disable_piksi_sim') pass return True except: traceback.print_exc() return False def cc_reset_piksi(self, msg): try: self.modules.trigger('reset_piksi') return True except: traceback.print_exc() return False def cc_update(self, msg): print "Updating now..." os.system("git pull") self.send_cc('restarting') os.system("systemctl restart spooky.service") def cc_unrecognized(self, msg): pass def cc_unsupported(self, msg): pass def send_heartbeat(self): payload = {'git-describe': spooky.get_version(), 'UID': os.getuid()} if self.modules.has_module('SBPUDPBroadcast'): bcast = self.modules.get_modules('SBPUDPBroadcast')[0] payload['base-survey-status'] = bcast.get_surveying_status() self.send_cc('heartbeat', payload=payload) def mainloop(self): if self.config.get_my("be-the-basestation"): print "I AM THE BASE STATION" self.modules.load_module('SBPUDPBroadcast') else: print "I AM NOT THE BASE STATION" piksi = self.modules.load_module('piksihandler') bcastmodule = self.modules.load_module('sbpbroadcastlistener') bcastmodule.set_data_callback(piksi.send_to_piksi) pixhawk = self.modules.load_module('pixhawkhandler', waitTimeout=10.0) try: # Here we do UDP command and control with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as cc_udp: cc_udp.setblocking(1) cc_udp.settimeout(0.1) # run this at 10hz cc_udp.bind((self.bind_ip, self.cc_local_port)) self.cc_udp = cc_udp print "CC bound to %s : %d" % (self.bind_ip, self.cc_local_port) heartbeat = spooky.DoPeriodically(lambda: self.send_heartbeat(), 1.0) while not self.dying: try: # For command and control, we're going to use JSON over UDP # UDP *already* has a simple checksum and delivers a complete packet at a time. # It also returns exactly one datagram per recv() call, so it's all good! # Our only requirement is, a cc message consists of at the very least: # {'msgtype': 'something', 'payload': {}} heartbeat.tick() cc_data, cc_addr = cc_udp.recvfrom(4096) self.handle_cc(cc_data, cc_addr) except (socket.error, socket.timeout) as e: pass except KeyboardInterrupt: self.stop()
class GroundStation(CommandLineHandler): def __init__(self, config, ident): print "GROUNDSTATION launching as '%s'" % ident self.ident = ident self.config = config self.modules = ModuleHandler(self, 'groundstation') self.dying = False self.init_death() self.command_map = { 'exit' : (self.cmd_stop, 'exit gracefully'), 'status' : (self.cmd_status, 'show status'), 'module' : (self.modules.cmd_module, 'manage modules'), 'config' : (self.config.cmd_config, 'manage configuration'), 'reinit' : (self.cmd_reinit, 'reconfigures network from config'), 'trigger' : (self.cmd_trigger, 'triggers a message across modules'), 'psim' : (self.cmd_piksisim, 'toggles the piksi simulator on connected piksis'), 'preset' : (self.cmd_piksireset, 'sends a reset message to a connected piksi (optionally specify an IP)'), 'shutdown' : (self.cmd_shutdown, '(IP) shuts down a single or all nodes in network'), 'restart' : (self.cmd_restart, '(IP) restart a single or all nodes in network (specify an IP to restart a specific node)'), 'update' : (self.cmd_update, '(IP) does a git pull and restart on a single or all nodes in network'), 'record' : (self.cmd_record, 'start or stop recording data for a session') } CommandLineHandler.__init__(self, self.command_map) def init_death(self): '''Setup Graceful Death''' def quit_handler(signum = None, frame = None): #print 'Signal handler called with signal', signum if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.dying = True self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self, hard=False): print "" print "Shutting down" self.dying = True self.modules.unload_all_modules() if hard: sys.exit(1) def cmd_trigger(self, args): if len(args) != 1: print "we need one and only one argument" return self.modules.trigger(args[0]) def cmd_stop(self, args): self.stop() def cmd_record(self, args): if len(args) > 0: if 'start' in args: return self.modules.trigger('cmd_record_start') elif 'stop' in args: return self.modules.trigger('cmd_record_stop') elif 'next' in args: return self.modules.trigger('cmd_record_next') print 'USAGE: record (start|stop|next)' def cmd_status(self, args): self.modules.trigger("cmd_status") def cmd_reinit(self, args): if len(args): if "--force" in args: self.modules.unload_all_modules() return self.configure_network_from_config() def cmd_piksisim(self, args): if len(args) > 0: if "t" in args: self.modules.trigger("enable_piksi_sim") else: self.modules.trigger("disable_piksi_sim") else: self.modules.trigger("enable_piksi_sim") print "enabling. use 't' for true and 'f' for false to toggle state." def cmd_piksireset(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "reset_piksi") else: self.modules.trigger("reset_piksi") def cmd_shutdown(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_shutdown") else: self.modules.trigger("cmd_shutdown") def cmd_restart(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_restart") else: self.modules.trigger("cmd_restart") def cmd_update(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_update") else: self.modules.trigger("cmd_update") def configure_network_from_config(self): ''' Will attempt to instantiate everything on our end. Won't reinstantiate running threads, won't interact with remote processes ''' self.modules.load_module('systemstate') if self.config.get_my("be-the-basestation"): print "I AM THE BASE STATION" self.modules.load_module('SBPUDPBroadcast') else: print "I AM NOT THE BASE STATION" for client in self.config.get_network('odroidperson'): self.modules.load_module('odroidperson_cc', instance_name=client) self.modules.load_module('odroidperson_sbp', instance_name=client) self.modules.load_module('odroidperson_mav', instance_name=client) self.modules.load_module('basestation_sbp') solo = self.modules.load_module('solo', waitTimeout=15.0) # NJ HACK SIGGRAPH 2016: USB broke off... solosbp = self.modules.load_module('solo_sbp') print "CONFIGURATON DONE! Spooky is ready for your commands:" def set_systemstate(self, module): self.systemstate = module def unset_systemstate(self): self.systemstate = None def get_systemstate(self): if self.systemstate: return self.systemstate else: return None def mainloop(self): #Set up our network! self.configure_network_from_config() # Main command line interface, ensures cleanup on exit while not self.dying: # Error handling on the INSIDE so we don't kill app try: self.handle_terminal_input() self.modules.check_modules_integrity() except EOFError: self.stop(hard=True) except KeyboardInterrupt: self.stop() except Exception: #CRUCIAL! This prevents death from exception traceback.print_exc()
class SoloSBP(object): def __init__(self, config, ident): print "SoloSBP launching as '%s'" % ident self.ident = ident self.config = config self.modules = ModuleHandler(self, 'solo_sbp_pump') self.dying = False self.init_death() def init_death(self): '''Setup Graceful Death''' def quit_handler(signum = None, frame = None): #print 'Signal handler called with signal', signum if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.dying = True self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self, hard=False): print "" print "Shutting down" self.dying = True self.modules.unload_all_modules() if hard: sys.exit(1) def configure_network_from_config(self): ''' Will attempt to instantiate everything on our end. Won't reinstantiate running threads, won't interact with remote processes ''' bcast_listener = self.modules.load_module('sbpbroadcastlistener') solo = self.modules.load_module('solo_sbp_pump', waitTimeout=15.0) if solo: bcast_listener.set_data_callback(solo.injectGPS) print "CONFIGURATON DONE! SoloSBP is ready" else: print "FAILED TO GET SOLO" def mainloop(self): #Set up our network! self.configure_network_from_config() while not self.dying: try: time.sleep(1) except KeyboardInterrupt: self.stop() except Exception: #CRUCIAL! This prevents death from exception traceback.print_exc()
class SoloSBP(object): def __init__(self, config, ident): print "SoloSBP launching as '%s'" % ident self.ident = ident self.config = config self.modules = ModuleHandler(self, 'solo_sbp_pump') self.dying = False self.init_death() def init_death(self): '''Setup Graceful Death''' def quit_handler(signum=None, frame=None): #print 'Signal handler called with signal', signum if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.dying = True self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self, hard=False): print "" print "Shutting down" self.dying = True self.modules.unload_all_modules() if hard: sys.exit(1) def configure_network_from_config(self): ''' Will attempt to instantiate everything on our end. Won't reinstantiate running threads, won't interact with remote processes ''' bcast_listener = self.modules.load_module('sbpbroadcastlistener') solo = self.modules.load_module('solo_sbp_pump', waitTimeout=15.0) if solo: bcast_listener.set_data_callback(solo.injectGPS) print "CONFIGURATON DONE! SoloSBP is ready" else: print "FAILED TO GET SOLO" def mainloop(self): #Set up our network! self.configure_network_from_config() while not self.dying: try: time.sleep(1) except KeyboardInterrupt: self.stop() except Exception: #CRUCIAL! This prevents death from exception traceback.print_exc()
class GroundStation(CommandLineHandler): def __init__(self, config, ident): print "GROUNDSTATION launching as '%s'" % ident self.ident = ident self.config = config self.modules = ModuleHandler(self, 'groundstation') self.dying = False self.init_death() self.command_map = { 'exit': (self.cmd_stop, 'exit gracefully'), 'status': (self.cmd_status, 'show status'), 'module': (self.modules.cmd_module, 'manage modules'), 'config': (self.config.cmd_config, 'manage configuration'), 'reinit': (self.cmd_reinit, 'reconfigures network from config'), 'trigger': (self.cmd_trigger, 'triggers a message across modules'), 'psim': (self.cmd_piksisim, 'toggles the piksi simulator on connected piksis'), 'preset': (self.cmd_piksireset, 'sends a reset message to a connected piksi (optionally specify an IP)' ), 'shutdown': (self.cmd_shutdown, '(IP) shuts down a single or all nodes in network'), 'restart': (self.cmd_restart, '(IP) restart a single or all nodes in network (specify an IP to restart a specific node)' ), 'update': (self.cmd_update, '(IP) does a git pull and restart on a single or all nodes in network' ), 'record': (self.cmd_record, 'start or stop recording data for a session') } CommandLineHandler.__init__(self, self.command_map) def init_death(self): '''Setup Graceful Death''' def quit_handler(signum=None, frame=None): #print 'Signal handler called with signal', signum if self.dying: print 'Clean shutdown impossible, forcing an exit' sys.exit(0) else: self.dying = True self.stop() # Listen for kill signals to cleanly shutdown modules fatalsignals = [signal.SIGTERM] try: fatalsignals.append(signal.SIGHUP, signal.SIGQUIT) except Exception: pass for sig in fatalsignals: signal.signal(sig, quit_handler) def stop(self, hard=False): print "" print "Shutting down" self.dying = True self.modules.unload_all_modules() if hard: sys.exit(1) def cmd_trigger(self, args): if len(args) != 1: print "we need one and only one argument" return self.modules.trigger(args[0]) def cmd_stop(self, args): self.stop() def cmd_record(self, args): if len(args) > 0: if 'start' in args: return self.modules.trigger('cmd_record_start') elif 'stop' in args: return self.modules.trigger('cmd_record_stop') elif 'next' in args: return self.modules.trigger('cmd_record_next') print 'USAGE: record (start|stop|next)' def cmd_status(self, args): self.modules.trigger("cmd_status") def cmd_reinit(self, args): if len(args): if "--force" in args: self.modules.unload_all_modules() return self.configure_network_from_config() def cmd_piksisim(self, args): if len(args) > 0: if "t" in args: self.modules.trigger("enable_piksi_sim") else: self.modules.trigger("disable_piksi_sim") else: self.modules.trigger("enable_piksi_sim") print "enabling. use 't' for true and 'f' for false to toggle state." def cmd_piksireset(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "reset_piksi") else: self.modules.trigger("reset_piksi") def cmd_shutdown(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_shutdown") else: self.modules.trigger("cmd_shutdown") def cmd_restart(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_restart") else: self.modules.trigger("cmd_restart") def cmd_update(self, args): if len(args) > 0: self.modules.trigger_on("odroidperson_cc", args[0], "cmd_update") else: self.modules.trigger("cmd_update") def configure_network_from_config(self): ''' Will attempt to instantiate everything on our end. Won't reinstantiate running threads, won't interact with remote processes ''' self.modules.load_module('systemstate') if self.config.get_my("be-the-basestation"): print "I AM THE BASE STATION" self.modules.load_module('SBPUDPBroadcast') else: print "I AM NOT THE BASE STATION" for client in self.config.get_network('odroidperson'): self.modules.load_module('odroidperson_cc', instance_name=client) self.modules.load_module('odroidperson_sbp', instance_name=client) self.modules.load_module('odroidperson_mav', instance_name=client) solo = self.modules.load_module('solo', waitTimeout=15.0) # NJ HACK SIGGRAPH 2016: USB broke off... solosbp = self.modules.load_module('solo_sbp') print "CONFIGURATON DONE! Spooky is ready for your commands:" def set_systemstate(self, module): self.systemstate = module def unset_systemstate(self): self.systemstate = None def get_systemstate(self): if self.systemstate: return self.systemstate else: return None def mainloop(self): #Set up our network! self.configure_network_from_config() # Main command line interface, ensures cleanup on exit while not self.dying: # Error handling on the INSIDE so we don't kill app try: self.handle_terminal_input() self.modules.check_modules_integrity() except EOFError: self.stop(hard=True) except KeyboardInterrupt: self.stop() except Exception: #CRUCIAL! This prevents death from exception traceback.print_exc()