In our previous lessons, we built individual components. We got our machine to listen locally using Speech-to-Text (STT), we got it to talk back cleanly with Text-to-Speech (TTS), and we learned how to manipulate physical hardware using precision servos. Today, we bridge the gap between software and the physical world. We are building a multi-threaded, autonomous edge system that tracks humanoids in real time.
The Engineering Mindset: Concurrency & Threading
If you try to build a system like this sequentially—running camera capture, object detection, audio listening, and audio speaking in a single while True: loop—your project will fail completely. Why? Because tasks like waiting for a voice command (stt.listen()) or synthesizing a voice line are blocking operations. If your code is stuck waiting for you to finish speaking a command, your camera frames freeze, your servo adjustments drop to zero, and your humanoid target escapes.
To solve this, we architect our software using Multi-threading. We spawn independent execution paths that run concurrently, passing data back and forth safely using thread-safe Queues:
- The Main Thread: Handles the high-speed Picamera2 video capture loop, executes Haar Cascade face/eye calculations, and updates the servo angles via mathematical error tracking.
- The Speak Thread: Idles quietly in the background until a message lands in the
speakQ, instantly triggering the local Piper TTS engine without stalling our camera frame rate. - The Command Thread: Keeps a continuous ear open via our local STT engine. When it parses a voice command like “track”, “release”, or “quit”, it safely pops that token into the
commandQfor the main loop to execute on its next pass.
The Complete Fusion Architecture Code
Below is the complete Python pipeline developed in today’s lesson. Make sure your hardware connections for the pan-and-tilt servos match pins 2 and 3 on your expansion setup, and verify your fusion_hat software stack is completely updated.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 |
import cv2 import time from picamera2 import Picamera2 ######################################### import threading from queue import Queue from fusion_hat.tts import Piper tts = Piper() tts.set_model('en_US-kristin-medium') from fusion_hat.stt import STT stt = STT('en-us') cnt = 0 running = True commandQ = Queue() speakQ = Queue() ########################################### from fusion_hat.servo import Servo panPin = 2 tiltPin = 3 panServo = Servo(panPin) tiltServo = Servo(tiltPin) panAngle = -10 tiltAngle =-20 panServo.angle(panAngle) tiltServo.angle(tiltAngle) piCam = Picamera2() W=1280 H=720 tStart = time.time() fps = 0 RES = (W,H) piCam.preview_configuration.main.size = RES piCam.preview_configuration.main.format = "RGB888" piCam.preview_configuration.controls.FrameRate=60 piCam.preview_configuration.align() piCam.configure("preview") piCam.start() textLowerLeft = (int(W*.01),int(H*.05)) fontFace = cv2.FONT_HERSHEY_SIMPLEX fontThickness = int(W/425) fontScale = H*.0015 fontColor = (0,0,255) faceFinder = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml') eyeFinder = cv2.CascadeClassifier(cv2.data.haarcascades + "haarcascade_eye.xml") track = False xFrameC = int((W-1)/2) yFrameC = int((H-1)/2) xBoxC = xFrameC yBoxC = yFrameC ########################################## def speak(): global running print('Speak Thread Running') while running: if speakQ.empty() == False: txt = speakQ.get() print(txt) tts.say(txt) print('Speak Thread is Terminated') print('') def getCommand(): print('Get Command Thread is Running') global running while running: myCommand = stt.listen(stream = False) myCommand = myCommand.strip() print(myCommand) if myCommand == 'quit': msg = 'Project Humanoid Elimination Terminated. Have a Nice Day.' speakQ.put(msg) running = False break commandQ.put(myCommand) print('Get Command Thread Terminated') print('') speakThread = threading.Thread(target=speak,daemon=True) speakThread.start() commandThread = threading.Thread(target=getCommand,daemon=True) commandThread.start() print('Main Program Has Commenced') msg = 'Hello, I am Ready to Eliminate Humanoids' speakQ.put(msg) msgOld = 'Hello, I am Ready to Eliminate Humanoids' cv2.namedWindow('Camera',cv2.WINDOW_GUI_NORMAL) cv2.moveWindow('Camera',0,65) cv2.resizeWindow('Camera',W,H) ############################################ while running: deltaT = time.time() - tStart tStart=time.time() fps = fps*.95 + (1/deltaT)*.05 frame= piCam.capture_array() frame=cv2.flip(frame,-1) if commandQ.empty() == False: myCommand = commandQ.get() if myCommand == 'track': track = True if myCommand == 'release': track = False gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY) faces = faceFinder.detectMultiScale(gray,scaleFactor=1.1, minNeighbors =5, minSize=(100,100)) if len(faces) != 0: cnt = 0 msg = "Humanoid Detected, Shall I track?" if len(faces) == 0: cnt = cnt + 1 if cnt == 25: msg = "No Humanoid Detected, Continuing to Scan" if msgOld != msg: speakQ.put(msg) msgOld = msg for (x,y,w,h) in faces: cv2.rectangle(frame, (x,y),(x+w,y+h),(0,255,255),3) xBoxC = x + int(w/2) yBoxC = y + int(h/2) xError = xBoxC - xFrameC yError = yBoxC - yFrameC eyes =eyeFinder.detectMultiScale(gray[y:y+h,x:x+w],scaleFactor=1.1,minNeighbors=5,minSize=(15,15)) for (i,j,w,h) in eyes: cv2.rectangle(frame,(x+i,y+j),(x+i+w,y+j+h),(255,0,0),3) if track == True and len(faces)>0: panAngle = panAngle - xError/50/2 panServo.angle(int(panAngle)) #time.sleep(.02) tiltAngle = tiltAngle + yError/50/2 tiltServo.angle(int(tiltAngle)) #time.sleep(.02) myText = "FPS: "+str(round(fps,1)) cv2.putText(frame,myText,textLowerLeft,fontFace,fontScale,fontColor,fontThickness) cv2.imshow("Camera", frame) cv2.moveWindow("Camera",0,60) if cv2.waitKey(1)==ord('q'): break cv2.destroyAllWindows() print('') print('Program Terminated') |
Breaking Down the Math & Logic
Pro-Tip on State Debouncing: Notice how we handle messages using the
cntvariable and themsgOldbuffer. If we simply asked the system to say “Humanoid Detected” every single time the frame loops, the TTS engine would crash from queue overflow, stuttering continuously. By checking ifmsgOld != msg, we ensure the system speaks a notification exactly once upon transition. The 25-frame hysteresis buffer ensures that a single missed frame doesn’t cause a false “No Humanoid” trigger.
Proportional Error Tracking
Look carefully at how the servo angles are adjusted:
|
1 |
panAngle = panAngle - xError/50/2 |
We are calculating the error vector—how many pixels the center of the bounding box (xBoxC) is from the absolute center of our camera stream (xFrameC). Instead of moving the servo by a fixed step, we move it proportionally to the size of the error. Big error? Fast movement. Small error? Gentle, microscopic correction. Dividing by 50 dampens our proportional gain loop so our servos don’t violently oscillate and throw our camera system completely out of alignment.
Your Homework Assignment
You have the system reading frames, listening asynchronously, and adjusting angles proportionally. Now it’s time to earn your stripes as an edge systems designer.
Your Assignment: Look at our msg tracking logic. Right now, if a humanoid is detected, it simply asks, “Shall I track?”. Your task is to tie our speech input directly to this state. Modify the control flow so that the tracking logic requires verification. If a humanoid is detected, the system must wait until the commandQ yields a verified verbal confirmation—like “execute” or “yes”—before the proportional servo tracking begins. If it hears “abort”, it must break off and look away.
Drop your custom implementation script in the comments section below, explain the logic behind your state modifications, and let’s see how you optimize your loop efficiency. Go get ’em!