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.
In order to run this script you will only need:
How the script works
What the script does is actually fairly simple, the sequence breaks down to this:
- Get a new image from the camera
- Find a red ball in the image
- Determine a steering angle based on where the ball is
- Select a speed based on how far away the ball is
- 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
autoFullSpeedAreait is far away, so we go at full speed (
- If the ball is larger than
autoMaxAreait is very close, so we stop moving :)
- Anything else we determine the speed as a value between
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
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)
Go to the DiddyBorg v2 code directory:
cd ~/diddyborgv2 and run the script:
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) 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 area = ball 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 '\nUser shutdown' TB.MotorsOff() except: # Unexpected error, shut down! e = sys.exc_info() print print e print '\nUnexpected 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.'