Play chase with your YetiBorg v2

Robots can make really fun pets and YetiBorg v2 is no exception. The best thing about pets is being able to play games and teach tricks, so we thought we would teach YetiBorg v2 how to follow us like a dog :)

With this example you can see how we can get your robot to follow you around using only the camera.

Parts

All we need for this script to work is:

• A YetiBorg v2
• A Raspberry Pi camera so the robot can see
• Someone to chase :P

How does it work?

The script works by taking images from the camera and looking to see if there is any motion. It does this by taking a pair of images and looking at how they are different. For example:

The left image is first, followed by the middle image. The right image is what we get when we take the difference between the two images. We see black where there are no changes.

When we see enough change we get YetiBorg v2 to move forward. In order to decide which way we are turning we take the same difference image and slice it up into sections.

On the left we have the difference image. In the middle we have sliced up the image into our sections. On the right we have chosen the section with the most differences and decided to aim for that. The further it is from the center the faster we will turn left or right.

This all sounds good, but what happens once we are already moving?

As you can see we get lots of movement and YetiBorg v2 will get confused :(

We have a way to fix this though. If you look at the first image you can see movement only in some sections. When YetiBorg v2 is driving we see movement in all sections. What we do then is take the average amount of change in all sections and use that as our baseline for no movement. Now he will not drive from the above images.

But does this still work when both moving and following someone?

It does, the robot can still see a much larger difference where someone is moving then it sees for the movement of the background.

Tweaking the behaviour

Like any good dog, YetiBorg v2 will often become distracted by other things and forget whom to follow. We can improve the behaviour by tweaking some of the settings used for detecting movement in the "Auto drive settings" section.

• autoZoneCount
  This is the number of slices the image is split into for detection. Either too high or too low can cause detection problems. We have found a value somewhere between 40 and 120 works best.
• autoMinimumMovement
  Changing this value will adjust how much motion is needed before YetiBorg v2 starts chasing. If the robot does not follow you at all try turning this down. If the robot chases inanimate objects a lot (chairs, tables, et cetera) you may need to turn it up.
• steeringGain
  This controls how fast YetiBorg v2 will turn to face movement. Smaller values may not react fast enough and larger values will often case the robot to wag instead of follow :) Turn this up if you cannot get the robot to keep up with your movement.
• flippedImage
  If the robot seems to be running away from movement he may have is camera flipped upside-down. Swap this setting between True and False to change which way up the images are to fix the problem.

There are also some other things which cannot be fixed by tweaking some numbers:

• The camera has a narrow field of view, making it easy to hide from YetiBorg v2 by standing too far left or right.
• Like a T-Rex YetiBorg v2 only sees movement. If you stand perfectly still you will be invisible :)
• Walking pace is best, very fast movements can also be missed.
• May not play well with other pets. You have been warned :D
• Works best in a well lit area. The camera cannot see much change with someone dressed in black in a dark room :(
• Very tall objects may still attract attention, such as lamp posts...

Get the example

The example is part of the standard set of YetiBorg v2 examples installed during the getting started instructions: bash <(curl https://www.piborg.org/installer/install-yetiborg-v2.txt)

Run once

Go to the YetiBorg v2 code directory: cd ~/yetiborgv2 and run the script using: ./yeti2FollowMe.py

Run at startup

Open /etc/rc.local to make an addition using: sudo nano /etc/rc.local Then add this line just above the exit 0 line: /home/pi/yetiborgv2/yeti2FollowMe.py & Finally press CTRL+O, ENTER to save the file followed by CTRL+X to exit nano. Next time you power up the Raspberry Pi it should start the script for you :)

Full code listing - yeti2FollowMe.py

#!/usr/bin/env python
# coding: Latin-1

# Load library functions we want
import time
import os
import sys
import ZeroBorg
import io
import threading
import picamera
import picamera.array
import cv2
import numpy

# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
print 'Libraries loaded'

# Global values
global running
global ZB
global camera
global processor
global motionDetected
running = True
motionDetected = False

# Setup the ZeroBorg
ZB = ZeroBorg.ZeroBorg()
#ZB.i2cAddress = 0x44                  # Uncomment and change the value if you have changed the board address
ZB.Init()
if not ZB.foundChip:
    boards = ZeroBorg.ScanForZeroBorg()
    if len(boards) == 0:
        print 'No ZeroBorg found, check you are attached :)'
    else:
        print 'No ZeroBorg at address %02X, but we did find boards:' % (ZB.i2cAddress)
        for board in boards:
            print '    %02X (%d)' % (board, board)
        print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
        print 'ZB.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#ZB.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    ZB.SetCommsFailsafe(True)
    failsafe = ZB.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (ZB.i2cAddress)
    sys.exit()
ZB.ResetEpo()

# Power settings
voltageIn = 8.4                         # Total battery voltage to the ZeroBorg (change to 9V if using a non-rechargeable battery)
voltageOut = 6.0                        # Maximum motor voltage

# Camera settings
imageWidth  = 320                       # Camera image width
imageHeight = 240                       # Camera image height
frameRate = 10                          # Camera image capture frame rate

# Auto drive settings
autoZoneCount = 80                      # Number of detection zones, higher is more accurate
autoMinimumMovement = 20                # Minimum movement detection before driving
steeringGain = 4.0                      # Use to increase or decrease the amount of steering used
flippedImage = True                     # True if the camera needs to be rotated
showDebug = True                        # True to display detection values

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Calculate the nearest zoning which fits
zones = range(0, imageWidth, imageWidth / autoZoneCount)
zoneWidth = zones[1]
zoneCount = len(zones)

# Image stream processing thread
class StreamProcessor(threading.Thread):
    def __init__(self):
        super(StreamProcessor, self).__init__()
        self.stream = picamera.array.PiRGBArray(camera)
        self.event = threading.Event()
        self.lastImage = None
        self.terminated = False
        self.reportTick = 0
        self.start()
        self.begin = 0

    def run(self):
        # This method runs in a separate thread
        while not self.terminated:
            # Wait for an image to be written to the stream
            if self.event.wait(1):
                try:
                    # Read the image and do some processing on it
                    self.stream.seek(0)
                    self.ProcessImage(self.stream.array)
                finally:
                    # Reset the stream and event
                    self.stream.seek(0)
                    self.stream.truncate()
                    self.event.clear()

    # Image processing function
    def ProcessImage(self, image):
        # Flip the image if needed
        if flippedImage:
            image = cv2.flip(image, -1)
        # If this is the first image store and move on
        if self.lastImage is None:
            self.lastImage = image.copy()
            return
        # Work out the difference from the last image
        imageDiff = cv2.absdiff(self.lastImage, image)
        # Build up the zone change levels
        zoneDetections = []
        for zone in zones:
            # Grab the zone from the differences
            zoneDiff = imageDiff[:, zone : zone + zoneWidth, :]
            # Get an average for the zone
            zoneChange = zoneDiff.mean()
            zoneDetections.append(zoneChange)
        # Set drives or report motion status
        self.SetSpeedFromDetection(zoneDetections)
        # Save the previous image
        self.lastImage = image.copy()

    # Set the motor speed from the motion detection
    def SetSpeedFromDetection(self, zoneDetections):
        global ZB
        global motionDetected
        # Find the largest and average detections
        largestZone = 0
        largestDetection = 0
        averageDetection = 0
        for i in range(zoneCount):
            if zoneDetections[i] > largestDetection:
                largestZone = i
                largestDetection = zoneDetections[i]
            averageDetection += zoneDetections[i]
        averageDetection /= float(zoneCount)
        # Remove the baseline motion from the largest zone
        detection = largestDetection - averageDetection
        # Determine if the motion is strong enough to count as a detection
        if detection > autoMinimumMovement:
            # Motion detected
            motionDetected = True
            if showDebug:
                if self.reportTick < 2:
                    print 'MOVEMENT   %05.2f [%05.2f %05.2f]' % (detection, largestDetection, averageDetection)
                    print '           Zone %d of %d' % (largestZone + 1, zoneCount)
                    self.reportTick = frameRate
                else:
                    self.reportTick -= 1
            # Calculate speeds based on zone
            steering = ((2.0 * largestZone) / float(zoneCount - 1)) - 1.0
            steering *= steeringGain
            if steering < 0.0:
                # Steer to the left
                driveLeft = 1.0 + steering
                driveRight = 1.0
                if driveLeft <= 0.05:
                    driveLeft = 0.05
            else:
                # Steer to the right
                driveLeft = 1.0
                driveRight = 1.0 - steering
                if driveRight <= 0.05:
                    driveRight = 0.05
        else:
            # No motion detected
            motionDetected = False
            if showDebug:
                if self.reportTick < 2:
                    print '--------   %05.2f [%05.2f %05.2f]' % (detection, largestDetection, averageDetection)
                    self.reportTick = frameRate
                else:
                    self.reportTick -= 1
            # Stop moving
            driveLeft  = 0.0
            driveRight = 0.0
        # Set the motors
        ZB.SetMotor1(-driveRight * maxPower) # Rear right
        ZB.SetMotor2(-driveRight * maxPower) # Front right
        ZB.SetMotor3(-driveLeft  * maxPower) # Front left
        ZB.SetMotor4(-driveLeft  * maxPower) # Rear left

# Image capture thread
class ImageCapture(threading.Thread):
    def __init__(self):
        super(ImageCapture, self).__init__()
        self.start()

    def run(self):
        global camera
        global processor
        print 'Start the stream using the video port'
        camera.capture_sequence(self.TriggerStream(), format='bgr', use_video_port=True)
        print 'Terminating camera processing...'
        processor.terminated = True
        processor.join()
        print 'Processing terminated.'

    # Stream delegation loop
    def TriggerStream(self):
        global running
        while running:
            if processor.event.is_set():
                time.sleep(0.01)
            else:
                yield processor.stream
                processor.event.set()

# Startup sequence
print 'Setup camera'
camera = picamera.PiCamera()
camera.resolution = (imageWidth, imageHeight)
camera.framerate = frameRate
imageCentreX = imageWidth / 2.0
imageCentreY = imageHeight / 2.0

print 'Setup the stream processing thread'
processor = StreamProcessor()

print 'Wait ...'
time.sleep(2)
captureThread = ImageCapture()

try:
    print 'Press CTRL+C to quit'
    ZB.MotorsOff()
    # Loop indefinitely
    while running:
        # # Change the LED to show if we have detected motion
        # We do this regularly to keep the communications failsafe test happy
        ZB.SetLed(motionDetected)
        # Wait for the interval period
        time.sleep(0.1)
    # Disable all drives
    ZB.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    print '
User shutdown'
    ZB.MotorsOff()
except:
    # Unexpected error, shut down!
    e = sys.exc_info()[0]
    print
    print e
    print '
Unexpected error, shutting down!'
    ZB.MotorsOff()
# Tell each thread to stop, and wait for them to end
running = False
captureThread.join()
processor.terminated = True
processor.join()
del camera
ZB.SetLed(False)
print 'Program terminated.'