コード例 #1
0
ファイル: beacons-gui.py プロジェクト: pobot/PyBot
    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
コード例 #2
0
ファイル: beacons-gui.py プロジェクト: pobot/PyBot
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))