In this lesson we actively track an object using the Jetson Nano, pan/tilt servos, and OpenCV. In order for this code to work, you need to first set up your servos, as explained in Lesson 31 on youtube. Make sure to follow the instructions on that lesson before trying to do this one, as that will show you how to get your servos connected and set up.
|
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 |
import cv2 print(cv2.__version__) import numpy as np from adafruit_servokit import ServoKit kit=ServoKit(channels=16) pan=90 tilt=45 kit.servo[0].angle=pan kit.servo[1].angle=tilt def nothing(x): pass cv2.namedWindow('Trackbars') cv2.moveWindow('Trackbars',1320,0) cv2.createTrackbar('hueLower', 'Trackbars',96,179,nothing) cv2.createTrackbar('hueUpper', 'Trackbars',120,179,nothing) cv2.createTrackbar('hue2Lower', 'Trackbars',50,179,nothing) cv2.createTrackbar('hue2Upper', 'Trackbars',0,179,nothing) cv2.createTrackbar('satLow', 'Trackbars',157,255,nothing) cv2.createTrackbar('satHigh', 'Trackbars',255,255,nothing) cv2.createTrackbar('valLow','Trackbars',100,255,nothing) cv2.createTrackbar('valHigh','Trackbars',255,255,nothing) dispW=640 dispH=480 flip=2 #Uncomment These next Two Line for Pi Camera #camSet='nvarguscamerasrc ! video/x-raw(memory:NVMM), width=3264, height=2464, format=NV12, framerate=21/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink' #cam= cv2.VideoCapture(camSet) #Or, if you have a WEB cam, uncomment the next line #(If it does not work, try setting to '1' instead of '0') cam=cv2.VideoCapture(1) width=cam.get(cv2.CAP_PROP_FRAME_WIDTH) height=cam.get(cv2.CAP_PROP_FRAME_HEIGHT) print('width:',width,'height:',height) while True: ret, frame = cam.read() #frame=cv2.imread('smarties.png') hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) hueLow=cv2.getTrackbarPos('hueLower', 'Trackbars') hueUp=cv2.getTrackbarPos('hueUpper', 'Trackbars') hue2Low=cv2.getTrackbarPos('hue2Lower', 'Trackbars') hue2Up=cv2.getTrackbarPos('hue2Upper', 'Trackbars') Ls=cv2.getTrackbarPos('satLow', 'Trackbars') Us=cv2.getTrackbarPos('satHigh', 'Trackbars') Lv=cv2.getTrackbarPos('valLow', 'Trackbars') Uv=cv2.getTrackbarPos('valHigh', 'Trackbars') l_b=np.array([hueLow,Ls,Lv]) u_b=np.array([hueUp,Us,Uv]) l_b2=np.array([hue2Low,Ls,Lv]) u_b2=np.array([hue2Up,Us,Uv]) FGmask=cv2.inRange(hsv,l_b,u_b) FGmask2=cv2.inRange(hsv,l_b2,u_b2) FGmaskComp=cv2.add(FGmask,FGmask2) cv2.imshow('FGmaskComp',FGmaskComp) cv2.moveWindow('FGmaskComp',0,530) _,contours,_=cv2.findContours(FGmaskComp,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True) for cnt in contours: area=cv2.contourArea(cnt) (x,y,w,h)=cv2.boundingRect(cnt) if area>=50: #cv2.drawContours(frame,[cnt],0,(255,0,0),3) cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),3) objX=x+w/2 objY=y+h/2 errorPan=objX-width/2 errorTilt=objY-height/2 if abs(errorPan)>15: pan=pan-errorPan/75 if abs(errorTilt)>15: tilt=tilt-errorTilt/75 if pan>180: pan=180 print("Pan Out of Range") if pan<0: pan=0 print("Pan Out of Range") if tilt>180: tilt=180 print("Tilt Out of Range") if tilt<0: tilt=0 print("Tilt Out of Range") kit.servo[0].angle=pan kit.servo[1].angle=tilt break cv2.imshow('nanoCam',frame) cv2.moveWindow('nanoCam',0,0) if cv2.waitKey(1)==ord('q'): break cam.release() cv2.destroyAllWindows() |