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'),
'solo' : (self.cmd_solo, 'send commands to the 3DR Solo (if connected)')
}
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_solo(self, args):
solos = self.modules.get_modules('solo')
if len(solos) > 0:
solos[0].cmd_solo(args)
else:
print "3DR Solo Module not loaded."
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()
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'),
'solo': (self.cmd_solo,
'send commands to the 3DR Solo (if connected)')
}
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_solo(self, args):
solos = self.modules.get_modules('solo')
if len(solos) > 0:
solos[0].cmd_solo(args)
else:
print "3DR Solo Module not loaded."
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()
