CBE 30338 Chemical Process Control, Spring 2019
Elizabeth Innis, Kari Minnich, Omosefe Obanor, Alyssa Schuettpelz
The object recognition and tracking of a specific object using a DJI Tello requires a method to communicate changes in the object's position as well as manipulation of the RBG values in a CV2 code. For object recognition, the DJI Tello will need to recognize the specific color of the object. The RBG values chosen for the object are unlikely to be exact, but it will need a specific range to be close enough in order for the Tello to recognize the specific object. To track the object, the DJI Tello will need to process that there is an offset in the original distance of the object from the center of frame and react accordingly to refocus.
This project uses software and coding to operate a DJI Tello Drone. Working with hardware presents a set of challenges that modeling with only software or running simulations does not provide. The issues of dying batteries and overheating had to be overcome before the code could be tested to run commands. It should be noted that if the battery life of the Tello is too low, the drone will not take flight. Before running code, charge all of the battery packs and have replacements ready if the drone will not lift off. In addition to this, there was a point during exploration that the batteries were known to be fully charged, yet the drone would not lift from the ground. The DJI Tello will not take flight if it senses that it is overheating. Carpeted floors have been observed to prevent the DJI Tello from cooling down. This can be circumvented by taking off from solid surfaces or placing a notebook, folder or other convenient smooth, flat object under the drone as a launch surface. Finally, several libraries must be downloaded to run the video feed of the Tello with CV2. Those libraries can be found in the appendix.
The goal of this project is to manipulate the operation of a working drone to enable recognition and tracking. The target detection will be optimized to incorporate internal camera streaming, an analysis of the stream, and subsequent reaction. Existing code repositories are utilized as a reference at a foundational level to guide the target acquisition method. Several applications exist in which such process control for target detection is beneficial. Key areas of use are in search and rescue, as well as other humanitarian efforts.
Operating the DJI Tello with keyboard commands is an important first step to completing the goal for the tracking of an object. The keyboard commands help the user to place the drone where he or she prefers, (proper height, angle, distance from object, etc.) in a ready position for tracking.
Once the DJI Tello can be controlled remotely, the next step to color tracking is to display the camera feed that the Tello sees on the computer using CV2. The pixels in the image can be searched for pixels of a certain color by analyzing RGB values. In this project we used a range of blue values. The code will blur together pixels within the largest area blue color range and draw a circle around the blue object we hope to track.
Tracking the object is done using a PID control loop incorporating the offset coordinates of the center of the object and the center of the frame. This is done by using the coordinates from the previous frame and comparing it to the current frame. These coordinate distances are computed in pixels. The goal of tracking is to track where the blue object is "now" based off of where it was in the last frame.
The control loop uses the offset coordinates and the distance from the center to find velocities. These velocities are used to command the drone to move left, right, up, or down. It runs the loop for each frame and moves the drone accordingly. The drone should move either up, down, left, or right by observing the sign of the velocity in either the x or y direction. The velocity at which the drone moves is the magnitude of the value computed by the control loop. All of this should result in a drone tracking a blue object.
More details about each step of or project are found below. For the full commented code see the appendix.
This code utilizes key presses to control the movements of the drone when tracking is not enabled. These are described by the following code:
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': 'forward',
's': 'backward',
'a': 'left',
'd': 'right',
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.counter_clockwise(speed),
'Key.right': lambda speed: self.drone.clockwise(speed),
'Key.up': lambda speed: self.drone.up(speed),
'Key.down': lambda speed: self.drone.down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
This code conducts image recogniton using CV2 and the video feed from the Tello camera. A range of color values is determined for the desired object and the video stream is converted, frame by frame, into an array of RGB values. The location of the object is determined by analyzing the RBG values and looking for values within the defined range. A circle is drawn surrounding object within the RBG range, and the drone attempts to maintain that circle around the object as it adjusts in flight.
# This code does object recognition using CV2 and the video feed from the Tello camera,
# A range of color values is determined for the desired object
color_lower = (110, 50, 50) #BGR
color_upper = (130, 255, 255) #RGB
#The video feed from the drone is then convereted, frame by frame, into an array of RGB values.
frame = get_frame(vid_stream, stream)
height, width = frame.shape[0], frame.shape[1]
colortracker = Tracker(height, width, color_lower, color_upper)
def get_frame(vid_stream, stream):
"""grab the current video frame"""
frame = vid_stream.read()
# handle the frame from VideoCapture or VideoStream
frame = frame[1] if stream else frame
# if we are viewing a video and we did not grab a frame,
# then we have reached the end of the video
if frame is None:
return None
else:
frame = imutils.resize(frame, width=600)
return frame
def track(self, frame):
"""Simple HSV color space tracking"""
# resize the frame, blur it, and convert it to the HSV
# color space
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
# construct a mask for the color then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, self.color_lower, self.color_upper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[0]
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
# Once the center of the object is determined, its distance from the center of the frame is calculated.
for c in cnts: #iterate through every contour
#c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
temp_xoffset = int(center[0] - self.midx) #store the xoffset and yoffset for each iteration of the loop
temp_yoffset = int(self.midy - center[1])
#calculate the distance from the previous x and y by finding the squared error
sqrd_error = ((temp_xoffset-self.xoffset)**2) + ((temp_yoffset - self.yoffset)**2)
#Set xoffset and yoffset for the lowest possible distance
if lowest_error is None or sqrd_error < lowest_error:
lowest_error = sqrd_error
best_xoffset = temp_xoffset
best_yoffset = temp_yoffset
best_x = x
best_y = y
best_radius = radius
best_center = center
#Set xoffset and yoffset to the best values calculated in the for loop
self.xoffset = best_xoffset
self.yoffset = best_yoffset
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(frame, (int(best_x), int(best_y)), int(best_radius), # Draws the yellow circle on video stream
(0, 255, 255), 2)
cv2.circle(frame, best_center, 5, (0, 0, 255), -1) # Draws a red dot in the center of the yellow circle
else:
self.xoffset = 0
self.yoffset = 0
return self.xoffset, self.yoffset #feed the optimized xoffset and yoffset to telloCV.py
We wanted to use a feedback control loop to direct the drone towards the recognized object from tracker.py
to do this we will used a PID controller.
This code finds an optimal x and y velocity (pixles/second) then tells the drone to go right, left, up, or down,
at the designated velocity.
The governing equations for the PID loop were found and adjusted from the course notes linked below.
https://nbviewer.jupyter.org/github/jckantor/CBE30338/blob/master/notebooks/04.01-Implementing_PID_Control_with_Python_Yield_Statement.ipynb
def init_PID(self):
def proportional():
Vx = 0
Vy = 0
prev_time = time.time()
Ix = 0
Iy = 0
ex_prev = 0
ey_prev = 0
while True:
#yield an x and y velocity from xoff, yoff, and distance
xoff, yoff, distance = yield Vx, Vy
#PID Calculations
ex = xoff - distance
ey = yoff - distance
current_time = time.time()
delta_t = current_time - prev_time
#Control Equations, constants are adjusted as needed
Px = 0.1*ex
Py = 0.1*ey
Ix = Ix + -0.001*ex*delta_t
Iy = Iy + -0.001*ey*delta_t
Dx = 0.01*(ex - ex_prev)/(delta_t)
Dy = 0.01*(ey - ey_prev)/(delta_t)
Vx = Px + Ix + Dx
Vy = Py + Iy + Dy
#update the stored data for the next iteration
ex_prev = ex
ey_prev = ey
prev_time = current_time
self.PID = proportional()
self.PID.send(None)
def process_frame(self, frame):
"""convert frame to cv2 image and show"""
image = cv2.cvtColor(numpy.array(
frame.to_image()), cv2.COLOR_RGB2BGR)
image = self.write_hud(image)
if self.record:
self.record_vid(frame)
distance = 0
xoff, yoff = self.colortracker.track(image)
image = self.colortracker.draw_arrows(image)
Vx,Vy=self.PID.send([xoff, yoff, distance])
# print("TARGET_V: ({Vx},{Vy})".format(Vx=Vx,Vy=Vy)) # Print statement to ensure Vx and Vy are reasonable values (<50)
# Create a loop to implement the Vx and Vy as a command to move the drone accordingly
cmd = ""
speed = 0
if self.tracking:
if abs(Vx) > abs(Vy):
if Vx > 0:
cmd = "right"
speed = abs(Vx)
elif Vx < 0:
cmd = "left"
speed = abs(Vx)
else:
if Vy > 0:
cmd = "up"
speed = abs(Vy)
elif Vy < 0:
cmd = "down"
speed = abs(Vy)
https://github.com/einnis01/CBE30338_Drone_Project/blob/master/DroneSmaller.mp4
The link above contains a video of the DJI Tello drone tracking a blue ball.
Our goal for this project was to enable object recognition and tracking. We were able to accomplish our goal with the implementation of a PID controller and adjusting the RGB values in the CV2 code for our specific object's color.
The RGB values we chose were lower bounds: [90,50,50] and upper bounds: [110,255,255]. These values gave us the best range for the specific blue of our object. The DJI Tello was able to recognize our object very well, but it is important to note that the object could not be in front of a black background or in extremely bright yellow lights. Through various trials, it was evident that a black background and/or bright yellow lights weakened the Tello's ability to recognize the object due to the color feedback the Tello was receiving, which was no longer in the specified range.
We successfully tracked our object using the object tracker in the CV2 code and the PID controller. Moving the object slowly away from the drone allowed the object tracker and PID controller time to process the changes and provide feeback to the drone. There were slight issues with oscillations when the object was moving too fast away from the center of frame. Therefore, slight adjustments were made to the constants in the PID controller to improve the stability of the drone.
All code can be found at https://github.com/einnis01/CBE30338_Drone_Project
This code is the main controller for drone.
"""
tellotracker:
Allows manual operation of the drone and demo tracking mode.
Requires mplayer to record/save video.
Controls:
- tab to lift off
- WASD to move the drone
- space/shift to ascend/descent slowly
- Q/E to yaw slowly
- arrow keys to ascend, descend, or yaw quickl
- backspace to land, or P to palm-land
- enter to take a picture
- R to start recording video, R again to stop recording
(video and photos will be saved to a timestamped file in ~/Pictures/)
- Z to toggle camera zoom state
(zoomed-in widescreen or high FOV 4:3)
- T to toggle tracking
@author Leonie Buckley, Saksham Sinha and Jonathan Byrne
@copyright 2018 see license file for details
"""
import time
import datetime
import os
import tellopy
import numpy
import av
import cv2.cv2 as cv2
from pynput import keyboard
from tracker import Tracker
import threading #Python library that allows you to create multiple threads to run multiple functions at the same time.
current_frame = None # initialize global variable current_frame as None until it is changed in frame_save #numpy.array([[[0,0,0]]]) #initialize global variable as a single black pixel
exiting = False
def frame_save(tellotrack):
""" Stores frames from the Tello VideoStream to a temporary variable """
global current_frame # allows the global variable defined above to be changed inside this function
global exiting
for packet in tellotrack.container.demux((tellotrack.vid_stream,)):
try:
for frame in packet.decode():
current_frame = frame
except Exception as e:
print("ERROR!!! {}".format(e))
exiting = True
print("EXITING FRAME_SAVE THREAD!")
def main():
""" Create a tello controller and show the video feed."""
global current_frame # allows the global variable defined above to be changed inside this function
global exiting
tellotrack = TelloCV()
threading.Thread(target=frame_save, args=[tellotrack]).start() #Create a thread that runs the function frame_save while main runs in the current thread
while not exiting:
if not current_frame is None: #don't run until there is a new current_frame
image = tellotrack.process_frame(current_frame)
cv2.imshow('tello', image)
_ = cv2.waitKey(1) & 0xFF
print("EXITING MAIN THREAD!")
class TelloCV(object):
"""
TelloTracker builds keyboard controls on top of TelloPy as well
as generating images from the video stream and enabling opencv support
"""
def __init__(self):
self.prev_flight_data = None
self.record = False
self.tracking = False
self.keydown = False
self.date_fmt = '%Y-%m-%d_%H%M%S'
self.speed = 50
self.drone = tellopy.Tello()
self.init_drone()
self.init_controls()
# container for processing the packets into frames
self.container = av.open(self.drone.get_video_stream())
self.vid_stream = self.container.streams.video[0]
self.out_file = None
self.out_stream = None
self.out_stream_writer = None
self.out_name = None
self.start_time = time.time()
# tracking a color
#Assign a range of RGB values for the drone to look for
#green_lower = (30, 50, 50)
#green_upper = (80, 255, 255)
#red_lower = (0, 50, 50)
#red_upper = (20, 255, 255)
#blue_lower = (0, 0, 130)
#upper_blue = (150, 220, 255)
color_lower = (90, 50, 50)
color_upper =(110, 255, 255,)
self.track_cmd = ""
self.track_speed = 0
self.init_PID()
self.colortracker = Tracker(self.vid_stream.height,
self.vid_stream.width,
color_lower, color_upper)
def init_drone(self):
"""Connect, uneable streaming and subscribe to events"""
# self.drone.log.set_level(2)
self.drone.connect()
self.drone.start_video()
self.drone.subscribe(self.drone.EVENT_FLIGHT_DATA,
self.flight_data_handler)
self.drone.subscribe(self.drone.EVENT_FILE_RECEIVED,
self.handle_flight_received)
def on_press(self, keyname):
"""handler for keyboard listener"""
if self.keydown:
return
try:
self.keydown = True
keyname = str(keyname).strip('\'')
print('+' + keyname)
if keyname == 'Key.esc':
self.drone.quit()
exit(0)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(self.speed)
else:
key_handler(self.speed)
except AttributeError:
print('special key {0} pressed'.format(keyname))
def on_release(self, keyname):
"""Reset on key up from keyboard listener"""
self.keydown = False
keyname = str(keyname).strip('\'')
print('-' + keyname)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(0)
else:
key_handler(0)
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': 'forward',
's': 'backward',
'a': 'left',
'd': 'right',
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.counter_clockwise(speed),
'Key.right': lambda speed: self.drone.clockwise(speed),
'Key.up': lambda speed: self.drone.up(speed),
'Key.down': lambda speed: self.drone.down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
# self.key_listener.join()
#We want to use a feedback control loop to direct the drone towards the recognized object from tracker.py
#to do this we will use a PID controller.
#find an optimal Vx and Vy using a PID
# distance = 100 # setpoint, pixels
# Vx = 0 #manipulated variable
# Vy = 0 #manipulated variable
def init_PID(self):
def proportional():
Vx = 0
Vy = 0
prev_time = time.time()
Ix = 0
Iy = 0
ex_prev = 0
ey_prev = 0
while True:
#yield an x and y velocity from xoff, yoff, and distance
xoff, yoff, distance = yield Vx, Vy
#PID Calculations
ex = xoff - distance
ey = yoff - distance
current_time = time.time()
delta_t = current_time - prev_time
#Control Equations, constants are adjusted as needed
Px = 0.1*ex
Py = 0.1*ey
Ix = Ix + -0.001*ex*delta_t
Iy = Iy + -0.001*ey*delta_t
Dx = 0.01*(ex - ex_prev)/(delta_t)
Dy = 0.01*(ey - ey_prev)/(delta_t)
Vx = Px + Ix + Dx
Vy = Py + Iy + Dy
#update the stored data for the next iteration
ex_prev = ex
ey_prev = ey
prev_time = current_time
self.PID = proportional()
self.PID.send(None)
def process_frame(self, frame):
"""convert frame to cv2 image and show"""
image = cv2.cvtColor(numpy.array(
frame.to_image()), cv2.COLOR_RGB2BGR)
image = self.write_hud(image)
if self.record:
self.record_vid(frame)
distance = 0
xoff, yoff = self.colortracker.track(image)
image = self.colortracker.draw_arrows(image)
Vx,Vy=self.PID.send([xoff, yoff, distance])
# print("TARGET_V: ({Vx},{Vy})".format(Vx=Vx,Vy=Vy)) # Print statement to ensure Vx and Vy are reasonable values (<50)
# Create a loop to implement the Vx and Vy as a command to move the drone accordingly
cmd = ""
speed = 0
if self.tracking:
if abs(Vx) > abs(Vy):
if Vx > 0:
cmd = "right"
speed = abs(Vx)
elif Vx < 0:
cmd = "left"
speed = abs(Vx)
else:
if Vy > 0:
cmd = "up"
speed = abs(Vy)
elif Vy < 0:
cmd = "down"
speed = abs(Vy)
#Original Code:
# if self.tracking:
# if xoff < -distance:
# cmd = "counter_clockwise"
# elif xoff > distance:
# cmd = "clockwise"
# elif yoff < -distance:
# cmd = "down"
# elif yoff > distance:
# cmd = "up"
# else:
# if self.track_cmd is not "":
# getattr(self.drone, self.track_cmd)(0)
# self.track_cmd = ""
print(self.track_cmd)
if cmd is not self.track_cmd or speed != self.track_speed: #!= means not equal to
if cmd is not "":
print("track command:", cmd)
print("track speed:", speed)
getattr(self.drone, cmd)(speed)
self.track_cmd = cmd
self.track_speed = speed
else:
if self.track_cmd is not "":
getattr(self.drone, self.track_cmd)(0)
self.track_cmd = ""
print("STOPPING!!!!!!!!")
getattr(self.drone, "down")(0) #Stop the drone
getattr(self.drone, "right")(0) #Stop the drone
return image
def write_hud(self, frame):
"""Draw drone info, tracking and record on frame"""
stats = self.prev_flight_data.split('|')
stats.append("Tracking:" + str(self.tracking))
if self.drone.zoom:
stats.append("VID")
else:
stats.append("PIC")
if self.record:
diff = int(time.time() - self.start_time)
mins, secs = divmod(diff, 60)
stats.append("REC {:02d}:{:02d}".format(mins, secs))
for idx, stat in enumerate(stats):
text = stat.lstrip()
cv2.putText(frame, text, (0, 30 + (idx * 30)),
cv2.FONT_HERSHEY_SIMPLEX,
1.0, (255, 0, 0), lineType=30)
return frame
def toggle_recording(self, speed):
"""Handle recording keypress, creates output stream and file"""
if speed == 0:
return
self.record = not self.record
if self.record:
datename = [
# os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt),
]
self.out_name = 'C:/Users/eliza/OneDrive/Computer_Files/Desktop/Desktop/Fourth_Year/SPRING_2019/PROCESS_CONTROLS/Untitled_Folder/telloCV-master/Images/tello-{}.mp4'.format(*datename)
print("Outputting video to:", self.out_name)
self.out_stream_writer = cv2.VideoWriter(self.out_name,cv2.CV_FOURCC('P','I','M','1'),25,(self.vid_stream.width,self.vid_stream.height))
# self.out_file = av.open(self.out_name, 'w')
# self.start_time = time.time()
# self.out_stream = self.out_file.add_stream(
# 'mpeg4', self.vid_stream.rate)
# self.out_stream.pix_fmt = 'yuv420p'
# self.out_stream.width = self.vid_stream.width
# self.out_stream.height = self.vid_stream.height
if not self.record:
print("Video saved to ", self.out_name)
# self.out_file.close()
# self.out_stream = None
self.out_stream_writer.release()
self.out_stream_writer = None
def record_vid(self, frame):
"""
convert frames to packets and write to file
"""
if not self.out_stream_writer is None:
print(frame.width, frame.height, frame.format.name)
arr = frame.to_ndarray(format='rgb24')
# print(arr)
# print(arr.shape)
self.out_stream_writer.write(arr)
# new_frame = av.video.frame.VideoFrame.from_ndarray(arr)
# # new_frame = av.VideoFrame(
# # width=frame.width, height=frame.height, format=frame.format.name)
# # for i in range(len(frame.planes)):
# # print(type(frame.planes[i]))
# # #new_frame.planes[i].update(frame.planes[i])
# # new_frame.planes[i].update(frame.planes[i].reformat(width=frame.width, height=frame.height, format=frame.format.name))
# pkt = None
# try:
# # pkt = self.out_stream.encode(new_frame)
# pkt = self.out_stream.encode(new_frame)
# except IOError as err:
# print("encoding failed: {0}".format(err))
# if pkt is not None:
# try:
# self.out_file.mux(pkt)
# except IOError:
# print('mux failed: ' + str(pkt))
def take_picture(self, speed):
"""Tell drone to take picture, image sent to file handler"""
if speed == 0:
return
self.drone.take_picture()
def palm_land(self, speed):
"""Tell drone to land"""
if speed == 0:
return
self.drone.palm_land()
def toggle_tracking(self, speed):
""" Handle tracking keypress"""
if speed == 0: # handle key up event
return
self.tracking = not self.tracking
print("tracking:", self.tracking)
return
def toggle_zoom(self, speed):
"""
In "video" mode the self.drone sends 1280x720 frames.
In "photo" mode it sends 2592x1936 (952x720) frames.
The video will always be centered in the window.
In photo mode, if we keep the window at 1280x720 that gives us ~160px on
each side for status information, which is ample.
Video mode is harder because then we need to abandon the 16:9 display size
if we want to put the HUD next to the video.
"""
if speed == 0:
return
self.drone.set_video_mode(not self.drone.zoom)
def flight_data_handler(self, event, sender, data):
"""Listener to flight data from the drone."""
text = str(data)
if self.prev_flight_data != text:
self.prev_flight_data = text
def handle_flight_received(self, event, sender, data):
"""Create a file in ~/Pictures/ to receive image from the drone"""
path = 'C:Users/eliza/OneDrive/Computer_Files/Desktop/Desktop/Fourth_Year/SPRING_2019/PROCESS_CONTROLS/Untitled_Folder/telloCV-master/Images/tello-{}.jpeg' % (
os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt))
with open(path, 'wb') as out_file:
out_file.write(data)
print('Saved photo to' % path)
if __name__ == '__main__':
main()
This code is responsible for the image recognition and object tracking.
"""
A tracker class for controlling the Tello and some sample code for showing how
it works. you can test it using your webcam or a video file to make sure it works.
it computes a vector of the ball's direction from the center of the
screen. The axes are shown below (assuming a frame width and height of 600x400):
+y (0,200)
Y (-300, 0) (0,0) (300,0)
-Y (0,-200)
-X X +X
Based on the tutorial:
https://www.pyimagesearch.com/2015/09/14/ball-tracking-with-opencv/
Usage:
for existing video:
python tracker.py --video ball_tracking_example.mp4
For live feed:
python tracking.py
@author Leonie Buckley and Jonathan Byrne
@copyright 2018 see license file for details
"""
# import the necessary packages
import argparse
import time
import cv2
import imutils
from imutils.video import VideoStream
def main():
"""Handles inpur from file or stream, tests the tracker class"""
arg_parse = argparse.ArgumentParser()
arg_parse.add_argument("-v", "--video",
help="path to the (optional) video file")
args = vars(arg_parse.parse_args())
# define the lower and upper boundaries of the "green"
# ball in the HSV color space. NB the hue range in
# opencv is 180, normally it is 360
#green_lower = (50, 50, 50)
#green_upper = (70, 255, 255)
# red_lower = (0, 50, 50)
# red_upper = (20, 255, 255)
# blue_lower = (110, 50, 50)
# upper_blue = (130, 255, 255)
color_lower = (110, 50, 50) #BGR
color_upper = (130, 255, 255) #RGB
# if a video path was not supplied, grab the reference
# to the webcam
if not args.get("video", False):
vid_stream = VideoStream(src=0).start()
# otherwise, grab a reference to the video file
else:
vid_stream = cv2.VideoCapture(args["video"])
# allow the camera or video file to warm up
time.sleep(2.0)
stream = args.get("video", False)
frame = get_frame(vid_stream, stream)
height, width = frame.shape[0], frame.shape[1]
colortracker = Tracker(height, width, color_lower, color_upper)
# keep looping until no more frames
more_frames = True
while more_frames:
colortracker.track(frame)
frame = colortracker.draw_arrows(frame)
show(frame)
frame = get_frame(vid_stream, stream)
if frame is None:
more_frames = False
# if we are not using a video file, stop the camera video stream
if not args.get("video", False):
vid_stream.stop()
# otherwise, release the camera
else:
vid_stream.release()
# close all windows
cv2.destroyAllWindows()
def get_frame(vid_stream, stream):
"""grab the current video frame"""
frame = vid_stream.read()
# handle the frame from VideoCapture or VideoStream
frame = frame[1] if stream else frame
# if we are viewing a video and we did not grab a frame,
# then we have reached the end of the video
if frame is None:
return None
else:
frame = imutils.resize(frame, width=600)
return frame
def show(frame):
"""show the frame to cv2 window"""
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
# if the 'q' key is pressed, stop the loop
if key == ord("q"):
exit()
class Tracker:
"""
A basic color tracker, it will look for colors in a range and
create an x and y offset valuefrom the midpoint
"""
def __init__(self, height, width, color_lower, color_upper):
self.color_lower = color_lower
self.color_upper = color_upper
self.midx = int(width / 2)
self.midy = int(height / 2)
self.xoffset = 0
self.yoffset = 0
def draw_arrows(self, frame):
"""Show the direction vector output in the cv2 window"""
#cv2.putText(frame,"Color:", (0, 35), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, thickness=2)
cv2.arrowedLine(frame, (self.midx, self.midy),
(self.midx + self.xoffset, self.midy - self.yoffset),
(0, 0, 255), 5)
return frame
def track(self, frame):
"""Simple HSV color space tracking"""
# resize the frame, blur it, and convert it to the HSV
# color space
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
# construct a mask for the color then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, self.color_lower, self.color_upper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = cnts[0]
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
#initialize temp variables
lowest_error = None
best_xoffset = 0
best_yoffset = 0
best_x = 0
best_y = 0
best_radius = 0
best_center = (0,0)
for c in cnts: #iterate through every contour
#c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
temp_xoffset = int(center[0] - self.midx) #store the xoffset and yoffset for each iteration of the loop
temp_yoffset = int(self.midy - center[1])
#calculate the distance from the previous x and y by finding the squared error
sqrd_error = ((temp_xoffset-self.xoffset)**2) + ((temp_yoffset - self.yoffset)**2)
#Set xoffset and yoffset for the lowest possible distance
if lowest_error is None or sqrd_error < lowest_error:
lowest_error = sqrd_error
best_xoffset = temp_xoffset
best_yoffset = temp_yoffset
best_x = x
best_y = y
best_radius = radius
best_center = center
#Set xoffset and yoffset to the best values calculated in the for loop
self.xoffset = best_xoffset
self.yoffset = best_yoffset
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(frame, (int(best_x), int(best_y)), int(best_radius), # Draws the yellow circle on video stream
(0, 255, 255), 2)
cv2.circle(frame, best_center, 5, (0, 0, 255), -1) # Draws a red dot in the center of the yellow circle
else:
self.xoffset = 0
self.yoffset = 0
return self.xoffset, self.yoffset #feed the optimized xoffset and yoffset to telloCV.py
if __name__ == '__main__':
main()