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 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 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