def __init__(self, args): #pylint: disable=W0621
self._debug = args.debug
log_level = logging.DEBUG if args.debug else logging.INFO
self._log = logging.getLogger('app')
self._log.setLevel(log_level)
BeaconEventHandler._log.setLevel(log_level)
self._wndgeom = args.geom
self._window = pygame.display.set_mode(self._wndgeom.size)
pygame.display.set_caption('Tracking beacons demo - by POBOT')
wnd_icon = pygame.image.load(
utils.make_resource_path('pobot-logo-right.png')
).convert_alpha()
pygame.display.set_icon(wnd_icon)
self._listen_port = args.listen_port
self._ctrl_name = args.ctrl_name
self._ctrl_host = args.ctrl_host
self._ctrl_port = args.ctrl_port
self._ctrl_socket = None
self._ctrl_rfile = None
self._logo = Logo(self._wndgeom.size)
self._hud = Hud(pygame.Rect(
(self._wndgeom.w - HUD_W) / 2, 10, HUD_W, HUD_H
))
self._status = StatusLine(pygame.Rect(
0, self._wndgeom.h - 20, self._wndgeom.w, 20
))
self._help = None
lg = math.sqrt(self._wndgeom.w * self._wndgeom.w + self._wndgeom.h * self._wndgeom.h)
self._beacons = (
Beacon(
(Beacon.SIZE / 2, Beacon.SIZE / 2),
(0, 90),
'alpha',
ray_length=lg,
debug=args.debug
),
Beacon(
(self._wndgeom.w - Beacon.SIZE / 2, Beacon.SIZE / 2),
(-90, 0),
'beta',
ray_length=lg,
debug=args.debug
)
)
self._origin = self._beacons[0].location
self._target = Target()
self._clock = pygame.time.Clock()
self._beacon_listener = None
self._use_2_sensors = args.use_2_sensors
class Application(object):
""" The application global object.
"""
#pylint: disable=W0212
def __init__(self, args): #pylint: disable=W0621
self._debug = args.debug
log_level = logging.DEBUG if args.debug else logging.INFO
self._log = logging.getLogger('app')
self._log.setLevel(log_level)
BeaconEventHandler._log.setLevel(log_level)
self._wndgeom = args.geom
self._window = pygame.display.set_mode(self._wndgeom.size)
pygame.display.set_caption('Tracking beacons demo - by POBOT')
wnd_icon = pygame.image.load(
utils.make_resource_path('pobot-logo-right.png')
).convert_alpha()
pygame.display.set_icon(wnd_icon)
self._listen_port = args.listen_port
self._ctrl_name = args.ctrl_name
self._ctrl_host = args.ctrl_host
self._ctrl_port = args.ctrl_port
self._ctrl_socket = None
self._ctrl_rfile = None
self._logo = Logo(self._wndgeom.size)
self._hud = Hud(pygame.Rect(
(self._wndgeom.w - HUD_W) / 2, 10, HUD_W, HUD_H
))
self._status = StatusLine(pygame.Rect(
0, self._wndgeom.h - 20, self._wndgeom.w, 20
))
self._help = None
lg = math.sqrt(self._wndgeom.w * self._wndgeom.w + self._wndgeom.h * self._wndgeom.h)
self._beacons = (
Beacon(
(Beacon.SIZE / 2, Beacon.SIZE / 2),
(0, 90),
'alpha',
ray_length=lg,
debug=args.debug
),
Beacon(
(self._wndgeom.w - Beacon.SIZE / 2, Beacon.SIZE / 2),
(-90, 0),
'beta',
ray_length=lg,
debug=args.debug
)
)
self._origin = self._beacons[0].location
self._target = Target()
self._clock = pygame.time.Clock()
self._beacon_listener = None
self._use_2_sensors = args.use_2_sensors
def _ctl_command(self, cmde):
""" Sends a command to the beacons controller."""
self._log.info('cmd: %s' % cmde)
self._ctrl_socket.sendall(cmde + '\n')
_reply = self._ctrl_rfile.readline().strip()
self._log.info('.... %s' % _reply)
def _open_control_socket(self, host, port):
""" Opens a socket with the beacons controller."""
self._ctrl_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._ctrl_socket.connect((host, port))
self._ctrl_rfile = self._ctrl_socket.makefile(mode='rt', bufsize=1024)
def _close_control_socket(self):
""" Closes the beacon controller socket."""
if self._ctrl_socket:
self._ctrl_socket.shutdown(socket.SHUT_RDWR)
self._ctrl_socket.close()
def run(self):
""" Application main process."""
if self._beacon_listener:
return
# set up a listener for beacon events
self._log.info('starting beacons event listener')
self._beacon_listener = BeaconEventListener(
listen_port=self._listen_port,
use_2_sensors=self._use_2_sensors,
debug=self._debug)
self._beacon_listener.start()
# set up a connection with the beacons controller
self._log.info('connecting to beacons controller')
try:
self._open_control_socket(self._ctrl_host, self._ctrl_port)
except socket.error:
self._log.error('cannot connect to beacons controller')
else:
self._log.info('retrieving controller identity')
ghba = socket.gethostbyaddr(self._ctrl_host)
# use the hostname returned by the controller itself instead of gethostbyaddr()
# result one, since the later contains what is set in /etc/hosts
self._status.controller = (self._ctrl_name, ghba[1], ghba[2])
self._log.info('--> %s', self._status.controller)
# hook ourself to the controller as an event listener
self._log.info('registering as beacons event listener')
self._ctl_command('register')
try:
self._log.info('starting application main loop')
echo_starts = [None, None]
scanning = [False, False]
while True:
mouse_pos = pygame.mouse.get_pos()
mouse_cursor = DEFAULT_CURSOR
if not self.handle_events():
break
echo = self._beacon_listener.get_echo_event()
if echo:
self._log.debug('got echo event : %s' % str(echo))
echo_side, echo_heading, echo_start = echo
else:
echo_side, echo_heading, echo_start = None, None, None
for side, beacon in enumerate(self._beacons):
if echo_side == side:
# got an echo start or end for this beacon
if echo_start:
# record the very first echo in case we use
# both sensors
if echo_starts[side] == None:
echo_starts[side] = echo_heading
elif echo_starts[side] != None:
beacon.target_heading = (echo_starts[side] + echo_heading) / 2.0
self._log.debug(
'target %s heading: %f (%f, %f)' %
(_SIDES[side],
beacon.target_heading, echo_starts[side], echo_heading
)
)
echo_starts[side] = None
scanning[side] = self._beacon_listener.get_scan_status(side)
if scanning[side]:
# if beacon is scanning, update its sprite heading
beacon.heading = self._beacon_listener.beacon_headings[side]
else:
# otherwise make beacon sprite point upwards
beacon.heading = 0
# handle mouse interaction with beacons
# Note: the (0, 0) position exclusion is a trick to
# avoid the left beacon be hilited at start, because
# the mouse has not yet been inside the window, and
# thus the mouse position is reported as (0, 0)
if mouse_pos != (0, 0) and beacon.collidepoint(mouse_pos):
beacon.hilited = True
mouse_cursor = HAND_CURSOR
else:
beacon.hilited = False
# updates the mouse cursor to give a feedback of
# interactions
pygame.mouse.set_cursor(*mouse_cursor)
alpha, beta = [beacon.target_heading for beacon in self._beacons]
self._hud.alpha = alpha if alpha != None else None
self._hud.beta = -beta if beta != None else None
# if we have two echoes available, we can compute the
# target position and display it
if all(scanning) and alpha != None and beta != None:
alpha = math.radians(-alpha)
beta = math.radians(beta)
l = self._beacons[1]._location[0] - self._beacons[0]._location[0]
tga_tgb = math.tan(alpha) + math.tan(beta)
x = l * math.tan(alpha) / tga_tgb
y = l / tga_tgb
self._target.location = (
self._origin[0] + x,
self._origin[1] - y
)
self._target.visible = True
self._hud.target_location = (x, -y)
else:
self._target.visible = False
self._hud.target_location = None
self._status.fps = int(round(self._clock.get_fps()))
# redraw the display
self.display_update()
self._clock.tick(60)
except KeyboardInterrupt:
self._log.info('!! Keyboard interrupt in application !!')
except Exception as e:
self._log.exception(e)
# time for the final housekeeping
self._ctl_command('stop')
self._ctl_command('unregister')
self._ctl_command('disc')
self._close_control_socket()
finally:
self._beacon_listener.terminate()
self._beacon_listener = None
def handle_events(self):
""" User interaction events handling.
In addition to the standard termination signals, we handle the following
events:
click on a beacon:
starts or stops scanning, depending on its current status
(if SHIFT pressed, both beacons are involved)
key 'L':
enables beacon lasers
key 'l':
disables beacon lasers
key 'S':
starts scanning for both beacons
key 's':
stops scanning for both beacons
"""
for event in pygame.event.get():
if event.type == pygame.QUIT:
return False
elif event.type == pygame.MOUSEBUTTONUP and event.button == 1:
self.handle_click(event.pos)
elif event.type == pygame.KEYDOWN:
self.handle_key(event.key)
return True
def handle_click(self, pos):
""" Mouse click events handling.
Parameters:
pos:
mouse position as a coordinates tuple
"""
change_all = pygame.key.get_mods() & pygame.KMOD_SHIFT
for side, beacon in enumerate(self._beacons):
if beacon.collidepoint(pos):
cmde = 'stop' if self._beacon_listener.get_scan_status(side) else 'scan'
if change_all:
self._ctl_command(cmde)
break
else:
self._ctl_command('%s %s' % (cmde, _SIDES[side]))
def handle_key(self, key):
""" Keyboard event handling.
Parameters:
key:
the pressed key
"""
k_shift = pygame.key.get_mods() & pygame.KMOD_SHIFT
if key == pygame.K_l:
state = 1 if k_shift else 0
self._ctl_command('laser L:%d R:%d' % (state, state))
elif key == pygame.K_s:
cmde = 'scan' if k_shift else 'stop'
self._ctl_command('%s L R' % cmde)
elif key in (pygame.K_h, pygame.K_QUESTION):
self.display_help()
elif self._help and key == pygame.K_ESCAPE:
self._help = None
def display_update(self):
""" Graphical display drawing."""
#self._window.blit(self._bkgnd, (0, 0))
self._window.fill((0, 0, 0))
for beacon in self._beacons:
beacon.draw(self._window)
self._hud.draw(self._window)
self._status.draw(self._window)
self._logo.draw(self._window)
self._target.draw(self._window)
if self._help:
self._help.draw(self._window)
pygame.display.flip()
def display_help(self):
if not self._help:
help_w = 640
help_h = 480
x = (self._wndgeom.w - help_w) / 2
y = (self._wndgeom.h - help_h) / 2
self._help = Help(pygame.Rect(x, y, help_w, help_h))