Adding Vision to the SunFounder PiCar-X

Introduction

Last time, we programmed a SunFounder PiCar-X to behave similar to a Roomba, to basically move around a room following an algorithm to go all over the place. This was a first step and had a few limitations. The main one is that it could easily get stuck, since if there is nothing in front of the ultrasonic sensor, then it doesn’t detect it is stuck. This blog post adds some basic image processing to the algorithm, so if two consecutive images from the camera are the same, then it considers itself stuck and will try to extricate itself.

The complete program listing is at the end of this posting.

Using Vilib

Vilib is a Python library provided by SunFounder that wraps a collection of lower level libraries making it easier to program the PiCar-X. This library includes routines to control the camera, along with a number of AI algorithms to detect objects, recognize faces and recognize gestures. These are implemented as Tensorflow Lite models. In our case, we’ll use Vilib to take pictures, then we’ll use OpenCV, the open source computer vision library to compare the images.

To use the camera, you need to import the Vilib library, start the camera and then you can take pictures or video. 

from vilib import Vilib
Vilib.camera_start(vflip=False,hflip=False)
Vilib.take_photo(name, path)

Most of the code is to build the name and path to save the file. The code uses the strftime routine to add the time to the file name. The resolution of this routine is seconds, so you have to be careful not to take pictures less than a second apart or the simple algorithm will get confused.

Using OpenCV

To compare two consecutive images, we use some code from this Tutorialspoint article by Shahid Akhtar Khan. Since the motor is running, the PiCar-X is vibrating and bouncing a bit, so the images won’t be exactly the same. This algorithm loads the two most recent images and converts them to grayscale. It then subtracts the two images, if they are exactly the same then the result will be all zeros. However this will never be the exact case. What we do is calculate the mean square error (MSE) and then compare that to a MSE_THRESHOLD value, which from experimentation, we determined a value of 20 seemed to work well. Calculating MSE isn’t part of OpenCV and we use NumPy directly to do this.

Error Handling

Last week’s version of this program didn’t have any error handling. One problem was that the reading the ultrasonic sensor failed now and again returning -1, which would trigger the car to assume it was stuck and backup unnecessarily. The program now checks for -1. Similarly taking a picture with the camera doesn’t always work, so it needs to check if the returned image is None. Strangely every now and then the size of the picture returned is different causing the subtract call to fail, this is handled by putting it in a try/except block to catch the error. Anyway, error handling is important and when dealing with hardware devices, they do happen and need to be handled.

Operation

I left the checks for getting stuck via the ultrasonic sensor in place. In the main loop in the main routine, the program takes a picture at the top, executes a state and then compares the pictures at the end. This seems to work fairly well. It sometimes takes a bit of time for the car to get sufficiently stuck that the pictures are close enough to count as the same. For instance when a wheel catches a chair leg, it will swing around a bit, until it gets stuck good and then the pictures will be the same and it can back out. The car now seems to go further and gets really stuck in fewer places, so an improvement, though not perfect.

Summary

Playing with programming the PiCar-X is fun, most things work pretty easily. I find I do most coding with the wheels lifted off the ground, connected to a monitor and keyboard, so I can debug in Thonny. Using the Vilib Python library makes life easy, plus you have the source code, so you can use it as an example of using the associated libraries like picamera and OpenCV.

from picarx import Picarx
import time
import random
from vilib import Vilib
import os
import cv2
import numpy as np

POWER = 20
SPIRAL = 1
BACKANDFORTH = 2
STUCK = 3
state = SPIRAL
StartTurn = 80
foundObstacle = 40
StuckDist = 10
spiralAngle = 40
lastPhoto = ""
currentPhoto = ""
MSE_THRESHOLD = 20

def compareImages():
    if lastPhoto == "":
        return 0
    img1 = cv2.imread(lastPhoto)
    img2 = cv2.imread(currentPhoto)
    if img1 is None or img2 is None:
        return(MSE_THRESHOLD + 1) 
    img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
    img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
    h, w = img1.shape
    try:
        diff = cv2.subtract(img1, img2)
    except:
        return(0)
    err = np.sum(diff**2)
    mse = err/(float(h*w))
    print("comp mse = ", mse)
    return mse    

def take_photo():
    global lastPhoto, currentPhoto
    _time = time.strftime('%Y-%m-%d-%H-%M-%S',time.localtime(time.time()))
    name = 'photo_%s'%_time
    username = os.getlogin()
    path = f"/home/{username}/Pictures/"
    Vilib.take_photo(name, path)
    print('photo save as %s%s.jpg'%(path,name))
    if lastPhoto != "":
        os.remove(lastPhoto)
    lastPhoto = currentPhoto
    currentPhoto = path + name + ".jpg"

def executeSpiral(px):
    global state, spiralAngle
    px.set_dir_servo_angle(spiralAngle)
    px.forward(POWER)
    time.sleep(1.2)
    spiralAngle = spiralAngle - 5
    if spiralAngle < 5:
        spiralAngle = 40
    distance = round(px.ultrasonic.read(), 2)
    print("spiral distance: ",distance)
    if distance <= foundObstacle and distance != -1:
        state = BACKANDFORTH

def executeUnskick(px):
    global state    
    print("unskick backing up")
    px.set_dir_servo_angle(random.randint(-50, 50))
    px.backward(POWER)
    time.sleep(1.2)
    state = SPIRAL                    

def executeBackandForth(px):
    global state    
    distance = round(px.ultrasonic.read(), 2)
    print("back and forth distance: ",distance)
    if distance >= StartTurn or distance == -1:
        px.set_dir_servo_angle(0)
        px.forward(POWER)
        time.sleep(1.2)
    elif distance < StuckDist:
        state = STUCK
        time.sleep(1.2)
    else:
        px.set_dir_servo_angle(40)
        px.forward(POWER)
        time.sleep(5)
    time.sleep(0.5)                

def main():
    global state
    try:
        px = Picarx()
        px.set_cam_tilt_angle(-90)        
        Vilib.camera_start(vflip=False,hflip=False)
        while True:
            take_photo()
            if state == SPIRAL:               
                executeSpiral(px)
            elif state == BACKANDFORTH:
                executeBackandForth(px)
            elif state == STUCK:
                    executeUnskick(px)
            if compareImages() < MSE_THRESHOLD:
                state = STUCK                    
    finally:
        px.forward(0)
if __name__ == "__main__":
    main()