def __init__(self, platformColor, settingsPath):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, Kp=P, Ki=I, Kd=D)
self.moveTime = 0.2
self.waitTime = 0.1
def __init__(self, platformColor):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P,I,D = 0.0054, 0.006352, 0.0011475
#P,I,D = 0.00405, 0.00285, 0
self.pid = PIDController(Kp = P, Ki = I, Kd = D)
self.moveTime = 0.2
self.waitTime = 0
self.pub_pid_xspeed = rospy.Publisher('pid_xspeed', Float32, queue_size = 10)
self.pub_pid_yspeed = rospy.Publisher('pid_yspeed', Float32, queue_size = 10)
self.pub_pid_in_alt = rospy.Publisher('pid_in_alt', Int32, queue_size = 10)
rate = rospy.Rate(5)
def __init__(self, orientation, lineColor, platformColor, settingsPath):
if orientation != "PARALLEL" and orientation != "PERPENDICULAR":
raise Exception("Orientation not recognized.")
else:
self.orientation = orientation
self.lineColor = lineColor
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, P, I, D)
self.moveTime = 0.2
self.waitTime = 0.1
def __init__(self, image):
self.pid = PIDController()
self.process = ProcessVideo()
#take input as bgr image, given orange is visible, will command to hover ontop of it
def HoverOnCheckpoint(self, image):
# calculating cx,cy,xspeed,yspeed
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
self.pid.UpdateDeltaTime()
self.pid.SetPoint(orang_image)
self.pid.UpdateError(self.cx, self.cy)
self.pid.SetPIDTerm()
xspeed, yspeed = self.pid.GetPIDValues()
self.pid.DrawArrow(orange_image, xspeed, yspeed)
return xspeed, yspeed
def OrienttoLine(self, image):
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
blue_image = self.process.DetectColor(image, 'blue')
angle = self.process.ShowLine(blue_image)
yawspeed = self.process.LineOrientation(blue_image, angle, 5)
return yawspeed
def OrienttoObject(self, image):
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
green_image = self.process.DetectColor(image, 'green')
angle = self.process.ShowLine(green_image)
yawspeed = self.process.ObjectOrientation(green_image, angle, 5)
return yawspeed
class PIDHoverColorDirective2(AbstractDroneDirective):
# sets up this directive
# platformColor: color to hover over. Altitude is maintained
def __init__(self, platformColor):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P,I,D = 0.0054, 0.006352, 0.0011475
#P,I,D = 0.00405, 0.00285, 0
self.pid = PIDController(Kp = P, Ki = I, Kd = D)
self.moveTime = 0.2
self.waitTime = 0
self.pub_pid_xspeed = rospy.Publisher('pid_xspeed', Float32, queue_size = 10)
self.pub_pid_yspeed = rospy.Publisher('pid_yspeed', Float32, queue_size = 10)
self.pub_pid_in_alt = rospy.Publisher('pid_in_alt', Int32, queue_size = 10)
rate = rospy.Rate(5)
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 0 if algorithm is still running and drone isn't on orange yet
# 1 if algorithm is finished and drone is now on orange
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
image = navdata["segImage"]
cx, cy = navdata["center"][1][0], navdata["center"][1][1]
altitude = navdata["altitude"][1]
self.pub_pid_in_alt.publish(altitude)
if cx != None and cy != None:
cv2.circle(image, (cx, cy), 7, (255, 255, 255), -1)
numRows, numCols, _ = image.shape
centerx = numCols/2
centery = numRows/2
windowSize = 40
xLower = centerx-windowSize
yLower = centery-windowSize
xUpper = centerx+windowSize
yUpper = centery+windowSize
cv2.rectangle(image, (xLower, yLower), (xUpper, yUpper), (255,255,255), 3)
xspeed, yspeed = self.pid.getOutput(cx,cy,altitude)
#self.pid.UpdateDeltaTime()
#self.pid.UpdateError(cx,cy,altitude)
#self.pid.SetPIDTerms()
#xspeed, yspeed = self.pid.GetPIDValues()
self.pub_pid_xspeed.publish(xspeed)
self.pub_pid_yspeed.publish(yspeed)
yspeed = 0
rospy.logwarn("Xspeed = " + str(xspeed) +" Yspeed = " + str(yspeed))
directiveStatus = 1
return directiveStatus, (xspeed, yspeed, 0, 0), image, (cx,cy), self.moveTime, self.waitTime, None
# This method is called by the state machine when it considers this directive finished
def Finished(self):
self.Reset()
def Reset(self):
self.pid.ResetPID()
class PIDOrientLineDirective(AbstractDroneDirective):
# orientation:
# > either "VERTICAL" or "PERPENDICULAR";
# algorithm will orient drone vertically or perpendicular to the line respectively
# lineColor:
# > color of the line to orient to
# platformColor:
# > color of the platform to orient to
# settingsPath:
# > Path for getting PID settings
# hoverAltitude:
# > how high to hover over the platform
def __init__(self, orientation, lineColor, platformColor, settingsPath):
if orientation != "PARALLEL" and orientation != "PERPENDICULAR":
raise Exception("Orientation not recognized.")
else:
self.orientation = orientation
self.lineColor = lineColor
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, P, I, D)
self.moveTime = 0.2
self.waitTime = 0.1
def GetSettings(self, settingsPath):
# read a text file as a list of lines
# find the last line, change to a file you have
fileHandle = open(settingsPath, 'r')
last = fileHandle.readlines()
fileHandle.close()
last = str(last[len(last) - 1]).split()
p, i, d = [float(x) for x in (last)]
return p, i, d
# Given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 0 if algorithm is still running and drone isn't oriented yet
# 1 if algorithm is finished and drone is now oriented
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
segLineImage = self.processVideo.DetectColor(image, self.lineColor)
cx, cy = navdata["center"][1][0], navdata["center"][1][1]
altitude = navdata["SVCLAltitude"][1]
#draws center of circle on image
self.processVideo.DrawCircle(segLineImage, (cx, cy))
self.pid.UpdateDeltaTime()
self.pid.UpdateError(cx, cy, altitude)
self.pid.SetPIDTerms()
xspeed, yspeed = self.pid.GetPIDValues()
if self.orientation == "PARALLEL":
angle = self.processVideo.ShowLine(segLineImage,
lowerAngleBound=0,
upperAngleBound=70,
secondBounds=(110, 180),
thresh=35)
yawspeed = self.processVideo.ObjectOrientation(segLineImage,
angle,
5,
yawspeed=0.5)
xWindowSize = 50
yWindowSize = 50
elif self.orientation == "PERPENDICULAR":
angle = self.processVideo.ShowLine(segLineImage,
lowerAngleBound=30,
upperAngleBound=125,
thresh=15)
yawspeed = self.processVideo.LineOrientation(segLineImage,
angle,
5,
yawspeed=0.5)
xWindowSize = 180
yWindowSize = 70
numRows, numCols, _ = image.shape
centerx = numCols / 2
centery = numRows / 2
# box defines when the directive is finished
xLower = centerx - xWindowSize
yLower = centery - yWindowSize
xUpper = centerx + xWindowSize
yUpper = centery + yWindowSize
cv2.rectangle(segLineImage, (xLower, yLower), (xUpper, yUpper),
(255, 255, 255), 3)
if (yawspeed == 0 and cx != None and cy != None and cx < xUpper
and cx > xLower and cy < yUpper and cy > yLower):
rospy.logwarn("Oriented " + self.orientation + " to " +
self.lineColor + " line")
directiveStatus = 1
elif cx == None or cy == None:
rospy.logwarn("*** ERROR: Lost " + self.platformColor +
" platform ***")
directiveStatus = -1
else:
# If drone is still trying to align, it adapts to one of three algorithms:
# if this frame failed to detect a line, go towards platform
# without turning in hopes that the next frames will detect one again
if yawspeed == None:
#rospy.logwarn("No line detected")
yawspeed = 0
# if drone is not "near" the center defined by a box, just focus on moving drone back;
# no turning
elif self.orientation == "PERPENDICULAR" and (
cx > xUpper or cx < xLower or cy > yUpper or cy < yLower):
cv2.rectangle(segLineImage, (xLower, yLower), (xUpper, yUpper),
(0, 0, 255), 3)
#rospy.logwarn("Only MOVING drone. x speed = " + str(xspeed) + "; y speed = " + str(yspeed))
rospy.logwarn("Only MOVING drone")
self.moveTime = 0.1
self.waitTime = 0.01
yawspeed = 0
# if drone isn't perpendicular yet and is "near" the center (defined by a box),
# just turn the drone; no need move drone
elif yawspeed != 0:
#rospy.logwarn("Only TURNING drone. yaw speed = " + str(yawspeed))
rospy.logwarn("Only TURNING drone")
self.moveTime = 0.2
self.waitTime = 0.1
xspeed = 0
yspeed = 0
#else:
# rospy.logwarn("Only MOVING drone")
#rospy.logwarn("Only MOVING drone. x speed = " + str(xspeed) + "; y speed = " + str(yspeed))
directiveStatus = 0
return directiveStatus, (xspeed, yspeed, yawspeed, 0), segLineImage, (
cx, cy), self.moveTime, self.waitTime, None
# This method is called by the state machine when it considers this directive finished
def Finished(self):
rospy.logwarn("***** Resetting PID Values *****")
self.pid.ResetPID()
class PIDHoverColorDirective(AbstractDroneDirective):
# sets up this directive
# platformColor: color to hover over
# settingsPath: path of file to read PID settings from
def __init__(self, platformColor, settingsPath):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, Kp=P, Ki=I, Kd=D)
self.moveTime = 0.2
self.waitTime = 0.1
def GetSettings(self, settingsPath):
# read a text file as a list of lines
# find the last line, change to a file you have
fileHandle = open(settingsPath, 'r')
last = fileHandle.readlines()
fileHandle.close()
last = str(last[len(last) - 1]).split()
p, i, d = [float(x) for x in (last)]
return p, i, d
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 0 if algorithm is still running and drone isn't on orange yet
# 1 if algorithm is finished and drone is now on orange
#
# A tuple of (xspeed, yspeed, yawspeed, zspeed):
# indicating the next instructions to fly the drone
#
# An image reflecting what is being done as part of the algorithm
def RetrieveNextInstruction(self, image, navdata):
image = navdata["segImage"]
cx, cy = navdata["center"][1][0], navdata["center"][1][1]
altitude = navdata["SVCLAltitude"][1]
if cx != None and cy != None:
cv2.circle(image, (cx, cy), 7, (255, 255, 255), -1)
self.pid.UpdateDeltaTime()
self.pid.UpdateError(cx, cy, altitude)
self.pid.SetPIDTerms()
xspeed, yspeed = self.pid.GetPIDValues()
numRows, numCols, _ = image.shape
centerx = numCols / 2
centery = numRows / 2
windowSize = 40
xLower = centerx - windowSize
yLower = centery - windowSize
xUpper = centerx + windowSize
yUpper = centery + windowSize
self.pid.DrawArrow(image, xspeed, yspeed)
cv2.rectangle(image, (xLower, yLower), (xUpper, yUpper),
(255, 255, 255), 3)
# if there is orange in the screen, and the drone is in the middle, return true
if cx != None and cy != None and cx < xUpper and cx > xLower and cy < yUpper and cy > yLower:
'''rospy.logwarn("PID: Done Hovering on " + self.platformColor)
'''
directiveStatus = 1
elif cx == None or cy == None:
'''rospy.logwarn("")
rospy.logwarn("PID: ERROR -- couldn't find " + self.platformColor + " platform")
'''
directiveStatus = -1
else:
p, i, d = self.pid.ReturnPIDvalues()
directiveStatus = 0
return directiveStatus, (xspeed, yspeed, 0, 0), image, (
cx, cy), self.moveTime, self.waitTime, None
# This method is called by the state machine when it considers this directive finished
def Finished(self):
rospy.logwarn("***** Resetting PID Values *****")
self.pid.ResetPID()