Including Imaginative and prescient to the SunFounder PiCar-X
Last time, we programmed a SunFounder PiCar-X to behave just like a Roomba, to principally transfer round a room following an algorithm to go far and wide. This was a primary step and had a number of limitations. The principle one is that it may simply get caught, since if there’s nothing in entrance of the ultrasonic sensor, then it doesn’t detect it’s caught. This weblog publish provides some primary picture processing to the algorithm, so if two consecutive photographs from the digital camera are the identical, then it considers itself caught and can attempt to extricate itself.
The entire program itemizing is on the finish of this posting.
Vilib is a Python library offered by SunFounder that wraps a group of decrease stage libraries making it simpler to program the PiCar-X. This library consists of routines to manage the digital camera, together with plenty of AI algorithms to detect objects, acknowledge faces and acknowledge gestures. These are carried out as Tensorflow Lite fashions. In our case, we’ll use Vilib to take footage, then we’ll use OpenCV, the open supply pc imaginative and prescient library to check the pictures.
To make use of the digital camera, it is advisable import the Vilib library, begin the digital camera after which you’ll be able to take footage or video.
from vilib import Vilib
Vilib.camera_start(vflip=False,hflip=False)
Vilib.take_photo(title, path)
Many of the code is to construct the title and path to save lots of the file. The code makes use of the strftime routine so as to add the time to the file title. The decision of this routine is seconds, so you must watch out to not take footage lower than a second aside or the easy algorithm will get confused.
To match two consecutive photographs, we use some code from this Tutorialspoint article by Shahid Akhtar Khan. For the reason that motor is operating, the PiCar-X is vibrating and bouncing a bit, so the pictures gained’t be precisely the identical. This algorithm hundreds the 2 most up-to-date photographs and converts them to grayscale. It then subtracts the 2 photographs, if they’re precisely the identical then the consequence will likely be all zeros. Nevertheless this can by no means be the precise case. What we do is calculate the mean square error (MSE) after which examine that to a MSE_THRESHOLD worth, which from experimentation, we decided a price of 20 appeared to work properly. Calculating MSE isn’t a part of OpenCV and we use NumPy straight to do that.
Final week’s model of this program didn’t have any error dealing with. One drawback was that the studying the ultrasonic sensor failed on occasion returning -1, which might set off the automotive to imagine it was caught and backup unnecessarily. This system now checks for -1. Equally taking an image with the digital camera doesn’t at all times work, so it must examine if the returned picture is None. Unusually from time to time the dimensions of the image returned is completely different inflicting the subtract name to fail, that is dealt with by placing it in a attempt/besides block to catch the error. Anyway, error dealing with is essential and when coping with {hardware} units, they do occur and have to be dealt with.
I left the checks for getting caught by way of the ultrasonic sensor in place. In the principle loop within the foremost routine, this system takes an image on the high, executes a state after which compares the images on the finish. This appears to work pretty properly. It generally takes a little bit of time for the automotive to get sufficiently caught that the images are shut sufficient to depend as the identical. As an illustration when a wheel catches a chair leg, it’s going to swing round a bit, till it will get caught good after which the images would be the identical and it will possibly again out. The automotive now appears to go additional and will get actually caught in fewer locations, so an enchancment, although not excellent.
Enjoying with programming the PiCar-X is enjoyable, most issues work fairly simply. I discover I do most coding with the wheels lifted off the bottom, linked to a monitor and keyboard, so I can debug in Thonny. Utilizing the Vilib Python library makes life simple, plus you’ve the supply code, so you need to use it for example of utilizing the related 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.form
attempt:
diff = cv2.subtract(img1, img2)
besides:
return(0)
err = np.sum(diff**2)
mse = err/(float(h*w))
print("comp mse = ", mse)
return mse
def take_photo():
international lastPhoto, currentPhoto
_time = time.strftime('%Y-%m-%d-%H-%M-%S',time.localtime(time.time()))
title="photo_percents"%_time
username = os.getlogin()
path = f"/house/{username}/Photos/"
Vilib.take_photo(title, path)
print('picture save as %spercents.jpg'%(path,title))
if lastPhoto != "":
os.take away(lastPhoto)
lastPhoto = currentPhoto
currentPhoto = path + title + ".jpg"
def executeSpiral(px):
international state, spiralAngle
px.set_dir_servo_angle(spiralAngle)
px.ahead(POWER)
time.sleep(1.2)
spiralAngle = spiralAngle - 5
if spiralAngle < 5:
spiralAngle = 40
distance = spherical(px.ultrasonic.learn(), 2)
print("spiral distance: ",distance)
if distance <= foundObstacle and distance != -1:
state = BACKANDFORTH
def executeUnskick(px):
international 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):
international state
distance = spherical(px.ultrasonic.learn(), 2)
print("forwards and backwards distance: ",distance)
if distance >= StartTurn or distance == -1:
px.set_dir_servo_angle(0)
px.ahead(POWER)
time.sleep(1.2)
elif distance < StuckDist:
state = STUCK
time.sleep(1.2)
else:
px.set_dir_servo_angle(40)
px.ahead(POWER)
time.sleep(5)
time.sleep(0.5)
def foremost():
international state
attempt:
px = Picarx()
px.set_cam_tilt_angle(-90)
Vilib.camera_start(vflip=False,hflip=False)
whereas True:
take_photo()
if state == SPIRAL:
executeSpiral(px)
elif state == BACKANDFORTH:
executeBackandForth(px)
elif state == STUCK:
executeUnskick(px)
if compareImages() < MSE_THRESHOLD:
state = STUCK
lastly:
px.ahead(0)
if __name__ == "__main__":
foremost()