/demo_face.py

#!/usr/bin/env python3
'''
Cozmo will recognize new faces and old faces and try to follow the faces.
Cozmo will display these on the screen.
Currently, these actions are done on the same thread, which slows cozmo down.
'''

import sys
import time
import asyncio

try:
    from PIL import ImageDraw, ImageFont
except ImportError:
    sys.exit('run `pip3 install --user Pillow numpy` to run this example')

import cozmo


# Define an annotator using the annotator decorator
@cozmo.annotate.annotator
def clock(image, scale, annotator=None, world=None, **kw):
    d = ImageDraw.Draw(image)
    bounds = (0, 0, image.width, image.height)
    text = cozmo.annotate.ImageText(time.strftime("%H:%m:%S"),
            position=cozmo.annotate.TOP_LEFT)
    text.render(d, bounds)

# Define another decorator as a subclass of Annotator
class Battery(cozmo.annotate.Annotator):
    def apply(self, image, scale):
        d = ImageDraw.Draw(image)
        bounds = (0, 0, image.width, image.height)
        batt = self.world.robot.battery_voltage
        text = cozmo.annotate.ImageText('BATT %.1fv' % batt, color='green')
        text.render(d, bounds)


def cozmo_program(robot: cozmo.robot.Robot):
    robot.world.image_annotator.add_static_text('text', 'Coz-Cam', position=cozmo.annotate.TOP_RIGHT)
    robot.world.image_annotator.add_annotator('clock', clock)
    robot.world.image_annotator.add_annotator('battery', Battery)
    robot.world.image_annotator.annotation_enabled = True

    # Move lift down and tilt the head up
    robot.move_lift(-3)
    robot.set_head_angle(cozmo.robot.MAX_HEAD_ANGLE).wait_for_completed()

    faces = []
    current_face = None
    light_colors = [cozmo.lights.green_light,
                    cozmo.lights.blue_light,
                    cozmo.lights.red_light]
    print("Press CTRL-C to quit")
    while True:
        turn_action= None
        if len(faces) > 0:
            found_face = False
            for face, light in zip(faces, light_colors):
                if face and face.is_visible:
                    found_face = True
                    robot.set_all_backpack_lights(light)
                    # start turning towards the face
                    turn_action = robot.turn_towards_face(face)
            if not found_face:
                robot.set_backpack_lights_off()
                # Wait until we we can see another face
                try:
                    face = None
                    face = robot.world.wait_for_observed_face(timeout=30)
                    if face in faces:
                        robot.say_text("Hello again!").wait_for_completed()
                        turn_action = robot.turn_towards_face(face)
                    else:
                        faces.append(face)
                        robot.say_text("Hello New Person!").wait_for_completed()
                except asyncio.TimeoutError:
                    print("Didn't find a face.")
                    return
        else:
            robot.set_backpack_lights_off()
            # Wait until we we can see another face
            try:
                face = None
                face = robot.world.wait_for_observed_face(timeout=30)
                if face in faces:
                    robot.say_text("Hello again!").wait_for_completed()
                    turn_action = robot.turn_towards_face(face)
                else:
                    faces.append(face)
                    robot.say_text("Hello New Person!").wait_for_completed()
            except asyncio.TimeoutError:
                print("Didn't find a face.")
                return
        if turn_action:
            # Complete the turn action if one was in progress
            turn_action.wait_for_completed()

        time.sleep(.1)


    robot.world.image_annotator.disable_annotator('faces')



cozmo.run_program(cozmo_program, use_viewer=True, force_viewer_on_top=True)