BachelorarbeitCozmo/Cozmo_LineFollow/venv/Lib/site-packages/cozmo/conn.py

# Copyright (c) 2016-2017 Anki, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

'''Engine connection.

The SDK operates by connecting to the Cozmo "engine" - typically the Cozmo
app that runs on an iOS or Android device.

The engine is responsible for much of the work that Cozmo does, including
image recognition, path planning, behaviors and animation handling, etc.

The :mod:`cozmo.run` module takes care of opening a connection over a USB
connection to a device, but the :class:`CozmoConnection` class defined in
this module does the work of relaying messages to and from the engine and
dispatching them to the :class:`cozmo.robot.Robot` instance.
'''

# __all__ should order by constants, event classes, other classes, functions.
__all__ = ['EvtRobotFound', 'CozmoConnection']


import asyncio
import platform

import cozmoclad

from . import logger
from . import anim
from . import clad_protocol
from . import event
from . import exceptions
from . import robot
from . import version

from . import _clad
from ._clad import _clad_to_engine_cozmo, _clad_to_engine_iface, _clad_to_game_cozmo, _clad_to_game_iface


class EvtConnected(event.Event):
    '''Triggered when the initial connection to the device has been established.

    This connection is setup before contacting the robot - Wait for EvtRobotFound
    or EvtRobotReady for a usefully configured Cozmo instance.
    '''
    conn = 'The connected CozmoConnection object'


class EvtRobotFound(event.Event):
    '''Triggered when a Cozmo robot is detected, but before he's initialized.

    :class:`cozmo.robot.EvtRobotReady` is dispatched when the robot is fully initialized.
    '''
    robot = 'The Cozmo object for the robot'


class EvtConnectionClosed(event.Event):
    '''Triggered when the connection to the controlling device is closed.
    '''
    exc = 'The exception that triggered the closure, or None'


# Some messages have no robotID but should still be forwarded to the primary robot
FORCED_ROBOT_MESSAGES = {"AnimationAborted",
                         "AnimationEvent",
                         "BehaviorObjectiveAchieved",
                         "BehaviorTransition",
                         "BlockPickedUp",
                         "BlockPlaced",
                         "BlockPoolDataMessage",
                         "CarryStateUpdate",
                         "ChargerEvent",
                         "ConnectedObjectStates",
                         "CreatedFixedCustomObject",
                         "CubeLightsStateTransition",
                         "CurrentCameraParams",
                         "DefinedCustomObject",
                         "DeviceAccelerometerValuesRaw",
                         "DeviceAccelerometerValuesUser",
                         "DeviceGyroValues",
                         "IsDeviceIMUSupported",
                         "LoadedKnownFace",
                         "LocatedObjectStates",
                         "MemoryMapMessage",
                         "MemoryMapMessageBegin",
                         "MemoryMapMessageEnd",
                         "ObjectAccel",
                         "ObjectAvailable",
                         "ObjectConnectionState",
                         "ObjectMoved",
                         "ObjectPowerLevel",
                         "ObjectProjectsIntoFOV",
                         "ObjectStoppedMoving",
                         "ObjectTapped",
                         "ObjectTappedFiltered",
                         "ObjectUpAxisChanged",
                         "PerRobotSettings",
                         "ReactionaryBehaviorTransition",
                         "RobotChangedObservedFaceID",
                         "RobotCliffEventFinished",
                         "RobotCompletedAction",
                         "RobotDeletedAllCustomObjects",
                         "RobotDeletedCustomMarkerObjects",
                         "RobotDeletedFixedCustomObjects",
                         "RobotDelocalized",
                         "RobotErasedAllEnrolledFaces",
                         "RobotErasedEnrolledFace",
                         "RobotObservedFace",
                         "RobotObservedMotion",
                         "RobotObservedObject",
                         "RobotObservedPet",
                         "RobotObservedPossibleObject",
                         "RobotOnChargerPlatformEvent",
                         "RobotPoked",
                         "RobotReachedEnrollmentCount",
                         "RobotRenamedEnrolledFace",
                         "RobotState",
                         "UnexpectedMovement"}


class CozmoConnection(event.Dispatcher, clad_protocol.CLADProtocol):
    '''Manages the connection to the Cozmo app to communicate with the core engine.

    An instance of this class is passed to functions used with
    :func:`cozmo.run.connect`.  At the point the function is executed,
    the connection is already established and verified, and the
    :class:`EvtConnected` has already been sent.

    However, after the initial connection is established, programs will usually
    want to call :meth:`wait_for_robot` to wait for an actual Cozmo robot to
    be detected and initialized before doing useful work.
    '''

    #: callable: The factory function that returns a
    #: :class:`cozmo.robot.Robot` class or subclass instance.
    robot_factory = robot.Robot

    #: callable: The factory function that returns an
    #: :class:`cozmo.anim.AnimationNames` class or subclass instance.
    anim_names_factory = anim.AnimationNames

    # overrides for CLADProtocol
    clad_decode_union = _clad_to_game_iface.MessageEngineToGame
    clad_encode_union = _clad_to_engine_iface.MessageGameToEngine

    def __init__(self, *a, **kw):
        super().__init__(*a, **kw)
        self._is_connected = False
        self._is_ui_connected = False
        self._running = True
        self._robots = {}
        self._primary_robot = None

        #: A dict containing information about the device the connection is using.
        self.device_info = {}

        #: An :class:`cozmo.anim.AnimationNames` object that references all
        #: available animation names
        self.anim_names = self.anim_names_factory(self)


    #### Private Methods ####

    def __repr__(self):
        info = ' '.join(['%s="%s"' % (k, self.device_info[k])
            for k in sorted(self.device_info.keys())])
        return '<%s %s>' % (self.__class__.__name__, info)

    def connection_made(self, transport):
        super().connection_made(transport)
        self._is_connected = True

    def connection_lost(self, exc):
        super().connection_lost(exc)
        self._is_connected = False
        if self._running:
            self.abort(exceptions.ConnectionAborted("Lost connection to the device"))
            logger.error("Lost connection to the device: %s", exc)

    async def shutdown(self):
        '''Close the connection to the device.'''
        if self._running and self._is_connected:
            logger.info("Shutting down connection")
            self._running = False
            event._abort_futures(exceptions.SDKShutdown())
            self._stop_dispatcher()
            self.transport.close()

    def abort(self, exc):
        '''Abort the connection to the device.'''
        if self._running:
            logger.info('Aborting connection: %s', exc)
            self._running = False
            # Allow any currently pending futures to complete before the
            # remainder are aborted.
            self._loop.call_soon(lambda: event._abort_futures(exc))
            self._stop_dispatcher()
            self.transport.close()


    def msg_received(self, msg):
        '''Receives low level communication messages from the engine.'''
        if not self._running:
            return

        try:
            tag_name = msg.tag_name

            if tag_name == 'Ping':
                # short circuit to avoid unnecessary event overhead
                return self._handle_ping(msg._data)

            elif tag_name == 'UiDeviceConnected':
                # handle outside of event dispatch for quick abort in case
                # of a version mismatch problem.
                return self._handle_ui_device_connected(msg._data)

            msg = msg._data
            robot_id = getattr(msg, 'robotID', None)

            event_name = '_Msg' +  tag_name

            evttype = getattr(_clad, event_name, None)
            if evttype is None:
                logger.error('Received unknown CLAD message %s', event_name)
                return

            # Dispatch messages to the robot if they either:
            # a) are explicitly white listed in FORCED_ROBOT_MESSAGES
            # b) have a robotID specified in the message
            # Otherwise dispatch the message through this connection.
            if (robot_id is not None) or (tag_name in FORCED_ROBOT_MESSAGES):
                if robot_id is None:
                    # The only robot ID ever used is 1, so it is safe to assume that here as a default.
                    robot_id = 1
                self._process_robot_msg(robot_id, evttype, msg)
            else:
                self.dispatch_event(evttype, msg=msg)

        except Exception as exc:
            # No exceptions should reach this point; it's a bug if they do.
            self.abort(exc)

    def _process_robot_msg(self, robot_id, evttype, msg):
        if robot_id != 1:
            # Note: some messages replace robotID with value!=1 (like mfgID for example)
            #       as a result, this log may fire quite often. Log Level is set to debug
            #       since it suppressed by default (prevents spamming).
            logger.debug('INVALID ROBOT_ID SEEN robot_id=%s event=%s msg=%s', robot_id, evttype, msg.__str__())
            robot_id = 1 # XXX remove when errant messages have been fixed

        # Note: this code constructs the robot if it doesn't exist at this time
        robot = self._robots.get(robot_id)
        if not robot:
            logger.info('Found robot id=%s', robot_id)
            robot = self.robot_factory(self, robot_id, is_primary=self._primary_robot is None)
            self._robots[robot_id] = robot
            if not self._primary_robot:
                self._primary_robot = robot
            # Dispatch an event notifying that a new robot has been found
            # the robot itself will send EvtRobotReady after initialization
            self.dispatch_event(EvtRobotFound, robot=robot)

            # _initialize will set the robot to a known good state in the
            # background and dispatch a EvtRobotReady event when completed.
            robot._initialize()

        robot.dispatch_event(evttype, msg=msg)


    #### Properties ####

    @property
    def is_connected(self):
        '''bool: True if currently connected to the remote engine.'''
        return self._is_connected


    #### Private Event handlers ####

    def _handle_ping(self, msg):
        '''Respond to a ping event.'''
        if msg.isResponse:
            # To avoid duplication, pings originate from engine, and engine
            # accumulates the latency info from the responses
            logger.error("Only engine should receive responses")
        else:
            resp = _clad_to_engine_iface.Ping(
                counter=msg.counter,
                timeSent_ms=msg.timeSent_ms,
                isResponse=True)
            self.send_msg(resp)

    def _recv_default_handler(self, event, **kw):
        '''Default event handler.'''
        if event.event_name.startswith('msg_animation'):
            return self.anim.dispatch_event(event)

        logger.debug('Engine received unhandled event_name=%s  kw=%s', event, kw)

    def _recv_msg_animation_available(self, evt, msg):
        self.anim_names.dispatch_event(evt)

    def _recv_msg_end_of_message(self, evt, *a, **kw):
        self.anim_names.dispatch_event(evt)

    def _handle_ui_device_connected(self, msg):
        if msg.connectionType != _clad_to_engine_cozmo.UiConnectionType.SdkOverTcp:
            # This isn't for us
            return

        if msg.deviceID != 1:
            logger.error('Unexpected Device Id %s', msg.deviceID)
            return

        # Verify that engine and SDK are compatible
        clad_hashes_match = False
        try:
            cozmoclad.assert_clad_match(msg.toGameCLADHash, msg.toEngineCLADHash)
            clad_hashes_match = True
        except cozmoclad.CLADHashMismatch as exc:
            logger.error(exc)

        build_versions_match = (cozmoclad.__build_version__ == '00000.00000.00000'
            or cozmoclad.__build_version__ == msg.buildVersion)

        if clad_hashes_match and not build_versions_match:
            # If CLAD hashes match, and this is only a minor version change,
            # then still allow connection (it's just an app hotfix
            # that didn't require CLAD or SDK changes)
            sdk_major_version = cozmoclad.__build_version__.split(".")[0:2]
            build_major_version = msg.buildVersion.split(".")[0:2]
            build_versions_match = (sdk_major_version == build_major_version)

        if clad_hashes_match and build_versions_match:
            connection_success_msg = _clad_to_engine_iface.UiDeviceConnectionSuccess(
                connectionType=msg.connectionType,
                deviceID=msg.deviceID,
                buildVersion = cozmoclad.__version__,
                sdkModuleVersion = version.__version__,
                pythonVersion = platform.python_version(),
                pythonImplementation = platform.python_implementation(),
                osVersion = platform.platform(),
                cpuVersion = platform.machine())

            self.send_msg(connection_success_msg)

        else:
            try:
                wrong_version_msg = _clad_to_engine_iface.UiDeviceConnectionWrongVersion(
                    reserved=0,
                    connectionType=msg.connectionType,
                    deviceID = msg.deviceID,
                    buildVersion = cozmoclad.__version__)

                self.send_msg(wrong_version_msg)
            except AttributeError:
                pass

            line_separator = "=" * 80
            error_message = "\n" + line_separator + "\n"

            def _trimmed_version(ver_string):
                # Trim leading zeros from the version string.
                trimmed_string = ""
                for i in ver_string.split("."):
                    trimmed_string += str(int(i)) + "."
                return trimmed_string[:-1]  # remove trailing "."

            if not build_versions_match:
                error_message += ("App and SDK versions do not match!\n"
                                  "----------------------------------\n"
                                  "SDK's cozmoclad version: %s\n"
                                  "         != app version: %s\n\n"
                                  % (cozmoclad.__version__, _trimmed_version(msg.buildVersion)))

                if cozmoclad.__build_version__ < msg.buildVersion:
                    # App is newer
                    error_message += ('Please update your SDK to the newest version by calling command:\n'
                                      '"pip3 install --user --upgrade cozmo"\n'
                                      'and downloading the latest examples from:\n'
                                      'http://cozmosdk.anki.com/docs/downloads.html\n')
                else:
                    # SDK is newer
                    error_message += ('Please either:\n\n'
                                      '1) Update your app to the most recent version on the app store.\n'
                                      '2) Or, if you prefer, please determine which SDK version matches\n'
                                      '   your app version at: http://go.anki.com/cozmo-sdk-version\n'
                                      '   Then downgrade your SDK by calling the following command,\n'
                                      '   replacing SDK_VERSION with the version listed at that page:\n'
                                      '   "pip3 install --ignore-installed cozmo==SDK_VERSION"\n')

            else:
                # CLAD version mismatch
                error_message += ('CLAD Hashes do not match!\n'
                                  '-------------------------\n'
                                  'Your Python and C++ CLAD versions do not match - connection refused.\n'
                                  'Please check that you have the most recent versions of both the SDK and the\n'
                                  'Cozmo app. You may update your SDK by calling:\n'
                                  '"pip3 install --user --upgrade cozmo".\n'
                                  'Please also check the app store for a Cozmo app update.\n')

            error_message += line_separator
            logger.error(error_message)

            exc = exceptions.SDKVersionMismatch("SDK library does not match software running on device",
                                                sdk_version=version.__version__,
                                                sdk_app_version=cozmoclad.__version__,
                                                app_version=_trimmed_version(msg.buildVersion))

            self._abort_connection = True  # Ignore remaining messages - they're not safe to unpack

            self.abort(exc)
            return

        self._is_ui_connected = True
        self.dispatch_event(EvtConnected, conn=self)

        logger.info('App connection established. sdk_version=%s '
                'cozmoclad_version=%s app_build_version=%s',
                version.__version__, cozmoclad.__version__, msg.buildVersion)

        # We send RequestConnectedObjects and RequestLocatedObjectStates before
        # refreshing the animation names as this ensures that we will receive
        # the responses before we mark the robot as ready.
        self._request_connected_objects()
        self._request_located_objects()

        self.anim_names.refresh()

    def _request_connected_objects(self):
        # Request information on connected objects (e.g. the object ID of each cube)
        # (this won't provide location/pose info)
        msg = _clad_to_engine_iface.RequestConnectedObjects()
        self.send_msg(msg)

    def _request_located_objects(self):
        # Request the pose information for all objects whose location we know
        # (this won't include any objects where the location is currently not known)
        msg = _clad_to_engine_iface.RequestLocatedObjectStates()
        self.send_msg(msg)

    def _recv_msg_image_chunk(self, evt, *, msg):
        if self._primary_robot:
            self._primary_robot.dispatch_event(evt)

    #### Public Event Handlers ####

    #### Commands ####

    async def _wait_for_robot(self, timeout=5):
        if not self._primary_robot:
            await self.wait_for(EvtRobotFound, timeout=timeout)
        if self._primary_robot.is_ready:
            return self._primary_robot
        await self._primary_robot.wait_for(robot.EvtRobotReady, timeout=timeout)
        return self._primary_robot

    async def wait_for_robot(self, timeout=5):
        '''Wait for a Cozmo robot to connect and complete initialization.

        Args:
            timeout (float): Maximum length of time to wait for a robot to be ready in seconds.
        Returns:
            A :class:`cozmo.robot.Robot` instance that's ready to use.
        Raises:
            :class:`asyncio.TimeoutError` if there's no response from the robot.
        '''
        try:
            robot = await self._wait_for_robot(timeout)
            if robot and robot.drive_off_charger_on_connect:
                await robot.drive_off_charger_contacts().wait_for_completed()
        except asyncio.TimeoutError:
            logger.error('Timed out waiting for robot to initialize')
            raise
        return robot