Mad man.py

# Copyright (c) 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.

'''This module provides a 3D visualizer for Cozmo's world state.

It uses PyOpenGL, a Python OpenGL 3D graphics library which is available on most
platforms. It also depends on the Pillow library for image processing.

The easiest way to make use of this viewer is to call :func:`cozmo.run_program`
with `use_3d_viewer=True` or :func:`cozmo.run.connect_with_3dviewer`.

Warning:
    This package requires Python to have the PyOpenGL package installed, along
    with an implementation of GLUT (OpenGL Utility Toolkit).

    To install the Python packages do ``pip3 install --user "cozmo[3dviewer]"``

    On Windows and Linux you must also install freeglut (macOS / OSX has one
    preinstalled).

    On Linux: ``sudo apt-get install freeglut3``

    On Windows: Go to http://freeglut.sourceforge.net/ to get a ``freeglut.dll``
    file. It's included in any of the `Windows binaries` downloads. Place the DLL
    next to your Python script, or install it somewhere in your PATH to allow any
    script to use it."
'''


# __all__ should order by constants, event classes, other classes, functions.
__all__ = ['DynamicTexture', 'LoadedObjFile', 'OpenGLViewer', 'OpenGLWindow',
           'RenderableObject',
           'LoadMtlFile']


import collections
import math
import time
from pkg_resources import resource_stream

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *

from PIL import Image

from .exceptions import InvalidOpenGLGlutImplementation, RobotBusy
from . import logger
from . import nav_memory_map
from . import objects
from . import robot
from . import util
from . import world


# Check if OpenGL imported correctly and bound to a valid GLUT implementation


def _glut_install_instructions():
    if sys.platform.startswith('linux'):
        return "Install freeglut: `sudo apt-get install freeglut3`"
    elif sys.platform.startswith('darwin'):
        return "GLUT should already be installed by default on macOS!"
    elif sys.platform in ('win32', 'cygwin'):
        return "Install freeglut: You can download it from http://freeglut.sourceforge.net/ \n"\
            "You just need the `freeglut.dll` file, from any of the 'Windows binaries' downloads. "\
            "Place the DLL next to your Python script, or install it somewhere in your PATH "\
            "to allow any script to use it."
    else:
        return "(Instructions unknown for platform %s)" % sys.platform


def _verify_glut_init():
    # According to the documentation, just checking bool(glutInit) is supposed to be enough
    # However on Windows with no GLUT DLL that can still pass, even if calling the method throws a null function error.
    if bool(glutInit):
        try:
            glutInit()
            return True
        except OpenGL.error.NullFunctionError as e:
            pass

    return False


if not _verify_glut_init():
    raise InvalidOpenGLGlutImplementation(_glut_install_instructions())


_resource_package = __name__  # All resources are in subdirectories from this file's location


# Global viewer instance
opengl_viewer = None  # type: OpenGLViewer


class DynamicTexture:
    """Wrapper around An OpenGL Texture that can be dynamically updated."""

    def __init__(self):
        self._texId =  glGenTextures(1)
        self._width = None
        self._height = None
        # Bind an ID for this texture
        glBindTexture(GL_TEXTURE_2D, self._texId)
        # Use bilinear filtering if the texture has to be scaled
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR)
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR)

    def bind(self):
        """Bind the texture for rendering."""
        glBindTexture(GL_TEXTURE_2D, self._texId)

    def update(self, pil_image: Image.Image):
        """Update the texture to contain the provided image.

        Args:
            pil_image (PIL.Image.Image): The image to write into the texture.
        """
        # Ensure the image is in RGBA format and convert to the raw RGBA bytes.
        image_width, image_height = pil_image.size
        image = pil_image.convert("RGBA").tobytes("raw", "RGBA")

        # Bind the texture so that it can be modified.
        self.bind()
        if (self._width==image_width) and (self._height==image_height):
            # Same size - just need to update the texels.
            glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, image_width, image_height,
                            GL_RGBA, GL_UNSIGNED_BYTE, image)
        else:
            # Different size than the last frame (e.g. the Window is resizing)
            # Create a new texture of the correct size.
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image_width, image_height,
                         0, GL_RGBA, GL_UNSIGNED_BYTE, image)

        self._width = image_width
        self._height = image_height


def LoadMtlFile(filename):
    """Load a .mtl material file, and return the contents as a dictionary.

    Supports the subset of MTL required for the Cozmo 3D viewer assets.

    Args:
        filename (str): The filename of the file to load.

    Returns:
        dict: A dictionary mapping named MTL attributes to values.
    """
    contents = {}
    current_mtl = None

    resource_path = '/'.join(('assets', filename))  # Note: Deliberately not os.path.join, for use with pkg_resources
    file_data = resource_stream(_resource_package, resource_path)

    for line in file_data:
        line = line.decode("utf-8")  # Convert bytes line to a string
        if line.startswith('#'):
            # ignore comments in the file
            continue
        values = line.split()
        if not values:
            # ignore empty lines
            continue
        attribute_name = values[0]
        if attribute_name == 'newmtl':
            # Create a new empty material
            current_mtl = contents[values[1]] = {}
        elif current_mtl is None:
            raise ValueError("mtl file must start with newmtl statement")
        elif attribute_name == 'map_Kd':
            # Diffuse texture map - load the image into memory
            image_name = values[1]
            image_resource_path = '/'.join(('assets', image_name))  # Note: Deliberately not os.path.join, for use with pkg_resources
            image_file_data = resource_stream(_resource_package, image_resource_path)
            with Image.open(image_file_data) as image:
                image_width, image_height = image.size
                image = image.convert("RGBA").tobytes("raw", "RGBA")

            # Bind the image as a texture that can be used for rendering
            texture_id =  glGenTextures(1)
            current_mtl['texture_Kd'] = texture_id

            glBindTexture(GL_TEXTURE_2D, texture_id)
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR)
            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR)
            glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image_width, image_height,
                         0, GL_RGBA, GL_UNSIGNED_BYTE, image)
        else:
            # Store the values for this attribute as a list of float values
            current_mtl[attribute_name] = list(map(float, values[1:]))
    # File loaded successfully - return the contents
    return contents


class LoadedObjFile:
    """The loaded / parsed contents of a 3D Wavefront OBJ file.

    This is the intermediary step between the file on the disk, and a renderable
    3D object. It supports the subset of the OBJ file that was used in the
    Cozmo and Cube assets, and does not attempt to exhaustively support every
    possible setting.

    Args:
        filename (str): The filename of the OBJ file to load.
    """
    def __init__(self, filename):
        # list: The vertices (each vertex stored as list of 3 floats).
        self.vertices = []
        # list: The vertex normals (each normal stored as list of 3 floats).
        self.normals = []
        # list: The texture coordinates (each coordinate stored as list of 2 floats).
        self.tex_coords = []
        # dict: The faces for each mesh, indexed by mesh name.
        self.mesh_faces = {}

        # dict: A dictionary mapping named MTL attributes to values.
        self.mtl = None

        group_name = None
        material = None

        resource_path = '/'.join(('assets', filename))  # Note: Deliberately not os.path.join, for use with pkg_resources
        file_data = resource_stream(_resource_package, resource_path)

        for line in file_data:
            line = line.decode("utf-8")  # Convert bytes to string
            if line.startswith('#'):
                # ignore comments in the file
                continue

            values = line.split()
            if not values:
                # ignore empty lines
                continue

            if values[0] == 'v':
                # vertex position
                v = list(map(float, values[1:4]))
                self.vertices.append(v)
            elif values[0] == 'vn':
                # vertex normal
                v = list(map(float, values[1:4]))
                self.normals.append(v)
            elif values[0] == 'vt':
                # texture coordinate
                self.tex_coords.append(list(map(float, values[1:3])))
            elif values[0] in ('usemtl', 'usemat'):
                # material
                material = values[1]
            elif values[0] == 'mtllib':
                # material library (a filename)
                self.mtl = LoadMtlFile(values[1])
            elif values[0] == 'f':
                # A face made up of 3 or 4 vertices - e.g. `f v1 v2 v3` or `f v1 v2 v3 v4`
                # where each vertex definition is multiple indexes seperated by
                # slashes and can follow the following formats:
                # position_index
                # position_index/tex_coord_index
                # position_index/tex_coord_index/normal_index
                # position_index//normal_index

                positions = []
                tex_coords = []
                normals = []

                for vertex in values[1:]:
                    vertex_components = vertex.split('/')

                    positions.append(int(vertex_components[0]))

                    # There's only a texture coordinate if there's at least 2 entries and the 2nd entry is non-zero length
                    if len(vertex_components) >= 2 and len(vertex_components[1]) > 0:
                        tex_coords.append(int(vertex_components[1]))
                    else:
                        # OBJ file indexing starts at 1, so use 0 to indicate no entry
                        tex_coords.append(0)

                    # There's only a normal if there's at least 2 entries and the 2nd entry is non-zero length
                    if len(vertex_components) >= 3 and len(vertex_components[2]) > 0:
                        normals.append(int(vertex_components[2]))
                    else:
                        # OBJ file indexing starts at 1, so use 0 to indicate no entry
                        normals.append(0)

                try:
                    mesh_face = self.mesh_faces[group_name]
                except KeyError:
                    # Create a new mesh group
                    self.mesh_faces[group_name] = []
                    mesh_face = self.mesh_faces[group_name]

                mesh_face.append((positions, normals, tex_coords, material))
            elif values[0] == 'o':
                # object name - ignore
                pass
            elif values[0] == 'g':
                # group name (for a sub-mesh)
                group_name = values[1]
            elif values[0] == 's':
                # smooth shading (1..20, and 'off') - ignore
                pass
            else:
                logger.warning("LoadedObjFile Ignoring unhandled type '%s' in line %s",
                               values[0], values)


class RenderableObject:
    """Container for an object that can be rendered via OpenGL.

    Can contain multiple meshes, for e.g. articulated objects.

    Args:
        object_data (LoadedObjFile): The object data (vertices, faces, etc.)
            to generate the renderable object from.
        override_mtl (dict): An optional material to use as an override instead
            of the material specified in the data. This allows one OBJ file
            to be used to create multiple objects with different materials
            and textures. Use :meth:`LoadMtlFile` to generate a dict from a
            MTL file.
    """
    def __init__(self, object_data: LoadedObjFile, override_mtl=None):
        #: dict: The individual meshes, indexed by name, for this object.
        self.meshes = {}
        mtl_dict = override_mtl if (override_mtl is not None) else object_data.mtl

        def _as_rgba(color):
            if len(color) >= 4:
                return color
            else:
                # RGB - add alpha defaulted to 1
                return color + [1.0]

        for key in object_data.mesh_faces:
            new_gl_list = glGenLists(1)
            glNewList(new_gl_list, GL_COMPILE)

            self.meshes[key] = new_gl_list

            part_faces = object_data.mesh_faces[key]

            glEnable(GL_TEXTURE_2D)
            glFrontFace(GL_CCW)

            for face in part_faces:
                vertices, normals, texture_coords, material = face

                mtl = mtl_dict[material]
                if 'texture_Kd' in mtl:
                    # use diffuse texture map
                    glBindTexture(GL_TEXTURE_2D, mtl['texture_Kd'])
                else:
                    # No texture map
                    glBindTexture(GL_TEXTURE_2D, 0)

                # Diffuse light
                mtl_kd_rgba = _as_rgba(mtl['Kd'])
                glColor(mtl_kd_rgba)

                # Ambient light
                if 'Ka' in mtl:
                    mtl_ka_rgba = _as_rgba(mtl['Ka'])
                    glMaterialfv(GL_FRONT, GL_AMBIENT, mtl_ka_rgba)
                    glMaterialfv(GL_FRONT, GL_DIFFUSE, mtl_kd_rgba)
                else:
                    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mtl_kd_rgba);

                # Specular light
                if 'Ks' in mtl:
                    mtl_ks_rgba = _as_rgba(mtl['Ks'])
                    glMaterialfv(GL_FRONT, GL_SPECULAR, mtl_ks_rgba);
                    if 'Ns' in mtl:
                        specular_exponent = mtl['Ns']
                        glMaterialfv(GL_FRONT, GL_SHININESS, specular_exponent);

                # Polygon (N verts) with optional normals and tex coords
                glBegin(GL_POLYGON)
                for i in range(len(vertices)):
                    normal_index = normals[i]
                    if normal_index > 0:
                        glNormal3fv(object_data.normals[normal_index - 1])
                    tex_coord_index = texture_coords[i]
                    if tex_coord_index > 0:
                        glTexCoord2fv( object_data.tex_coords[tex_coord_index - 1])
                    glVertex3fv(object_data.vertices[vertices[i] - 1])
                glEnd()

            glDisable(GL_TEXTURE_2D)
            glEndList()

    def draw_all(self):
        """Draw all of the meshes."""
        for mesh in self.meshes.values():
            glCallList(mesh)


def _make_unit_cube():
    """Make a unit-size cube, with normals, centered at the origin"""
    new_gl_list = glGenLists(1)
    glNewList(new_gl_list, GL_COMPILE)

    # build each of the 6 faces
    for face_index in range(6):
        # calculate normal and vertices for this face
        vertex_normal = [0.0, 0.0, 0.0]
        vertex_pos_options1 = [-1.0, 1.0,  1.0, -1.0]
        vertex_pos_options2 = [ 1.0, 1.0, -1.0, -1.0]
        face_index_even = ((face_index % 2) == 0)
        # odd and even faces point in opposite directions
        normal_dir = 1.0 if face_index_even else -1.0
        if face_index < 2:
            # -X and +X faces (vert positions differ in Y,Z)
            vertex_normal[0] = normal_dir
            v1i = 1
            v2i = 2
        elif face_index < 4:
            # -Y and +Y faces (vert positions differ in X,Z)
            vertex_normal[1] = normal_dir
            v1i = 0
            v2i = 2
        else:
            # -Z and +Z faces (vert positions differ in X,Y)
            vertex_normal[2] = normal_dir
            v1i = 0
            v2i = 1

        vertex_pos = list(vertex_normal)

        # Polygon (N verts) with optional normals and tex coords
        glBegin(GL_POLYGON)
        for vert_index in range(4):
            vertex_pos[v1i] = vertex_pos_options1[vert_index]
            vertex_pos[v2i] = vertex_pos_options2[vert_index]
            glNormal3fv(vertex_normal)
            glVertex3fv(vertex_pos)
        glEnd()

    glEndList()

    return new_gl_list


class OpenGLWindow():
    """A Window displaying an OpenGL viewport.

    Args:
        x (int): The initial x coordinate of the window in pixels.
        y (int): The initial y coordinate of the window in pixels.
        width (int): The initial height of the window in pixels.
        height (int): The initial height of the window in pixels.
        window_name (str): The name / title for the window.
        is_3d (bool): True to create a Window for 3D rendering.
    """
    def __init__(self, x, y, width, height, window_name, is_3d):
        self._pos = (x, y)
        #: int: The width of the window
        self.width = width
        #: int: The height of the window
        self.height = height
        self._gl_window = None
        self._window_name = window_name
        self._is_3d = is_3d

    def init_display(self):
        """Initialze the OpenGL display parts of the Window.

        Warning:
            Must be called on the same thread as OpenGL (usually the main thread),
            and after glutInit().
        """
        glutInitWindowSize(self.width, self.height)
        glutInitWindowPosition(*self._pos)
        self.gl_window = glutCreateWindow(self._window_name)

        if self._is_3d:
            glClearColor(0, 0, 0, 0)
            glEnable(GL_DEPTH_TEST)
            glShadeModel(GL_SMOOTH)

        glutReshapeFunc(self._reshape)

    def _reshape(self, width, height):
        # Called from OpenGL whenever this window is resized.
        self.width = width
        self.height = height
        glViewport(0, 0, width, height)


class RobotRenderFrame():
    """Minimal copy of a Robot's state for 1 frame of rendering."""
    def __init__(self, robot):
        self.pose = robot.pose
        self.head_angle = robot.head_angle
        self.lift_position = robot.lift_position


class ObservableElementRenderFrame():
    """Minimal copy of a Cube's state for 1 frame of rendering."""
    def __init__(self, element):
        self.pose = element.pose
        self.is_visible = element.is_visible
        self.last_observed_time = element.last_observed_time

    @property
    def time_since_last_seen(self):
        # Equivalent of ObservableElement's method
        '''float: time since this element was last seen (math.inf if never)'''
        if self.last_observed_time is None:
            return math.inf
        return time.time() - self.last_observed_time


class CubeRenderFrame(ObservableElementRenderFrame):
    """Minimal copy of a Cube's state for 1 frame of rendering."""
    def __init__(self, cube):
        super().__init__(cube)


class FaceRenderFrame(ObservableElementRenderFrame):
    """Minimal copy of a Face's state for 1 frame of rendering."""
    def __init__(self, face):
        super().__init__(face)


class CustomObjectRenderFrame(ObservableElementRenderFrame):
    """Minimal copy of a CustomObject's state for 1 frame of rendering."""
    def __init__(self, obj, is_fixed):
        if is_fixed:
            # Not an observable, so init directly
            self.pose = obj.pose
            self.is_visible = None
            self.last_observed_time = None
        else:
            super().__init__(obj)

        self.is_fixed = is_fixed
        self.x_size_mm = obj.x_size_mm
        self.y_size_mm = obj.y_size_mm
        self.z_size_mm = obj.z_size_mm


class WorldRenderFrame():
    """Minimal copy of the World's state for 1 frame of rendering."""
    def __init__(self, robot):
        world = robot.world

        self.robot_frame = RobotRenderFrame(robot)

        self.cube_frames = []
        for i in range(3):
            cube_id = objects.LightCubeIDs[i]
            cube = world.get_light_cube(cube_id)
            if cube is None:
                self.cube_frames.append(None)
            else:
                self.cube_frames.append(CubeRenderFrame(cube))

        self.face_frames = []
        for face in world._faces.values():
            # Ignore faces that have a newer version (with updated id)
            # or if they haven't been seen in a while).
            if not face.has_updated_face_id and (face.time_since_last_seen < 60):
                self.face_frames.append(FaceRenderFrame(face))

        self.custom_object_frames = []
        for obj in world._objects.values():
            is_custom = isinstance(obj, objects.CustomObject)
            is_fixed = isinstance(obj, objects.FixedCustomObject)
            if is_custom or is_fixed:
                self.custom_object_frames.append(CustomObjectRenderFrame(obj, is_fixed))


class RobotControlIntents():
    """Input intents for controlling the robot.

    These are sent from the OpenGL thread, and consumed by the SDK thread for
    issuing movement commands on Cozmo (to provide a remote-control interface).
    """
    def __init__(self, left_wheel_speed=0.0, right_wheel_speed=0.0,
                 lift_speed=0.0, head_speed=0.0):
        self.left_wheel_speed = left_wheel_speed
        self.right_wheel_speed = right_wheel_speed
        self.lift_speed = lift_speed
        self.head_speed = head_speed


class OpenGLViewer():
    """OpenGL based 3D Viewer.

    Handles rendering of both a 3D world view and a 2D camera window.

    Args:
        enable_camera_view (bool): True to also open a 2nd window to display
            the live camera view.
        show_viewer_controls (bool): Specifies whether to draw controls on the view.
    """
    def __init__(self, enable_camera_view, show_viewer_controls=True):
        # Queues from SDK thread to OpenGL thread
        self._img_queue = collections.deque(maxlen=1)
        self._nav_memory_map_queue = collections.deque(maxlen=1)
        self._world_frame_queue = collections.deque(maxlen=1)
        # Queue from OpenGL thread to SDK thread
        self._input_intent_queue = collections.deque(maxlen=1)

        self._last_robot_control_intents = RobotControlIntents()

        self._is_keyboard_control_enabled = False

        self._image_handler = None
        self._nav_map_handler = None
        self._robot_state_handler = None
        self._exit_requested = False

        global opengl_viewer
        if opengl_viewer is not None:
            logger.error("Multiple OpenGLViewer instances not expected: "
                         "OpenGL / GLUT only supports running 1 blocking instance on the main thread.")
        opengl_viewer = self

        self.main_window = OpenGLWindow(0, 0, 800, 600,
                                        b"Cozmo 3D Visualizer", is_3d=True)

        self._camera_view_texture = None  # type: DynamicTexture
        self.viewer_window = None  # type: OpenGLWindow
        if enable_camera_view:
            self.viewer_window = OpenGLWindow(self.main_window.width, 0, 640, 480,
                                              b"Cozmo CameraFeed", is_3d=False)

        self.cozmo_object = None  # type: RenderableObject
        self.cube_objects = []

        self._latest_world_frame = None  # type: WorldRenderFrame
        self._nav_memory_map_display_list = None

        # Keyboard
        self._is_key_pressed = {}
        self._is_alt_down = False
        self._is_ctrl_down = False
        self._is_shift_down = False

        # Mouse
        self._is_mouse_down = {}
        self._mouse_pos = None  # type: util.Vector2

        # Controls
        self._show_controls = show_viewer_controls
        self._instructions = '\n'.join(['W, S: Move forward, backward',
                                        'A, D: Turn left, right',
                                        'R, F: Lift up, down',
                                        'T, G: Head up, down',
                                        '',
                                        'LMB: Rotate camera',
                                        'RMB: Move camera',
                                        'LMB + RMB: Move camera up/down',
                                        'LMB + Z: Zoom camera',
                                        'X: same as RMB',
                                        'TAB: center view on robot',
                                        '',
                                        'H: Toggle help'])

        # Camera position and orientation defined by a look-at positions
        # and a pitch/and yaw to rotate around that along with a distance
        self._camera_look_at = util.Vector3(100.0, -25.0, 0.0)
        self._camera_pitch = math.radians(40)
        self._camera_yaw = math.radians(270)
        self._camera_distance = 500.0
        self._camera_pos = util.Vector3(0, 0, 0)
        self._camera_up = util.Vector3(0.0, 0.0, 1.0)
        self._calculate_camera_pos()

    def _request_exit(self):
        self._exit_requested = True
        if bool(glutLeaveMainLoop):
            glutLeaveMainLoop()

    def _calculate_camera_pos(self):
        # Calculate camera position based on look-at, distance and angles
        cos_pitch = math.cos(self._camera_pitch)
        sin_pitch = math.sin(self._camera_pitch)
        cos_yaw = math.cos(self._camera_yaw)
        sin_yaw = math.sin(self._camera_yaw)
        cam_distance = self._camera_distance
        cam_look_at = self._camera_look_at

        self._camera_pos._x = cam_look_at.x + (cam_distance * cos_pitch * cos_yaw)
        self._camera_pos._y = cam_look_at.y + (cam_distance * cos_pitch * sin_yaw)
        self._camera_pos._z = cam_look_at.z + (cam_distance * sin_pitch)

    def _update_modifier_keys(self):
        modifiers = glutGetModifiers()
        self._is_alt_down = (modifiers & GLUT_ACTIVE_ALT != 0)
        self._is_ctrl_down = (modifiers & GLUT_ACTIVE_CTRL != 0)
        self._is_shift_down = (modifiers & GLUT_ACTIVE_SHIFT != 0)

    def _update_intents_for_robot(self):
        # Update driving intents based on current input, and pass to SDK thread
        # so that it can pass the input on to the robot.
        def get_intent_direction(key1, key2):
            # Helper for keyboard inputs that have 1 positive and 1 negative input
            pos_key = self._is_key_pressed.get(key1, False)
            neg_key = self._is_key_pressed.get(key2, False)
            return pos_key - neg_key

        drive_dir = get_intent_direction(b'w', b's')
        turn_dir = get_intent_direction(b'd', b'a')
        lift_dir = get_intent_direction(b'r', b'f')
        head_dir = get_intent_direction(b't', b'g')

        if drive_dir < 0:
            # It feels more natural to turn the opposite way when reversing
            turn_dir = -turn_dir

        # Scale drive speeds with SHIFT (faster) and ALT (slower)
        if self._is_shift_down:
            speed_scalar = 2.0
        elif self._is_alt_down:
            speed_scalar = 0.5
        else:
            speed_scalar = 1.0

        drive_speed = 75.0 * speed_scalar
        turn_speed = 100.0 * speed_scalar

        left_wheel_speed = (drive_dir * drive_speed) + (turn_speed * turn_dir)
        right_wheel_speed = (drive_dir * drive_speed) - (turn_speed * turn_dir)
        lift_speed = 4.0 * lift_dir * speed_scalar
        head_speed = head_dir * speed_scalar

        control_intents = RobotControlIntents(left_wheel_speed, right_wheel_speed,
                                              lift_speed, head_speed)
        self._input_intent_queue.append(control_intents)

    def _idle(self):
        if self._is_keyboard_control_enabled:
            self._update_intents_for_robot()
        glutPostRedisplay()

    def _visible(self, vis):
        # Called from OpenGL when visibility changes (windows are either visible
        # or completely invisible/hidden)
        if vis == GLUT_VISIBLE:
            glutIdleFunc(self._idle)
        else:
            glutIdleFunc(None)

    def _draw_memory_map(self):
        # Update the renderable map if new data is available, and
        # render the latest map received.
        new_nav_memory_map = None
        try:
            new_nav_memory_map = self._nav_memory_map_queue.popleft()
        except IndexError:
            # no new nav map - queue is empty
            pass

        # Rebuild the renderable map if it has changed
        if new_nav_memory_map is not None:
            cen = new_nav_memory_map.center
            half_size = new_nav_memory_map.size * 0.5

            if self._nav_memory_map_display_list is None:
                self._nav_memory_map_display_list = glGenLists(1)
            glNewList(self._nav_memory_map_display_list, GL_COMPILE)

            glPushMatrix()

            color_light_gray = (0.65, 0.65, 0.65)
            glColor3f(*color_light_gray)
            glBegin(GL_LINE_STRIP)
            glVertex3f(cen.x + half_size, cen.y + half_size, cen.z)  # TL
            glVertex3f(cen.x + half_size, cen.y - half_size, cen.z)  # TR
            glVertex3f(cen.x - half_size, cen.y - half_size, cen.z)  # BR
            glVertex3f(cen.x - half_size, cen.y + half_size, cen.z)  # BL
            glVertex3f(cen.x + half_size, cen.y + half_size,
                       cen.z)  # TL (close loop)
            glEnd()

            def color_for_content(content):
                nct = nav_memory_map.NodeContentTypes
                colors = {nct.Unknown.id: (0.3, 0.3, 0.3),         # dark gray
                          nct.ClearOfObstacle.id: (0.0, 1.0, 0.0), # green
                          nct.ClearOfCliff.id: (0.0, 0.5, 0.0),    # dark green
                          nct.ObstacleCube.id: (1.0, 0.0, 0.0),    # red
                          nct.ObstacleCharger.id: (1.0, 0.5, 0.0), # orange
                          nct.Cliff.id: (0.0, 0.0, 0.0),           # black
                          nct.VisionBorder.id: (1.0, 1.0, 0.0)     # yellow
                          }

                col = colors.get(content.id)
                if col is None:
                    logger.error("Unhandled content type %s" % str(content))
                    col = (1.0, 1.0, 1.0)  # white
                return col

            fill_z = cen.z - 0.4

            def _recursive_draw(grid_node: nav_memory_map.NavMemoryMapGridNode):
                if grid_node.children is not None:
                    for child in grid_node.children:
                        _recursive_draw(child)
                else:
                    # leaf node - render as a quad
                    map_alpha = 0.5
                    cen = grid_node.center
                    half_size = grid_node.size * 0.5

                    # Draw outline
                    glColor4f(*color_light_gray, 1.0)  # fully opaque
                    glBegin(GL_LINE_STRIP)
                    glVertex3f(cen.x + half_size, cen.y + half_size, cen.z)
                    glVertex3f(cen.x + half_size, cen.y - half_size, cen.z)
                    glVertex3f(cen.x - half_size, cen.y - half_size, cen.z)
                    glVertex3f(cen.x - half_size, cen.y + half_size, cen.z)
                    glVertex3f(cen.x + half_size, cen.y + half_size, cen.z)
                    glEnd()

                    # Draw filled contents
                    glColor4f(*color_for_content(grid_node.content), map_alpha)
                    glBegin(GL_TRIANGLE_STRIP)
                    glVertex3f(cen.x + half_size, cen.y + half_size, fill_z)
                    glVertex3f(cen.x + half_size, cen.y - half_size, fill_z)
                    glVertex3f(cen.x - half_size, cen.y + half_size, fill_z)
                    glVertex3f(cen.x - half_size, cen.y - half_size, fill_z)
                    glEnd()

            _recursive_draw(new_nav_memory_map.root_node)

            glPopMatrix()
            glEndList()
        else:
            # The source data hasn't changed - keep using the same call list
            pass

        if self._nav_memory_map_display_list is not None:
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
            glEnable(GL_BLEND)
            glPushMatrix()
            glCallList(self._nav_memory_map_display_list)
            glPopMatrix()


    def _draw_cozmo(self, robot_frame):
        if self.cozmo_object is None:
            return

        robot_pose = robot_frame.pose
        robot_head_angle = robot_frame.head_angle
        robot_lift_position = robot_frame.lift_position

        # Angle of the lift in the object's initial default pose.
        LIFT_ANGLE_IN_DEFAULT_POSE = -11.36

        robot_matrix = robot_pose.to_matrix()
        head_angle = robot_head_angle.degrees
        # Get the angle of Cozmo's lift for rendering - we subtract the angle
        # of the lift in the default pose in the object, and apply the inverse
        # rotation
        lift_angle = -(robot_lift_position.angle.degrees - LIFT_ANGLE_IN_DEFAULT_POSE)

        glPushMatrix()
        glEnable(GL_LIGHTING)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glEnable(GL_BLEND)

        glMultMatrixf(robot_matrix.in_row_order)

        robot_scale_amt = 10.0  # cm to mm
        glScalef(robot_scale_amt, robot_scale_amt, robot_scale_amt)
        glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)

        # Pivot offset for where the fork rotates around itself
        FORK_PIVOT_X = 3.0
        FORK_PIVOT_Z = 3.4

        # Offset for the axel that the upper arm rotates around.
        UPPER_ARM_PIVOT_X = -3.73
        UPPER_ARM_PIVOT_Z = 4.47

        # Offset for the axel that the lower arm rotates around.
        LOWER_ARM_PIVOT_X = -3.74
        LOWER_ARM_PIVOT_Z = 3.27

        # Offset for the pivot that the head rotates around.
        HEAD_PIVOT_X = -1.1
        HEAD_PIVOT_Z = 4.75

        # Render the static body meshes - first the main body:
        glCallList(self.cozmo_object.meshes["body_geo"])
        # Render the left treads and wheels
        glCallList(self.cozmo_object.meshes["trackBase_L_geo"])
        glCallList(self.cozmo_object.meshes["wheel_BL_geo"])
        glCallList(self.cozmo_object.meshes["wheel_FL_geo"])
        glCallList(self.cozmo_object.meshes["tracks_L_geo"])
        # Render the right treads and wheels
        glCallList(self.cozmo_object.meshes["trackBase_R_geo"])
        glCallList(self.cozmo_object.meshes["wheel_BR_geo"])
        glCallList(self.cozmo_object.meshes["wheel_FR_geo"])
        glCallList(self.cozmo_object.meshes["tracks_R_geo"])

        # Render the fork at the front (but not the arms)
        glPushMatrix()
        # The fork rotates first around upper arm (to get it to the correct position).
        glTranslatef(UPPER_ARM_PIVOT_X, 0.0, UPPER_ARM_PIVOT_Z)
        glRotatef(lift_angle, 0, 1, 0)
        glTranslatef(-UPPER_ARM_PIVOT_X, 0.0, -UPPER_ARM_PIVOT_Z)
        # The fork then rotates back around itself as it always hangs vertically.
        glTranslatef(FORK_PIVOT_X, 0.0, FORK_PIVOT_Z)
        glRotatef(-lift_angle, 0, 1, 0)
        glTranslatef(-FORK_PIVOT_X, 0.0, -FORK_PIVOT_Z)
        # Render
        glCallList(self.cozmo_object.meshes["fork_geo"])
        glPopMatrix()

        # Render the upper arms:
        glPushMatrix()
        # Rotate the upper arms around the upper arm joint
        glTranslatef(UPPER_ARM_PIVOT_X, 0.0, UPPER_ARM_PIVOT_Z)
        glRotatef(lift_angle, 0, 1, 0)
        glTranslatef(-UPPER_ARM_PIVOT_X, 0.0, -UPPER_ARM_PIVOT_Z)
        # Render
        glCallList(self.cozmo_object.meshes["uprArm_L_geo"])
        glCallList(self.cozmo_object.meshes["uprArm_geo"])
        glPopMatrix()

        # Render the lower arms:
        glPushMatrix()
        # Rotate the lower arms around the lower arm joint
        glTranslatef(LOWER_ARM_PIVOT_X, 0.0, LOWER_ARM_PIVOT_Z)
        glRotatef(lift_angle, 0, 1, 0)
        glTranslatef(-LOWER_ARM_PIVOT_X, 0.0, -LOWER_ARM_PIVOT_Z)
        # Render
        glCallList(self.cozmo_object.meshes["lwrArm_L_geo"])
        glCallList(self.cozmo_object.meshes["lwrArm_R_geo"])
        glPopMatrix()

        # Render the head:
        glPushMatrix()
        # Rotate the head around the pivot
        glTranslatef(HEAD_PIVOT_X, 0.0, HEAD_PIVOT_Z)
        glRotatef(-head_angle, 0, 1, 0)
        glTranslatef(-HEAD_PIVOT_X, 0.0, -HEAD_PIVOT_Z)
        # Render all of the head meshes
        glCallList(self.cozmo_object.meshes["head_geo"])
        # Screen
        glCallList(self.cozmo_object.meshes["backScreen_mat"])
        glCallList(self.cozmo_object.meshes["screenEdge_geo"])
        glCallList(self.cozmo_object.meshes["overscan_1_geo"])
        # Eyes
        glCallList(self.cozmo_object.meshes["eye_L_geo"])
        glCallList(self.cozmo_object.meshes["eye_R_geo"])
        # Eyelids
        glCallList(self.cozmo_object.meshes["eyeLid_R_top_geo"])
        glCallList(self.cozmo_object.meshes["eyeLid_L_top_geo"])
        glCallList(self.cozmo_object.meshes["eyeLid_L_btm_geo"])
        glCallList(self.cozmo_object.meshes["eyeLid_R_btm_geo"])
        # Face cover (drawn last as it's translucent):
        glCallList(self.cozmo_object.meshes["front_Screen_geo"])
        glPopMatrix()

        glDisable(GL_LIGHTING)
        glPopMatrix()


    def _draw_unit_cube(self, color, draw_solid):
        glColor(color)

        if draw_solid:
            ambient_color = [color[0]*0.1, color[1]*0.1, color[2]*0.1, 1.0]
        else:
            ambient_color = color
        glMaterialfv(GL_FRONT, GL_AMBIENT, ambient_color)
        glMaterialfv(GL_FRONT, GL_DIFFUSE, color)
        glMaterialfv(GL_FRONT, GL_SPECULAR,  color)

        glMaterialfv(GL_FRONT, GL_SHININESS, 10.0);

        if draw_solid:
            glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
        else:
            glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)

        glCallList(self.unit_cube)


    def _display_3d_view(self, window):
        glutSetWindow(window.gl_window)

        # Clear the screen and the depth buffer
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

        # Set up the projection matrix
        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        fov = 45.0
        aspect_ratio = window.width / window.height
        near_clip_plane = 1.0
        far_clip_plane = 1000.0
        gluPerspective(fov, aspect_ratio, near_clip_plane, far_clip_plane)

        # Switch to model matrix for rendering everything
        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()

        # Add a light near the origin
        light_ambient = [1.0, 1.0, 1.0, 1.0]
        light_diffuse = [1.0, 1.0, 1.0, 1.0]
        light_specular = [1.0, 1.0, 1.0, 1.0]
        glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient)
        glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse)
        glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular)
        light_pos = [0, 20, 10, 1]
        glLightfv(GL_LIGHT0, GL_POSITION, light_pos)
        glEnable(GL_LIGHT0)

        glScalef(0.1, 0.1, 0.1)  # mm to cm

        # Orient the camera
        self._calculate_camera_pos()

        gluLookAt(*self._camera_pos.x_y_z,
                  *self._camera_look_at.x_y_z,
                  *self._camera_up.x_y_z)

        # Update the latest world frame if there is a new one available
        try:
            world_frame = self._world_frame_queue.popleft()  # type: WorldRenderFrame
            self._latest_world_frame = world_frame
        except IndexError:
            world_frame = self._latest_world_frame
            pass

        if world_frame is not None:
            glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
            glEnable(GL_LIGHTING)
            glEnable(GL_NORMALIZE)  # to re-scale scaled normals

            robot_frame = world_frame.robot_frame
            robot_pose = robot_frame.pose

            # Render the cubes
            for i in range(3):
                cube_obj = self.cube_objects[i]
                cube_frame = world_frame.cube_frames[i]
                if cube_frame is None:
                    continue

                cube_pose = cube_frame.pose
                if cube_pose is not None and cube_pose.is_comparable(robot_pose):
                    glPushMatrix()

                    # TODO if cube_pose.is_accurate is False, render half-translucent?
                    #  (This would require using a shader, or having duplicate objects)

                    cube_matrix = cube_pose.to_matrix()
                    glMultMatrixf(cube_matrix.in_row_order)

                    # Cube is drawn slightly larger than the 10mm to 1 cm scale, as the model looks small otherwise
                    cube_scale_amt = 10.7
                    glScalef(cube_scale_amt, cube_scale_amt, cube_scale_amt)

                    cube_obj.draw_all()
                    glPopMatrix()

            glBindTexture(GL_TEXTURE_2D, 0)

            for face in world_frame.face_frames:
                face_pose = face.pose
                if face_pose is not None and face_pose.is_comparable(robot_pose):
                    glPushMatrix()
                    face_matrix = face_pose.to_matrix()
                    glMultMatrixf(face_matrix.in_row_order)

                    # Approximate size of a head
                    glScalef(100, 25, 100)

                    FACE_OBJECT_COLOR = [0.5, 0.5, 0.5, 1.0]
                    draw_solid = face.time_since_last_seen < 30
                    self._draw_unit_cube(FACE_OBJECT_COLOR, draw_solid)

                    glPopMatrix()

            for obj in world_frame.custom_object_frames:
                obj_pose = obj.pose
                if obj_pose is not None and obj_pose.is_comparable(robot_pose):
                    glPushMatrix()
                    obj_matrix = obj_pose.to_matrix()
                    glMultMatrixf(obj_matrix.in_row_order)

                    glScalef(obj.x_size_mm * 0.5,
                             obj.y_size_mm * 0.5,
                             obj.z_size_mm * 0.5)

                    # Only draw solid object for observable custom objects

                    if obj.is_fixed:
                        # fixed objects are drawn as transparent outlined boxes to make
                        # it clearer that they have no effect on vision.
                        FIXED_OBJECT_COLOR = [1.0, 0.7, 0.0, 1.0]
                        self._draw_unit_cube(FIXED_OBJECT_COLOR, False)
                    else:
                        CUSTOM_OBJECT_COLOR = [1.0, 0.3, 0.3, 1.0]
                        self._draw_unit_cube(CUSTOM_OBJECT_COLOR, True)

                    glPopMatrix()

            glDisable(GL_LIGHTING)

            self._draw_cozmo(robot_frame)

        if self._show_controls:
            self._draw_controls()

        # Draw the (translucent) nav map last so it's sorted correctly against opaque geometry
        self._draw_memory_map()

        glutSwapBuffers()

    def _draw_controls(self):
        try:
            GLUT_BITMAP_9_BY_15
        except NameError:
            pass
        else: 
            self._draw_text(GLUT_BITMAP_9_BY_15, self._instructions, 10, 10)

    def _draw_text(self, font, input, x, y, line_height=16, r=1.0, g=1.0, b=1.0):
        '''Render text based on window position. The origin is in the bottom-left.'''
        glColor3f(r, g, b)
        glWindowPos2f(x,y)
        input_list = input.split('\n')
        y = y + (line_height * (len(input_list) -1))
        for line in input_list:
            glWindowPos2f(x, y)
            y -= line_height
            for ch in line:
                glutBitmapCharacter(font, ctypes.c_int(ord(ch)))

    def _display_camera_view(self, window):
        glutSetWindow(window.gl_window)

        if self._camera_view_texture is None:
            self._camera_view_texture = DynamicTexture()

        target_width = window.width
        target_height = window.height
        target_aspect = 320 / 240  # (Camera-feed resolution and aspect ratio)
        max_u = 1.0
        max_v = 1.0
        if (target_width / target_height) < target_aspect:
            target_height = target_width / target_aspect
            max_v *= target_height / window.height
        elif (target_width / target_height) > target_aspect:
            target_width = target_height * target_aspect
            max_u *= target_width / window.width
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glEnable(GL_TEXTURE_2D)

        # Try getting a new image if one has been added
        image = None
        try:
            image = self._img_queue.popleft()
        except IndexError:
            # no new image - queue is empty
            pass

        if image:
            # There's a new image - update the texture
            self._camera_view_texture.update(image)
        else:
            # keep using the most recent texture
            self._camera_view_texture.bind()

        # Display the image as a tri-strip with 4 vertices
        glBegin(GL_TRIANGLE_STRIP)
        # (0,0) = Top Left, (1,1) = Bottom Right
        # left, bottom
        glTexCoord2f(0.0, 1.0)
        glVertex2f(-max_u, -max_v)
        # right, bottom
        glTexCoord2f(1.0, 1.0)
        glVertex2f(max_u, -max_v)
        # left, top
        glTexCoord2f(0.0, 0.0)
        glVertex2f(-max_u, max_v)
        # right, top
        glTexCoord2f(1.0, 0.0)
        glVertex2f(max_u, max_v)
        glEnd()

        glDisable(GL_TEXTURE_2D)

        glutSwapBuffers()

    def _display(self):
        try:
            self._display_3d_view(self.main_window)

            if self.viewer_window:
                self._display_camera_view(self.viewer_window)
        except KeyboardInterrupt:
            logger.info("_display caught KeyboardInterrupt - exitting")
            self._request_exit()

    def _key_byte_to_lower(self, key):
        # Convert bytes-object (representing keyboard character) to lowercase equivalent
        if (key >= b'A') and (key <= b'Z'):
            lowercase_key = ord(key) - ord(b'A') + ord(b'a')
            lowercase_key = bytes([lowercase_key])
            return lowercase_key
        return key

    def _on_key_up(self, key, x, y):
        key = self._key_byte_to_lower(key)
        self._update_modifier_keys()
        self._is_key_pressed[key] = False

    def _on_key_down(self, key, x, y):
        key = self._key_byte_to_lower(key)
        self._update_modifier_keys()
        self._is_key_pressed[key] = True

        if ord(key) == 9:  # Tab
            # Set Look-At point to current robot position
            world_frame = self._latest_world_frame
            if world_frame is not None:
                robot_pos = world_frame.robot_frame.pose.position
                self._camera_look_at.set_to(robot_pos)
        elif ord(key) == 27:  # Escape key
            self._request_exit()
        elif ord(key) == 72 or ord(key) == 104: # H key
            self._show_controls = not self._show_controls


    def _on_special_key_up(self, key, x, y):
        self._update_modifier_keys()

    def _on_special_key_down(self, key, x, y):
        self._update_modifier_keys()

    def _on_mouse_button(self, button, state, x, y):
        # Don't update modifier keys- reading modifier keys is unreliable
        # from _on_mouse_button (for LMB down/up), only SHIFT key seems to read there
        #self._update_modifier_keys()
        is_down = (state == GLUT_DOWN)
        self._is_mouse_down[button] = is_down
        self._mouse_pos = util.Vector2(x, y)

    def _on_mouse_move_internal(self, x, y, is_active):
        # is_active is True if this is not passive (i.e. a mouse button was down)
        last_mouse_pos = self._mouse_pos
        self._mouse_pos = util.Vector2(x, y)
        if last_mouse_pos is None:
            # First mouse update - ignore (we need a delta of mouse positions)
            return

        left_button = self._is_mouse_down.get(GLUT_LEFT_BUTTON, False)
        # For laptop and other 1-button mouse users, treat 'x' key as a right mouse button too
        right_button = (self._is_mouse_down.get(GLUT_RIGHT_BUTTON, False) or
                        self._is_key_pressed.get(b'x', False))

        MOUSE_SPEED_SCALAR = 1.0  # general scalar for all mouse movement sensitivity
        MOUSE_ROTATE_SCALAR = 0.025  # additional scalar for rotation sensitivity
        mouse_delta = (self._mouse_pos - last_mouse_pos) * MOUSE_SPEED_SCALAR

        if left_button and right_button:
            # Move up/down
            self._camera_look_at._z -= mouse_delta.y
        elif right_button:
            # Move forward/back and left/right
            pitch = self._camera_pitch
            yaw = self._camera_yaw
            camera_offset = util.Vector3(math.cos(yaw), math.sin(yaw), math.sin(pitch))

            heading = math.atan2(camera_offset.y, camera_offset.x)

            half_pi = math.pi * 0.5
            self._camera_look_at._x += mouse_delta.x * math.cos(heading + half_pi)
            self._camera_look_at._y += mouse_delta.x * math.sin(heading + half_pi)

            self._camera_look_at._x += mouse_delta.y * math.cos(heading)
            self._camera_look_at._y += mouse_delta.y * math.sin(heading)
        elif left_button:
            if self._is_key_pressed.get(b'z', False):
                # Zoom in/out
                self._camera_distance = max(0.1, self._camera_distance + mouse_delta.y)
            else:
                # Adjust the Camera pitch and yaw
                self._camera_pitch = (self._camera_pitch - (mouse_delta.y * MOUSE_ROTATE_SCALAR))
                self._camera_yaw = (self._camera_yaw + (mouse_delta.x * MOUSE_ROTATE_SCALAR)) % (2.0 * math.pi)
                # Clamp pitch to slightyly less than pi/2 to avoid lock/errors at full up/down
                max_rotation = math.pi * 0.49
                self._camera_pitch = max(-max_rotation, min(max_rotation, self._camera_pitch))

    def _on_mouse_move(self, x, y):
        # Mouse movement when at least one button down
        self._on_mouse_move_internal(x, y, True)

    def _on_mouse_move_passive(self, x, y):
        # Mouse movement when no button down
        self._on_mouse_move_internal(x, y, False)

    def init_display(self):
        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH)

        self.main_window.init_display()

        glutDisplayFunc(self._display)  # Note: both windows call the same DisplayFunc
        glutKeyboardFunc(self._on_key_down)
        glutSpecialFunc(self._on_special_key_down)

        # [Keyboard/Special]Up methods aren't supported on some old GLUT implementations
        has_keyboard_up = False
        has_special_up = False
        try:
            if bool(glutKeyboardUpFunc):
                glutKeyboardUpFunc(self._on_key_up)
                has_keyboard_up = True
            if bool(glutSpecialUpFunc):
                glutSpecialUpFunc(self._on_special_key_up)
                has_special_up = True
        except OpenGL.error.NullFunctionError:
            # Methods aren't available on this GLUT version
            pass

        if not has_keyboard_up or not has_special_up:
            # Warn on old GLUT implementations that don't implement much of the interface.
            logger.warning("Warning: Old GLUT implementation detected - keyboard remote control of Cozmo disabled."
                            "We recommend installing freeglut. %s", _glut_install_instructions())
            self._is_keyboard_control_enabled = False
        else:
            self._is_keyboard_control_enabled = True

        try:
            GLUT_BITMAP_9_BY_15
        except NameError:
            logger.warning("Warning: GLUT font not detected. Help message will be unavailable.")

        glutMouseFunc(self._on_mouse_button)
        glutMotionFunc(self._on_mouse_move)
        glutPassiveMotionFunc(self._on_mouse_move_passive)

        glutIdleFunc(self._idle)
        glutVisibilityFunc(self._visible)

        # Load 3D objects

        _cozmo_obj = LoadedObjFile("cozmo.obj")
        self.cozmo_object = RenderableObject(_cozmo_obj)

        # Load the cubes, reusing the same file geometry for all 3.
        _cube_obj = LoadedObjFile("cube.obj")
        self.cube_objects.append(RenderableObject(_cube_obj))
        self.cube_objects.append(RenderableObject(_cube_obj, override_mtl=LoadMtlFile("cube2.mtl")))
        self.cube_objects.append(RenderableObject(_cube_obj, override_mtl=LoadMtlFile("cube3.mtl")))

        self.unit_cube = _make_unit_cube()

        if self.viewer_window:
            self.viewer_window.init_display()
            glutDisplayFunc(self._display)  # Note: both windows call the same DisplayFunc

    def mainloop(self):
        self.init_display()

        # use a non-blocking update loop if possible to make exit conditions
        # easier (not supported on all GLUT versions).
        if bool(glutCheckLoop):
            while not self._exit_requested:
                glutCheckLoop()
        else:
            # This blocks until quit
            glutMainLoop()

        if self._exit_requested:
            # Pass the keyboard interrupt on to SDK so that it can close cleanly
            raise KeyboardInterrupt

    async def connect(self, sdk_conn):
        sdk_robot = await sdk_conn.wait_for_robot()

        # Note: OpenGL and SDK are on different threads, so we deliberately don't
        # store a reference to the robot here, as we should only access it from
        # events called on the SDK thread (where we can then thread-safely move
        # the data into OpenGL)

        self._robot_state_handler = sdk_robot.world.add_event_handler(
            robot.EvtRobotStateUpdated, self.on_robot_state_update)

        if self.viewer_window is not None:
            # Automatically enable camera stream when viewer window is used.
            sdk_robot.camera.image_stream_enabled = True
            self._image_handler = sdk_robot.world.add_event_handler(
                world.EvtNewCameraImage, self.on_new_camera_image)
        # Automatically enable streaming of the nav memory map when using the
        # viewer (can be overridden by user application after connection).
        sdk_robot.world.request_nav_memory_map(0.5)
        self._nav_map_handler = sdk_robot.world.add_event_handler(
            nav_memory_map.EvtNewNavMemoryMap, self.on_new_nav_memory_map)

    def disconnect(self):
        """Called from the SDK when the program is complete and it's time to exit."""
        if self._image_handler:
            self._image_handler.disable()
            self._image_handler = None
        if self._nav_map_handler:
            self._nav_map_handler.disable()
            self._nav_map_handler = None
        if self._robot_state_handler:
            self._robot_state_handler.disable()
            self._robot_state_handler = None
        if not self._exit_requested:
            self._request_exit()

    def _update_robot_remote_control(self, robot):
        # Called on SDK thread, for controlling robot from input intents
        # pushed from the OpenGL thread.
        try:
            input_intents = self._input_intent_queue.popleft()  # type: RobotControlIntents
        except IndexError:
            # no new input intents - do nothing
            return

        # Track last-used intents so that we only issue motor controls
        # if different from the last frame (to minimize it fighting with an SDK
        # program controlling the robot):
        old_intents = self._last_robot_control_intents
        self._last_robot_control_intents = input_intents

        if robot.is_on_charger:
            # Cozmo is stuck on the charger
            if input_intents.left_wheel_speed > 0 and input_intents.right_wheel_speed > 0:
                # User is trying to drive forwards (off the charger) - issue an explicit drive off action
                try:
                    # don't wait for action to complete
                    robot.drive_off_charger_contacts(in_parallel=True)
                except RobotBusy:
                    # Robot is busy doing another action - try again next time we get a drive impulse
                    pass

        if ((old_intents.left_wheel_speed != input_intents.left_wheel_speed) or
            (old_intents.right_wheel_speed != input_intents.right_wheel_speed)):
            robot.drive_wheel_motors(input_intents.left_wheel_speed,
                                     input_intents.right_wheel_speed,
                                     input_intents.left_wheel_speed * 4,
                                     input_intents.right_wheel_speed * 4)

        if (old_intents.lift_speed != input_intents.lift_speed):
            robot.move_lift(input_intents.lift_speed)

        if (old_intents.head_speed != input_intents.head_speed):
            robot.move_head(input_intents.head_speed)

    def on_robot_state_update(self, evt, *, robot, **kw):
        # Called from SDK whenever the robot state is updated (so i.e. every engine tick).
        # Note: This is called from the SDK thread, so only access safe things
        # We can safely capture any robot and world state here, and push to OpenGL
        # (main) thread via a thread-safe queue.
        world_frame = WorldRenderFrame(robot)
        self._world_frame_queue.append(world_frame)

        # We update remote control of the robot here too as it's the one
        # method that's called frequently on the SDK thread.
        self._update_robot_remote_control(robot)

    def on_new_camera_image(self, evt, *, image, **kw):
        # Called from SDK whenever a new image is available
        # Note: This is called from the SDK thread, so only access safe things:
        # viewer_window will already be created, and reading width/height is safe
        # (worst case it'll be a frame old, or e.g just width/height updated)
        fit_size=(self.viewer_window.width, self.viewer_window.height)
        annotated_image = image.annotate_image(fit_size=fit_size)
        self._img_queue.append(annotated_image)

    def on_new_nav_memory_map(self, evt, *, nav_memory_map, **kw):
        # Called from SDK whenever a new nav memory map is available
        # Note: This is called from the SDK thread, so only access safe things
        self._nav_memory_map_queue.append(nav_memory_map)