class HoverColorDirective(AbstractDroneDirective):
# sets up this directive
# platformColor: color to hover over
# hoverAltitude: how high to hover over the color, in mm
def __init__(self, platformColor, hoverAltitude):
self.platformColor = platformColor
self.hoverAltitude = hoverAltitude
self.processVideo = ProcessVideo()
self.moveTime = 0
self.waitTime = 0.10
# 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
# -1 if an error has occured
#
# 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
#
# A coordinate pair that represents where things are being tracked
# If nothing is being tracked, (None,None) is returned
def RetrieveNextInstruction(self, image, navdata):
# color segmented image
segImage = self.processVideo.DetectColor(image, self.platformColor)
cx, cy = self.processVideo.CenterOfMass(segImage)
xspeed, yspeed, zspeed = self.processVideo.ApproximateSpeed(segImage, cx, cy,
navdata["altitude"][1], self.hoverAltitude, xtolerance = 55, ytolerance = 55)
# if there is orange in the screen, and the drone is in the middle, return true
if cx != None and cy != None and xspeed == 0 and yspeed == 0 and zspeed == 0:
rospy.logwarn("Done Hovering on " + self.platformColor)
directiveStatus = 1
elif cx == None or cy == None:
rospy.logwarn("Can't Hover on " + self.platformColor + "; lost platform")
directiveStatus = -1
else:
rospy.logwarn("Trying to Hover on " + self.platformColor)
directiveStatus = 0
return directiveStatus, (xspeed, yspeed, 0, zspeed), segImage, (cx,cy), self.moveTime, self.waitTime, None
class DronePosition(object):
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 ReturnToOriginDirective(AbstractDroneDirective):
# sets up this directive
# tolerance is in mm; how close it needs to be from where it took off
def __init__(self, platformColor, tolerance):
self.platformColor = platformColor
self.tolerance = tolerance
self.processVideo = ProcessVideo()
# given the image and navdata of the drone, returns the following in order:
#
# A directive status int:
# 1 if algorithm is finished and the drone has reached where it took off
# 0 if the algorithm is still running and the drone hasn't reached where it took off
#
# 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):
dispH = navdata["dispLR"][1]
speed = 0.2
# calculating horizontal
if dispH < self.tolerance * -1:
roll = speed
rospy.logwarn("Drone is returning to origin; going left")
elif dispH > self.tolerance:
roll = speed * -1
rospy.logwarn("Drone is returning to origin; going right")
else:
roll = 0
# calculating vertical
dispV = navdata["dispFB"][1]
if dispV < self.tolerance * -1:
rospy.logwarn("Drone is returning to origin; going forward")
pitch = speed
elif dispV > self.tolerance:
rospy.logwarn("Drone is returning to origin; going backwards")
pitch = speed * -1
else:
pitch = 0
# check if the platform is visible
platform_image = self.processVideo.DetectColor(image, self.platformColor)
hasPlatform = self.processVideo.IsHueDominant(platform_image, 0, 360, 0.2)
if (pitch == 0 and roll == 0) or hasPlatform:
directiveStatus = 1
rospy.logwarn("Drone has returned to origin")
else:
directiveStatus = 0
return directiveStatus, (roll, pitch, 0, 0), image, (None,None), 0, 0, None
class OrientLineDirective(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
# hoverAltitude:
# > how high to hover over the platform
def __init__(self,
orientation,
lineColor,
platformColor,
hoverAltitude,
heightTolerance=50,
yOffset=0,
ySizeOffset=0):
if orientation != "PARALLEL" and orientation != "PERPENDICULAR":
raise Exception("Orientation not recognized.")
else:
self.orientation = orientation
self.lineColor = lineColor
self.platformColor = platformColor
self.hoverAltitude = hoverAltitude
self.processVideo = ProcessVideo()
self.prevCenter = None
self.forceCenter = None
self.prevAngle = None
self.heightTolerance = heightTolerance
self.yOffset = yOffset
self.ySizeOffset = ySizeOffset
# 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):
self.moveTime = 0.17
#self.moveTime=0.23
self.waitTime = 0.1
segLineImage = self.processVideo.DetectColor(image, self.lineColor)
cx, cy = navdata["center"][1][0], navdata["center"][1][1]
if cx != None and cy != None:
cv2.circle(segLineImage, (cx, cy), 6, (255, 255, 255), -1)
centers = navdata["allCenters"][1]
if self.forceCenter != None:
self.forceCenter = None
# when directive first starts, it latches onto the first correct orange platform it sees
if self.prevCenter == None:
rospy.logwarn("FINDING INITAL CENTER---")
if cx != None and cy != None:
self.prevCenter = (cx, cy)
# pick the rightmost center
rightmostCenter = centers[0]
if self.orientation == "PARALLEL":
for i in range(len(centers)):
if centers[i][0] > rightmostCenter[0]:
rightmostCenter = centers[i]
self.forceCenter = rightmostCenter
else:
# pick the center that is closest to the most vertical pink line
# finding most vertical line
objectLineImg = self.processVideo.DetectColor(image, "pink")
objectLines, objectLineImg = self.processVideo.MultiShowLine(
objectLineImg, sort=False)
mostVertical = None
rospy.logwarn(" All pink lines: ")
for line in objectLines:
if line != None:
rospy.logwarn("@: " + str(line[1]) + ", " +
str(line[0]) + " degrees")
if mostVertical == None or (
(abs(90 - line[0]) < abs(90 - mostVertical[0]))
and line[4] > 30):
#if ( mostHorizontal == None or
#( min(mostHorizontal[0], 180 - mostHorizontal[0] ) > (min(line[0], 180 - line[0] ) )
#and line[4] > 30 ) ):
#mostHorizontal = line
mostVertical = line
rospy.logwarn("Found most vertical pink line @: " +
str(mostVertical[1]) + ", with angle " +
str(mostVertical[0]))
"""
# finding center closest to the left endpoint of that vertical line
if mostHorizontal[2][0] < mostHorizontal[3][0]:
leftEndpoint = mostHorizontal[2]
else:
leftEndpoint = mostHorizontal[3]
"""
correctCenter = centers[0]
rospy.logwarn("All centers: ")
for i in range(len(centers)):
"""
correctEndpointDist = math.sqrt( math.pow((correctCenter[1] - leftEndpoint[1]),2)
+ math.pow((correctCenter[0] - leftEndpoint[0]),2 ) )
currEndpointDist = math.sqrt( math.pow((centers[i][1] - leftEndpoint[1]),2)
+ math.pow((centers[i][0] - leftEndpoint[0]),2 ) )
if currEndpointDist < correctEndpointDist:
correctCenter = centers[i]
"""
rospy.logwarn("@: " + str(centers[i]))
if abs(mostVertical[1][0] -
centers[i][0]) < abs(mostVertical[1][0] -
correctCenter[0]):
correctCenter = centers[i]
self.forceCenter = correctCenter
rospy.logwarn("Closest center to vertical pink line is @: " +
str(correctCenter))
elif cx != None and cy != None:
# checking if curr center is consistent with previous one
centerDist = math.sqrt(
math.pow((self.prevCenter[1] - cy), 2) +
math.pow((self.prevCenter[0] - cx), 2))
if centerDist > 225:
rospy.logwarn("ERROR: ORIGINAL CENTER LOST, showing all " +
str(len(centers)))
for i in range(len(centers)):
cv2.circle(segLineImage, centers[i], 6, (255, 0, 0), -1)
if cx != None and cy != None:
cv2.circle(segLineImage, (cx, cy), 10, (255, 255, 255), -1)
cx = self.prevCenter[0]
cy = self.prevCenter[1]
cv2.circle(segLineImage, (cx, cy), 10, (0, 0, 255), 4)
directiveStatus = -1
return directiveStatus, (0, 0, 0,
0), segLineImage, (cx, cy), 0, 0, None
else:
self.prevCenter = (cx, cy)
if self.orientation == "PARALLEL":
lines, segLineImage = self.processVideo.MultiShowLine(segLineImage,
sort=False)
# pick the pink line closest to the hover platform
angle = None
closest = None
closestDist = None
for line in lines:
if cx != None:
dist = math.sqrt(
math.pow((line[1][1] - cy), 2) +
math.pow((line[1][0] - cx), 2))
if (line != None and line[4] > 30
and (closest == None or (dist < closestDist))):
closest = line
angle = closest[0]
closestDist = dist
if closest != None:
cv2.circle(segLineImage, closest[1], 15, (0, 255, 0), -1)
for line in lines:
if line != None and line[1] != closest[1]:
cv2.circle(segLineImage, line[1], 15, (0, 0, 255), -1)
#converting angle
if angle != None:
# checking if previous angle is consistent with current one
if self.prevAngle == None or min(
abs(self.prevAngle - angle),
180 - abs(self.prevAngle - angle)) < 17:
self.prevAngle = angle
else:
rospy.logwarn(
"ERROR: ORIGINAL CENTER LOST; angle mismatch. Before: "
+ str(self.prevAngle) + " Now: " + str(angle))
directiveStatus = -1
return directiveStatus, (0, 0, 0, 0), segLineImage, ((
cx, cy), self.prevAngle), 0, 0, None
if angle == 90:
angle = 0
elif angle < 90:
angle = angle + 90
else:
angle = angle - 90
yawspeed = self.processVideo.ObjectOrientation(segLineImage,
angle,
4,
yawspeed=0.50)
#.42
if yawspeed != None:
yawspeed = -1 * yawspeed
xWindowSize = 60
yWindowSize = 105 + self.ySizeOffset
xWindowOffset = 0
yWindowOffset = self.yOffset
altLowerTolerance = self.heightTolerance
altUpperTolerance = self.heightTolerance - 15
# defines window to make the drone focus on moving away from the edges and back into
# the center; yaw will be turned off
xReturnSize = xWindowSize + 210
yReturnSize = yWindowSize + 110
elif self.orientation == "PERPENDICULAR":
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (2, 2))
segLineImage = cv2.morphologyEx(segLineImage, cv2.MORPH_OPEN,
kernel)
lines, segLineImage = self.processVideo.MultiShowLine(segLineImage,
sort=False)
# pick the blue line closest to the hover platform, AND is right of the hover platform
angle = None
closest = None
for line in lines:
if (line != None and cx != None and line[1][0] >= cx and
(closest == None
or abs(cx - line[1][0]) < abs(cx - closest[1][0]))):
closest = line
angle = closest[0]
if closest != None:
cv2.circle(segLineImage, closest[1], 15, (0, 255, 0), -1)
for line in lines:
if line != None and line[1] != closest[1]:
cv2.circle(segLineImage, line[1], 15, (0, 0, 255), -1)
#converting angle
if angle != None:
# checking if previous angle is consistent with current one
if self.prevAngle == None or min(
abs(self.prevAngle - angle),
180 - abs(self.prevAngle - angle)) < 27:
self.prevAngle = angle
else:
rospy.logwarn(
"ERROR: ORIGINAL CENTER LOST; angle mismatch. Before: "
+ str(self.prevAngle) + " Now: " + str(angle))
directiveStatus = -1
return directiveStatus, (0, 0, 0, 0), segLineImage, ((
cx, cy), self.prevAngle), 0, 0, None
if angle == 90:
angle = 0
elif angle < 90:
angle = angle + 90
else:
angle = angle - 90
yawspeed = self.processVideo.LineOrientation(segLineImage,
angle,
9,
yawspeed=0.50)
if yawspeed != None:
yawspeed = -1 * yawspeed
xWindowSize = 295
yWindowSize = 70
xWindowOffset = 0
yWindowOffset = 0
altLowerTolerance = 200
altUpperTolerance = 250
# defines window to make the drone focus on moving away from the edges and back into
# the center; yaw will be turned off
xReturnSize = xWindowSize
yReturnSize = yWindowSize
numRows, numCols, _ = image.shape
centerx = numCols / 2 - xWindowOffset
centery = numRows / 2 - yWindowOffset
xspeed, yspeed, zspeed = self.processVideo.ApproximateSpeed(
segLineImage,
cx,
cy,
centerx,
centery,
navdata["SVCLAltitude"][1],
self.hoverAltitude,
xtolerance=xWindowSize,
ytolerance=yWindowSize,
ztolerance=(altLowerTolerance, altUpperTolerance),
xOffset=xWindowOffset,
yOffset=yWindowOffset)
# box defines when the directive is finished
xLower = (numCols / 2) - xReturnSize
yLower = (numRows / 2) - yReturnSize
xUpper = (numCols / 2) + xReturnSize
yUpper = (numRows / 2) + yReturnSize
# perpendicular can disregard height
#if self.orientation == "PERPENDICULAR":
# zspeed = 0
if (yawspeed == 0 and xspeed == 0 and yspeed == 0 and zspeed == 0
and cx != None and cy != None):
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:
# Drone will just go back near the center if: 1) no line is detcted, or 2)
# the drone is not "near" the center as defined by a bounding box
# No turning or altitude change applied
if yawspeed == None or (cx > xUpper or cx < xLower or cy > yUpper
or cy < yLower):
cv2.rectangle(segLineImage, (xLower, yLower), (xUpper, yUpper),
(0, 0, 255), 2)
rospy.logwarn("Too far out; only MOVING drone back to center")
yawspeed = 0
zspeed = zspeed * 0.2
# 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))
self.moveTime = 3.5
xspeed = 0
yspeed = 0
zspeed = zspeed * 0.45
# if the drone is aligned to the line and is near the center,
# keep moving it to the center and adjusting the height until the
# directive is finished
else:
rospy.logwarn("Curr Altitude = " +
str(int(navdata["SVCLAltitude"][1])) +
" mm; Goal = [ " +
str(self.hoverAltitude - altLowerTolerance) +
" mm, " +
str(self.hoverAltitude + altUpperTolerance) +
" mm ].")
directiveStatus = 0
return directiveStatus, (
xspeed, yspeed, yawspeed, zspeed), segLineImage, (
(cx, cy),
self.prevAngle), self.moveTime, self.waitTime, self.forceCenter
def Finished(self):
center = self.prevCenter
self.prevAngle = None
self.prevCenter = None
self.forceCenter = None
return center
def OnErrorReturn(self, returnData):
# set previous center to what was found in the error algorithm
rospy.logwarn("ORIENT LINE ON ERROR RETURN***")
self.prevCenter = returnData
self.prevAngle == None
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 ReturnToColorDirective(AbstractDroneDirective):
# sets up this directive
# platformColor: color of the platform to return to
def __init__(self, platformColor, lineColor, speedModifier = 0.6, radiusThresh = 170):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.lineColor = lineColor
self.moveTime = 0.35
self.waitTime = 0.10
self.bestPlatformFound = None
def InsideCircle(self, point, circleCenter, circleRadius):
x = point[0]
y = point[1]
center_x = circleCenter[0]
center_y = circleCenter[1]
radius = circleRadius
return (math.pow((x-center_x),2) + math.pow((y-center_y),2)) < math.pow(radius,2)
def PointAlongLine(self, linePoint, lineAngle, point, thresh):
# edge case: both lines are vertical
if point[0] == linePoint[0]:
if lineAngle == 90:
return True
else:
return False
else:
# slope compensates for a upper left origin coord system
slope = (float(linePoint[1])-point[1]) / (point[0]-float(linePoint[0]))
slopeAngle = math.degrees(math.atan(slope))
if slopeAngle < 0:
#rospy.logwarn("seen added 180")
slopeAngle += 180
#rospy.logwarn(str(point) + str(linePoint) +" >> slope: " + str(slope))
#rospy.logwarn("Original: " + str(lineAngle) + " seen: " + str(slopeAngle))
if( min( abs( lineAngle - slopeAngle), 180 - abs( lineAngle - slopeAngle) ) < thresh):
#rospy.logwarn("GOOD")
return True
else:
#rospy.logwarn("BAD")
return False
# 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 hasn't returned to the color yet
# 1 if algorithm is finished and drone is now over the color
#
# 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):
lineSeg = self.processVideo.DetectColor(image, self.lineColor)
#platformSeg = self.processVideo.DetectColor(image, self.platformColor)
#platformSeg = self.processVideo.RemoveNoise(platformSeg)
#platformSeg = navdata[0]["segImage"]
#centers, _ = self.processVideo.MultiCenterOfMass(platformSeg)
centers = navdata[0]["allCenters"][1]
#navdata stores the last location in the case of an error
cx = navdata[1][0][0]
cy = navdata[1][0][1]
lastAngle = navdata[1][1]
hasPlatform = False
# find last platform based on last seen line angle
if lastAngle != None:
lines, image = self.processVideo.MultiShowLine(lineSeg, sort = False)
for center in centers:
cv2.circle(image, center, 4, (255,255,255), -1)
thresh = 21
validLine = None
# picks the line closest to last angle, and within thresh (degrees)
for line in lines:
#rospy.logwarn("last: " + str(lastAngle) + " this: " + str(line[0]))
angleDifference = min( abs(lastAngle -line[0]), 180 -abs(lastAngle -line[0]) )
if( line != None and angleDifference < thresh and (validLine == None or validLine[1] > angleDifference) ):
#rospy.logwarn("valid")
validLine = (line, angleDifference)
if validLine != None:
line = validLine[0]
# finding center closest to the most valid line
for c in centers:
alongLine = self.PointAlongLine(line[1], line[0], c, 25)
# blue line => orient perpendicular => valid point must be to the left of line
if self.lineColor == "blue" and c[0] > line[1][0]:
alongLine = False
if alongLine:
lastAngle = line[0]
cv2.line(image, line[1], c, (0,255,255),3)
cx, cy = c[0], c[1]
cv2.circle(image, (cx,cy), 12, (0,255,0), -1)
cv2.circle(image, (cx,cy), 12, (255,255,255), 7)
cv2.circle(image, line[1], 7, (0,255,0), -1)
cv2.circle(image, line[1], 7, (255,255,255), 4)
hasPlatform = True
# if no angle was found, just use location
else:
image = navdata[0]["segImage"]
cv2.circle(image, (cx,cy), self.radiusThresh, (0,255,0), 1)
cv2.circle(image, (cx,cy), 7, (0,255,0), -1)
for c in centers:
cv2.circle(image, c, 10, (0,255,255), -1)
if self.InsideCircle( c , (cx,cy), self.radiusThresh):
hasPlatform = True
cx, cy = c[0], c[1]
if hasPlatform:
rospy.logwarn("Successfully returned to platform -- last angle seen was "+ str(lastAngle))
directiveStatus = 1
zspeed = 0
else:
rospy.logwarn("Returning to platform -- last angle seen was "+ str(lastAngle))
directiveStatus = 0
zspeed = 0.2
xspeed, yspeed, _ = self.processVideo.ApproximateSpeed(image.copy(), cx, cy,
ytolerance = 50, xtolerance = 50)
xspeed = min( xspeed * self.speedModifier, 1 )
yspeed = min( yspeed * self.speedModifier, 1 )
rospy.logwarn("X Speed: " + str(xspeed) + " Y Speed: " + str(yspeed))
# draw rectangles so it's easy to tell that it's in return mode
border = 15
offset = 2
cv2.rectangle(image, (border, border), (640-border,360-border), (0,0, 255), 1)
cv2.rectangle(image, (border-1*offset, border-1*offset), (640-border+1*offset,360-border+1*offset), (0,229, 255), 1)
cv2.rectangle(image, (border-2*offset, border-2*offset), (640-border+2*offset,360-border+2*offset), (0,0, 255), 1)
return directiveStatus, (xspeed, yspeed, 0, zspeed), image, ((cx,cy), lastAngle), self.moveTime, self.waitTime, None
def Finished(self):
rospy.logwarn("RETURN TO COLOR FINISHED *******************")
rospy.logwarn("RETURN TO COLOR FINISHED *******************")
self.bestPlatformFound = None
return None
class ReturnToLineDirective(AbstractDroneDirective):
# sets up this directive
# platformColor: color of the platform to return to
def __init__(self, lineColor, speedModifier=0.5, radiusThresh=255):
self.lineColor = lineColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.moveTime = 0.25
self.waitTime = 0.10
def InsideCircle(self, point, circleCenter, circleRadius):
x = point[0]
y = point[1]
center_x = circleCenter[0]
center_y = circleCenter[1]
radius = circleRadius
return (math.pow((x - center_x), 2) + math.pow(
(y - center_y), 2)) < math.pow(radius, 2)
# 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 hasn't returned to the line yet
# 1 if algorithm is finished and drone is now over the color
#
# 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)
lines, image = self.processVideo.MultiShowLine(segLineImage)
#navdata stores the last location and angle in the case of an error
cx = navdata[1][0][0]
cy = navdata[1][0][1]
angle = navdata[1][1]
#cv2.circle(image, (cx,cy), self.radiusThresh, (0,255,0), 1)
hasPlatform = False
# thresh in degrees
thresh = 18
for line in lines:
if line != None:
# original line was found if angle matches original, to some threshold
if ((abs(angle - line[0]) < thresh or abs(angle - line[0]) >
(180 - thresh))):
hasPlatform = True
cv2.circle(image, line[1], 15, (0, 255, 0), -1)
cx = line[1][0]
cy = line[1][1]
else:
cv2.circle(image, line[1], 15, (0, 0, 255), 5)
if hasPlatform:
rospy.logwarn("Returned to " + self.lineColor + " line")
directiveStatus = 1
zspeed = 0
else:
rospy.logwarn("Returning to " + self.lineColor + " line")
directiveStatus = 0
zspeed = 0.1
if cx == None or cy == None:
rospy.logwarn("Returning -- no " + self.lineColor +
" detected @ this altitude, increasing altitude")
return 0, (0, 0, 0, 0.5), image, (cx, cy), 0, 0
xspeed, yspeed, _ = self.processVideo.ApproximateSpeed(image,
cx,
cy,
ytolerance=50,
xtolerance=50)
yspeed = min(yspeed * self.speedModifier, 1)
xspeed = min(xspeed * self.speedModifier, 1)
rospy.logwarn("X Speed: " + str(xspeed) + " Y Speed: " + str(yspeed))
self.processVideo.DrawCircle(image, (cx, cy))
return directiveStatus, (xspeed, yspeed, 0, zspeed), image, ((
cx, cy), angle), self.moveTime, self.waitTime, None
class FollowLineDirective(AbstractDroneDirective):
# sets up this directive
# lineColor: color of the line to follow
def __init__(self, lineColor, speed=0.4):
self.lineColor = lineColor
self.speed = speed
self.processVideo = ProcessVideo()
self.moveTime = 0.45
self.waitTime = 0.1
self.prevAngle = None
self.prevAngleCount = 0
self.prevAngleCountMax = 185
# 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 is still following line
# 1 if algorithm is finished and has finished following line
#
# 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)
platforms = navdata["allCenters"][1]
lines, image = self.processVideo.MultiShowLine(segLineImage)
if lines[0] != None:
cv2.circle(image, lines[0][1], 15, (0, 0, 255), -1)
if lines[1] != None:
cv2.circle(image, lines[1][1], 15, (0, 255, 0), -1)
if lines[2] != None:
cv2.circle(image, lines[2][1], 15, (255, 0, 0), -1)
linesVisible = (lines[0] != None) + (lines[1] != None) + (lines[2] !=
None)
if linesVisible == 0:
rospy.logwarn(" *** ERROR: Lost " + self.lineColor + " line *** ")
return -1, (0, 0, 0, 0), segLineImage, (None, None), 0, 0, None
cx = lines[1][1][0]
cy = lines[1][1][1]
_, yspeed, _ = self.processVideo.ApproximateSpeed(
segLineImage,
cx,
cy,
None,
None,
navdata["SVCLAltitude"][1],
0,
xtolerance=80,
ytolerance=95)
newCenter = None
thresh = 15
# Checking if angle is consistent with before
if (self.prevAngle == None
or abs(self.prevAngle - lines[1][0]) < thresh
or abs(self.prevAngle - lines[1][0]) > (180 - thresh)):
self.prevAngle = lines[1][0]
self.prevAngleCount = 0
directiveStatus = 0
# checking finish conditions
# alternate way to finish
xWindowSize = 200
yWindowSize = 120
xLower = 320 - xWindowSize
yLower = 180 - yWindowSize
xUpper = 320 + xWindowSize
yUpper = 180 + yWindowSize
foundRightPlatform = False
tolerance = 15
for platform in platforms:
if (linesVisible > 1 and lines[1] != None
and platform[0] > lines[1][1][0]
and min(abs(lines[1][0] - 180),
180 - abs(lines[1][0] - 180)) < 16
and platform[0] < xUpper and platform[0] > xLower
and platform[1] < yUpper and platform[1] > yLower):
cv2.rectangle(image, (xLower, yLower), (xUpper, yUpper),
(255, 255, 255), 4)
foundRightPlatform = True
newCenter = platform
# in order to be considered "finished", there must be 2 lines,
# one which is horizontal and one that is less than 90 degrees.
# The horizontal line must be far enough left.
if (foundRightPlatform or
(len(platforms) == 1 and yspeed == 0 and lines[1] != None
and lines[2] != None and
((lines[1][0] < (0 + tolerance)) or (lines[1][0]) >
(180 - tolerance)) and lines[2][1][0] < int(640 * 0.9))):
xspeed = 0
yspeed = 0
yawspeed = 0
directiveStatus = 1
rospy.logwarn("Finished following line")
return directiveStatus, (xspeed, yspeed, yawspeed, 0), image, (
(cx, cy),
self.prevAngle), self.moveTime, self.waitTime, newCenter
else:
# only uses the angle if it is similar to the last one. If it is too different, algorithm
# uses the previous angle for a time until the timer is up, as a buffer against a random large angle change
rospy.logwarn(
"Large sudden change in angle -- using old angle of " +
str(self.prevAngle) + " instead of " + str(lines[1][0]))
directiveStatus = -1
xspeed = -self.speed
# converting
line1Angle = self.prevAngle
if line1Angle == 90:
line1Angle = 0
elif line1Angle < 90:
line1Angle = line1Angle + 90
else:
line1Angle = line1Angle - 90
yawspeed = self.processVideo.LineOrientation(segLineImage,
line1Angle,
8,
yawspeed=0.45)
# If drone is still trying follow the line, it adapts to one of three algorithms:
# Drone will just go back near the center if the drone is not "near" the
# center as defined by a bounding box.
# No turning or horizontal movement is applied.
if yspeed != 0:
rospy.logwarn("Moving blue line back to center")
self.moveTime = 0.2
self.waitTime = 0.1
xspeed = 0
yawspeed = 0
# If drone is near the center but angle is off, it fixes the angle.
# Drone does not move forward.
elif yawspeed != 0:
yawspeed = -yawspeed
direction = "LEFT"
if yawspeed < 0:
direction = "RIGHT"
rospy.logwarn("Turning the drone horizontal " + direction +
", yaw = " + str(yawspeed))
self.moveTime = 1
self.waitTime = 0.1
xspeed = 0
else:
rospy.logwarn("Drone just going forward")
self.moveTime = 0.9
self.waitTime = 0.1
return directiveStatus, (xspeed, yspeed, yawspeed, 0), image, ((
cx, cy), self.prevAngle), self.moveTime, self.waitTime, newCenter
def Finished(self):
self.prevAngle = None
self.prevAngleCount = 0
return None