hasselto66176/BachelorarbeitCozmo
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
BachelorarbeitCozmo/Cozmo_LineFollow/venv/Lib/site-packages/cozmo/opengl.py

1490 lines
58 KiB
Python
Raw Normal View History

Add python pycharm project
2019-04-28 11:16:27 +02:00
# 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)
Reference in New Issue Copy Permalink