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 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 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
class ResumePlatformDirective(AbstractDroneDirective):
# sets up this directive
# platformColor: color of the platform to return to
def __init__(self, platformColor, speedModifier=0.3, radiusThresh=155):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.moveTime = 0.20
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 identified the previous platform
# 1 if algorithm is finished and drone is now over the previous platform
#
# 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"]
centers = navdata["allCenters"][1]
# contains last platform location
cx = navdata["lastRecordedCenter"][1][0]
cy = navdata["lastRecordedCenter"][1][1]
if cx == None or cy == None:
rospy.logwarn("ERROR")
return -1, (0, 0, 0, 0.5), image, (cx, cy), 0, 0
cv2.circle(image, (cx, cy), self.radiusThresh, (0, 255, 0), 1)
cv2.circle(image, (cx, cy), 7, (0, 255, 0), -1)
resumed = False
for c in centers:
cv2.circle(image, c, 10, (0, 255, 255), -1)
if self.InsideCircle(c, (cx, cy), self.radiusThresh):
resumed = True
cx, cy = c[0], c[1]
if resumed:
rospy.logwarn("Over last platform")
directiveStatus = 1
zspeed = 0
else:
rospy.logwarn("Returning to last platform -- increasing altitude")
directiveStatus = 0
zspeed = 0.5
#todo: incorrect calculation. need to change center to last, not center
xspeed, yspeed, _ = self.processVideo.ApproximateSpeed(image.copy(),
cx,
cy,
ytolerance=0,
xtolerance=0)
xspeed = xspeed * self.speedModifier
yspeed = yspeed * self.speedModifier
rospy.logwarn("X Speed: " + str(xspeed) + " Y Speed: " + str(yspeed))
return directiveStatus, (xspeed, yspeed, 0, zspeed), image, (
cx, cy), self.moveTime, self.waitTime, None