1490 lines
58 KiB
Python
1490 lines
58 KiB
Python
# 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)
|