Play fetch with your DiddyBorg v2

Pets can be great fun, so how about a pet robot? All we need to do is get DiddyBorg v2 to behave like a pet and we can have hours of fun watching him play :)

This example shows how we can make use of OpenCV and the Pi camera to get DiddyBorg v2 to chase a red ball.

Parts

In order to run this script you will only need:

• A DiddyBorg v2 with any top plate you like
• A Raspberry Pi camera so the robot can see

How the script works

What the script does is actually fairly simple, the sequence breaks down to this:

1. Get a new image from the camera
2. Find a red ball in the image
3. Determine a steering angle based on where the ball is
4. Select a speed based on how far away the ball is
5. Update the motor speeds

This gets repeated several times a second so that DiddyBorg v2 responds to both the movement of the ball and his own movement towards the ball.

Detecting the ball

Image processing can be hard to understand in words, instead it is easier to explain using pictures :)
The first thing we need to do is get the camera image from the Pi camera.
We do this several times a second so we can track the ball as it is moving.
Next we blur the image a bit. This removes unneeded small details which can confuse things.
The items on the shelf in the background are now hard to see but the ball looks basically the same as before.
After that we convert the image into hue, saturation, and value (HSV) format. This is easier for the Pi to process as it makes the colour easier to detect. Actual HSV images look the same as normal (RGB) images, so this is the HSV data shown as though it were RGB instead.

The level of red shows the colour, the level of green is how far from grey it is, and the level of blue is how bright things are. You can see that the ball is nice and distinctive.
Now we look for any colour which might be our red ball. This becomes what is known as a mask.
In the mask the white sections are the bits we want. Notice how the bright spot on the right side of the ball is excluded as not red enough!
We then take the mask and get OpenCV to get a list of contours around each section. I have then drawn these over the blurred image from earlier.

As you can see there are a lot of them, some very small.
The last step is to find the largest contour in terms of size. This removes all of the small areas which are not actually a ball. Here is the largest contour on its own.

You can see how DiddyBorg v2 could get confused by other red objects as well. If the wall was closer he could mistake it for the ball instead!
Now we know where the ball is we know if we need to turn towards it or keep going straight on. We also make a note of the size of the ball for later.

Deciding how fast to move

If you have tried the example already you may have noticed that DiddyBorg v2 slows down as he gets close to the ball and stops when he is touching or nearly touching. So why does this happen? When we found the ball earlier we made a note of how big the detected area was in the image. This is approximately the same as how big the ball is in the image. When the ball is further away it looks smaller, when it is closer it looks bigger. The movement speed is worked out based on the size the ball appears to be:

• If the ball is too small (less than autoMinArea) we cannot really be sure it is a ball, so we do not move
• If the ball is smaller than autoFullSpeedArea it is far away, so we go at full speed (autoMaxPower)
• If the ball is larger than autoMaxArea it is very close, so we stop moving :)
• Anything else we determine the speed as a value between autoMaxPower and autoMinPower. The closer we are the lower the speed used

If you use a ball which is somewhat smaller or bigger than ours you may need to change these values to get him to behave right. Our ball is about 65 mm in diameter. For smaller balls reduce the autoFullSpeedArea and autoMaxArea values, larger balls will probably want larger values.

Get the example

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

Run once

Go to the DiddyBorg v2 code directory: cd ~/diddyborgv2 and run the script: ./diddy2FollowBall.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/diddyborgv2/diddy2FollowBall.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 - diddy2FollowBall.py

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

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

print 'Libraries loaded'

# Global values
global running
global TB
global camera
global processor
running = True

# Setup the ThunderBorg
TB = ThunderBorg.ThunderBorg()
#TB.i2cAddress = 0x15                  # Uncomment and change the value if you have changed the board address
TB.Init()
if not TB.foundChip:
    boards = ThunderBorg.ScanForThunderBorg()
    if len(boards) == 0:
        print 'No ThunderBorg found, check you are attached :)'
    else:
        print 'No ThunderBorg at address %02X, but we did find boards:' % (TB.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 'TB.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
TB.SetCommsFailsafe(False)

# Power settings
voltageIn = 12.0                        # Total battery voltage to the ThunderBorg
voltageOut = 12.0 * 0.95                # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power

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

# Auto drive settings
autoMaxPower = 1.0                      # Maximum output in automatic mode
autoMinPower = 0.2                      # Minimum output in automatic mode
autoMinArea = 10                        # Smallest target to move towards
autoMaxArea = 10000                     # Largest target to move towards
autoFullSpeedArea = 300                 # Target size at which we use the maximum allowed output

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

# 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.terminated = False
        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):
        # Get the red section of the image
        image = cv2.medianBlur(image, 5)
        image = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) # Swaps the red and blue channels!
        red = cv2.inRange(image, numpy.array((115, 127, 64)), numpy.array((125, 255, 255)))
        # Find the contours
        contours,hierarchy = cv2.findContours(red, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        # Go through each contour
        foundArea = -1
        foundX = -1
        foundY = -1
        for contour in contours:
            x,y,w,h = cv2.boundingRect(contour)
            cx = x + (w / 2)
            cy = y + (h / 2)
            area = w * h
            if foundArea < area:
                foundArea = area
                foundX = cx
                foundY = cy
        if foundArea > 0:
            ball = [foundX, foundY, foundArea]
        else:
            ball = None
        # Set drives or report ball status
        self.SetSpeedFromBall(ball)

    # Set the motor speed from the ball position
    def SetSpeedFromBall(self, ball):
        global TB
        driveLeft  = 0.0
        driveRight = 0.0
        if ball:
            x = ball[0]
            area = ball[2]
            if area < autoMinArea:
                print 'Too small / far'
            elif area > autoMaxArea:
                print 'Close enough'
            else:
                if area < autoFullSpeedArea:
                    speed = 1.0
                else:
                    speed = 1.0 / (area / autoFullSpeedArea)
                speed *= autoMaxPower - autoMinPower
                speed += autoMinPower
                direction = (x - imageCentreX) / imageCentreX
                if direction < 0.0:
                    # Turn right
                    driveLeft  = speed
                    driveRight = speed * (1.0 + direction)
                else:
                    # Turn left
                    driveLeft  = speed * (1.0 - direction)
                    driveRight = speed
                print '%.2f, %.2f' % (driveLeft, driveRight)
        else:
            print 'No ball'
        TB.SetMotor1(driveLeft)
        TB.SetMotor2(driveRight)

# 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'
    TB.MotorsOff()
    TB.SetLedShowBattery(True)
    # Loop indefinitely until we are no longer running
    while running:
        # Wait for the interval period
        # You could have the code do other work in here :)
        time.sleep(1.0)
    # Disable all drives
    TB.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    print '
User shutdown'
    TB.MotorsOff()
except:
    # Unexpected error, shut down!
    e = sys.exc_info()[0]
    print
    print e
    print '
Unexpected error, shutting down!'
    TB.MotorsOff()
# Tell each thread to stop, and wait for them to end
running = False
captureThread.join()
processor.terminated = True
processor.join()
del camera
TB.MotorsOff()
TB.SetLedShowBattery(False)
TB.SetLeds(0,0,0)
print 'Program terminated.'