class PIDYawDirective(AbstractDroneDirective):
# sets up this directive
# plrratformColor: color to hover over. Altitude is maintained
def __init__(self, poseTracker, target, yaw, platformNumber, waitDist=0.1):
#self.Kp,self.Ki,self.Kd = 0.1,20.0,0.0005 #best
self.Kp, self.Ki, self.Kd = 0.2, 0.0, 0.0005
self.moveTime = 0.2
self.waitTime = 0.0
self.tracker = poseTracker
self.target = target
self.waitDist = waitDist
self.worldTarget = self.tracker.body2World(target)[:, 0]
self.processVideo = ProcessVideo()
self.platformNumber = platformNumber
self.centery = 360 / 2.0
self.centerx = 640 / 2.0
self.pub = rospy.Publisher('ardrone/tracker', tracker)
self.track = tracker()
self.platform = [0, 0, 0]
self.filterSize = 50
self.buff = np.repeat(np.asarray([self.worldTarget]).T,
self.filterSize,
axis=1)
self.KpYaw, self.KiYaw, self.KdYaw = (1 / 180.0) * 0.8, 0, 0
self.targetYaw = (yaw + 360) % 360
self.worldPoint = np.asarray([[0, 0, 0]]).T
#the amount of weight we would like to put towards correcting the drones drift by recognizing landmarks
self.correctionRatio = 0.9999
def distance(self, x, y):
dist = (x[0] - y[0])**2 + (x[1] - y[1])**2
dist = dist**(0.5)
return dist
def weightedUpdate(self, prediction, updateTerm):
return (self.correctionRatio * updateTerm[0, 0] +
(1 - self.correctionRatio) * prediction[0, 0],
self.correctionRatio * updateTerm[1, 0] +
(1 - self.correctionRatio) * prediction[1, 0],
updateTerm[2, 0], 1.0)
# 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):
segImage, radius, center = self.processVideo.RecognizeShape(
image, 'orange', (None, None))
blue = self.processVideo.detectColor(image, 'blue', 'segmented')
lines, blueImg = self.processVideo.MultiShowLine(blue)
bestTheta = None
minDist = -1
for line in lines:
theta = line[0]
theta = -theta
tapeCenter = line[1]
dist = self.distance(center, tapeCenter)
if minDist == -1 or dist < minDist:
minDist = dist
bestTheta = theta
if bestTheta != None:
self.currentYaw = bestTheta
else:
self.currentYaw = 0
#Calculate closest rotation to get to target angle
theta = ((0 - self.currentYaw) % 360 + 360) % 360
theta = (theta - 360) if (theta > 180) else theta
loc = (0, 0, 0, 0)
#circle detection
#rospy.logwarn("x: "+str(self.tracker.translation[0])+" y: "+str(self.tracker.translation[1]))
if radius != None:
predictedZ = self.processVideo.CalcDistanceNew(88.0,
radius * 2) / 1000.0
scale = (88.0 / (radius * 2)) / 1000.0 #meters/pixel
x = (center[0] - self.centerx) * scale
y = (self.centery - center[1]) * scale
tape = self.tracker.camera2Body([x, y, -predictedZ])
worldPoint = self.tracker.camera2World([x, y, -predictedZ])
self.worldPoint = worldPoint
if (self.distance(worldPoint, self.worldTarget) < 0.35):
for i in range(self.filterSize - 1):
self.buff[:, i] = self.buff[:, i + 1]
self.buff[:, self.filterSize - 1] = np.asarray(
[worldPoint[0, 0], worldPoint[1, 0], worldPoint[2, 0]])
self.worldTarget = np.mean(self.buff, 1)
'''
if self.tapeLocation != None:
dist = self.distance(worldPoint,self.tapeLocation)
if dist < 0.35 and dist > 0.15:
loc = self.tracker.tape2World([x,y,-predictedZ],self.yaw,[self.tapeLocation[0],self.tapeLocation[1],0])
loc = self.weightedUpdate(worldPoint,loc)
rospy.logwarn("Fixing location to ..."+str(loc))
'''
self.track.landMark = (self.worldTarget[0], self.worldTarget[1],
0.0, 1.0)
else:
self.track.landMark = (0, 0, 0, 0.0)
#rospy.logwarn("world target: " + str(self.worldTarget))
self.track.landMark = (self.worldTarget[0], self.worldTarget[1], 0.0,
1.0)
self.track.loc = loc
self.pub.publish(self.track)
self.currentTarget = self.tracker.world2Body(self.worldTarget)
self.currentTime = time.time()
if self.lastTime == 0:
self.rollError = 0
self.pitchError = 0
self.yawError = 0
else:
self.rollError = self.currentTarget[0]
self.pitchError = self.currentTarget[1]
self.yawError = theta
self.dt = (self.currentTime - self.lastTime) / 1000.
self.totalError = [
self.totalError[0] + self.rollError * self.dt,
self.totalError[1] + self.pitchError * self.dt,
self.totalError[2] + self.yawError * self.dt, 0
]
pRoll = -self.Kp * (self.rollError)
iRoll = -self.Ki * (self.totalError[0])
dRoll = -self.Kd * ((self.rollError - self.lastError[0]) / self.dt)
pPitch = self.Kp * (self.pitchError)
iPitch = self.Ki * (self.totalError[1])
dPitch = self.Kd * ((self.pitchError - self.lastError[1]) / self.dt)
pYaw = self.KpYaw * (self.yawError)
iYaw = self.KiYaw * (self.totalError[2])
dYaw = self.KdYaw * ((self.yawError - self.lastYawError) / self.dt)
yaw = pYaw + iYaw + dYaw
self.lastError = self.currentTarget
self.lastYawError = self.yawError
self.lastTime = self.currentTime
roll = pRoll + iRoll + dRoll
pitch = pPitch + iPitch + dPitch
if (abs(self.rollError) <= self.waitDist
and abs(self.pitchError) <= self.waitDist
and abs(self.yawError) < 2):
directiveStatus = 1
rospy.logwarn(self.yawError)
else:
directiveStatus = 0
#Trim commands over the drones command limit
roll = 1 if roll > 1 else roll
roll = -1 if roll < -1 else roll
pitch = 1 if pitch > 1 else pitch
pitch = -1 if pitch < -1 else pitch
#rospy.logwarn("roll: "+str(self.tracker.roll))
#rospy.logwarn("pitch: "+str(self.tracker.pitch))
rospy.logwarn(directiveStatus)
return directiveStatus, (
roll, pitch, 0,
0), segImage, None, self.moveTime, self.waitTime, None
# This method is called by the state machine when it considers this directive finished
def Finished(self):
self.Reset()
#tapeLocation = self.tracker.body2World(self.target)[:,0]
#loc = self.tracker.tape2World([x,y,-predictedZ],self.yaw,[tapeLocation[0],tapeLocation[1],0])
if (self.platformNumber % 3 == 0):
loc = np.asarray([self.target]).T
loc[2] = 1.0
rospy.logwarn("Reseting location to" + str(loc))
loc = self.weightedUpdate(self.worldPoint, loc)
self.track.loc = loc
self.pub.publish(self.track)
def Reset(self):
self.dt = 0
self.currentTime = time.time()
self.lastTime = 0
self.rollError = 0
self.pitchError = 0
self.lastError = [0, 0, 0]
self.lastYawError = 0
self.totalError = [0, 0, 0, 0]
class FlightstatsReceiver(object):
def __init__(self):
super(FlightstatsReceiver, self).__init__()
# Subscribe to the navdata topic, and call self.ProcessNavdata whenever
# update navdata messages are recieved
self.navdataSub = rospy.Subscriber('/ardrone/navdata', Navdata,
self.UpdateNavdata)
self.altitudeSub = rospy.Subscriber('/ardrone/navdata_altitude',
navdata_altitude,
self.UpdateAltitude)
self.video = rospy.Subscriber('/ardrone/image_raw', Image,
self.VideoUpdate)
# dictionary will hold a useful subset of available flight info
# Key is the variable name; value is (description, value, units, (optional) direction string following units)
self.defaultValue = "?"
self.flightInfo = collections.OrderedDict()
self.flightInfo["batteryPercent"] = [
"Battery Left: ", self.defaultValue, "%", ""
]
self.flightInfo["state"] = ["Status: ", self.defaultValue, "", ""]
self.flightInfo["altitude"] = [
"Drone Altitude: ", self.defaultValue, "mm", ""
]
self.flightInfo["altitude_raw"] = [
"Drone Raw Altitude: ", self.defaultValue, "mm", ""
]
self.flightInfo["SVCLAltitude"] = ["SVCL Altitude: ", -1, "mm", ""]
self.flightInfo["center"] = [
"Inferred Center: ", self.defaultValue, "", ""
]
self.flightInfo["lastCenter"] = [
"Previous Center: ", (None, None), "", ""
]
self.flightInfo["lastRecordedCenter"] = [
"Last Recorded Algorithm Center: ", (None, None), "", ""
]
self.flightInfo["allCenters"] = [
"All Centers: ", self.defaultValue, "", ""
]
self.flightInfo["rotX"] = [
"Left/Right Tilt: ", self.defaultValue, u'\N{DEGREE SIGN}', ""
]
self.flightInfo["rotY"] = [
"Front/Back Tilt: ", self.defaultValue, u'\N{DEGREE SIGN}', ""
]
self.flightInfo["rotZ"] = [
"Rotation Amount: ", self.defaultValue, u'\N{DEGREE SIGN}', ""
]
self.flightInfo["velY"] = [
"Left/Right Velocity: ", self.defaultValue, "mm/s", ""
]
self.flightInfo["velX"] = [
"Forwards/Backwards Velocity: ", self.defaultValue, "mm/s", ""
]
self.flightInfo["velZ"] = [
"Up/Down Velocity: ", self.defaultValue, "mm/s", ""
]
self.flightInfo["accelZ"] = [
"Up/Down Acceleration: ", self.defaultValue, "mm/s", ""
]
self.flightInfo["dispLR"] = [
"Left/Right Displacement: ", self.defaultValue, "mm", ""
]
self.flightInfo["dispFB"] = [
"Forwards/Backwards Displacement: ", self.defaultValue, "mm", ""
]
self.flightInfo["dispUD"] = [
"Up/Down Displacement: ", self.defaultValue, "mm", ""
]
self.flightInfo["segImage"] = [None]
self.LRDisplacement = 0.0
self.FBDisplacement = 0.0
self.UDDisplacement = 0.0
self.oldTime = rospy.Time.now()
self.bridge = CvBridge()
self.processVideo = ProcessVideo()
# sometimes the altitude doesn't start at 0; "zero balances" the drone such that where it started is considered 0 altitude
self.zeroBalanced = False
self.zeroAltitude = 0
# counter is designed to throttle the lag that comes with executing videoupdate too often
# a higher videoUpdateMax will make everything run faster, but getting height/center updates will be slower
# describes a ratio: compute/rest
self.computeMax = 1
self.restMax = 0
self.counter = 0
self.lastLocation = (None, None)
self.lastLoc = (None, None)
self.lastCenter = (None, None)
self.lastCenterCount = 0
self.lastCenterMax = 8
def VideoUpdate(self, image):
if (self.counter < self.computeMax) or self.restMax == 0:
self.counter += 1
# converting to hsv
image = self.bridge.imgmsg_to_cv2(image, "bgr8")
segImage, radius, center = self.processVideo.RecognizeShape(
image, 'orange', self.lastLocation)
(self.flightInfo["center"][1]) = self.InferCenter(
self.processVideo.RemoveNoise(segImage))
if radius == None:
if center == (None, None):
distance = -1
(self.flightInfo["SVCLAltitude"])[1] = distance
else:
distance = self.processVideo.CalcDistanceNew(88, radius * 2)
(self.flightInfo["SVCLAltitude"])[1] = distance
#(self.flightInfo["center"])[1] = center
self.lastLocation = center
(self.flightInfo["segImage"]) = segImage
else:
if self.counter < self.computeMax + self.restMax:
self.counter += 1
if self.counter >= self.computeMax + self.restMax:
self.counter = 0
# Uses a more complex way to infer what platform to use. If there is only one platform, this is trivial,
# but becomes more useful if there are >1 visible.
def InferCenter(self, image):
centers, _ = self.processVideo.MultiCenterOfMass(image)
(self.flightInfo["allCenters"][1]) = centers
if len(centers) == 0:
#rospy.logwarn("No Platform")
center = (None, None)
elif len(centers) == 1:
center = centers[0]
#rospy.logwarn("Found 1")
elif len(centers) == 2:
if self.lastLoc != (None, None):
#rospy.logwarn("Found 2 -- picking closer to last")
# just pick whichever is closer to the last center
c1Dist = math.sqrt(
math.pow((self.lastLoc[1] - centers[0][1]), 2) +
math.pow((self.lastLoc[0] - centers[0][0]), 2))
c2Dist = math.sqrt(
math.pow((self.lastLoc[1] - centers[1][1]), 2) +
math.pow((self.lastLoc[0] - centers[1][0]), 2))
if c1Dist < c2Dist:
center = centers[0]
else:
center = centers[1]
else:
#rospy.logwarn("Found 2 -- picking closer to center")
# just pick whichever's x-coord is nearer to center
xCenter = 320
c1XDist = abs(xCenter - centers[0][0])
c2XDist = abs(xCenter - centers[1][0])
if c1XDist < c2XDist:
center = centers[0]
else:
center = centers[1]
# if there are 3 or more platforms
else:
centers = sorted(centers, key=self.getX)
if len(centers) % 2 == 0:
midX = int((centers[len(centers) / 2][0] +
centers[(len(centers) / 2) + 1][0]) / 2)
midY = int((centers[len(centers) / 2][1] +
centers[(len(centers) / 2) + 1][1]) / 2)
else:
midX = centers[int((len(centers) / 2.0) + 0.5) - 1][0]
midY = centers[int((len(centers) / 2.0) + 0.5) - 1][1]
center = (midX, midY)
#rospy.logwarn("Found " + str(len(centers)) + ": " + str(centers))
#rospy.logwarn("Using " + str(center))
if len(centers) > 1 and self.lastCenter != (None, None):
closest = None
smallestLen = None
# loop to pick whichever is closer to the last center
for i in range(len(centers)):
cXDist = abs(self.lastCenter[0] - centers[i][0])
cYDist = abs(self.lastCenter[1] - centers[i][1])
length = math.sqrt(math.pow(cXDist, 2) + math.pow(cYDist, 2))
if smallestLen == None or length < smallestLen:
closest = centers[i]
smallestLen = length
self.lastCenter = closest
(self.flightInfo["lastCenter"][1]) = self.lastCenter
# algorithm waits for a brief moment if last location changes drastically to make
# sure that it really changed
if self.lastLoc != (None, None) and center != (None, None):
dist = math.sqrt(
math.pow((self.lastLoc[1] - center[1]), 2) +
math.pow((self.lastLoc[0] - center[0]), 2))
if dist > 140:
self.lastCenterCount += 1
rospy.logwarn("visible: " + str(len(centers)) +
" change Center Count: " +
str(self.lastCenterCount) + " / " +
str(self.lastCenterMax))
if self.lastCenterCount >= self.lastCenterMax:
rospy.logwarn("Changing center")
self.lastCenterCount = 0
# saving last center in case
self.lastCenter = self.lastLoc
self.lastLoc = center
else:
self.lastCenterCount = 0
self.lastLoc = center
else:
self.lastLoc = center
#return center
return self.lastLoc
def getX(self, coordinates):
return coordinates[0]
# force set what the center is
def SetCenter(self, center):
self.lastLoc = center
def UpdateRecordedCenter(self, center):
(self.flightInfo["lastRecordedCenter"][1]) = center
def UpdateAltitude(self, altitude):
if self.zeroBalanced:
(self.flightInfo["altitude_raw"]
)[1] = altitude.altitude_raw - self.zeroAltitude
else:
self.zeroBalanced = True
self.zeroAltitude = altitude.altitude_raw
# update dictionary as new info from drone comes
def UpdateNavdata(self, navdata):
# first, update instance variables
(self.flightInfo["batteryPercent"])[1] = navdata.batteryPercent
(self.flightInfo["state"])[1] = navdata.state
#(self.flightInfo["altitude"])[1] = navdata.altd
(self.flightInfo["rotX"])[1] = navdata.rotX
(self.flightInfo["rotY"])[1] = navdata.rotY
(self.flightInfo["rotZ"])[1] = navdata.rotZ
(self.flightInfo["velX"])[1] = navdata.vx
(self.flightInfo["velY"])[1] = navdata.vy
(self.flightInfo["velZ"])[1] = navdata.vz
(self.flightInfo["altitude"])[1] = navdata.altd
(self.flightInfo["accelZ"])[1] = navdata.az
dt = rospy.Time.now() - self.oldTime
# Calculating horizontal displacement
currLRVelocity = self.flightInfo["velY"][1]
self.LRDisplacement += float(currLRVelocity) * dt.to_sec()
(self.flightInfo["dispLR"])[1] = self.LRDisplacement
# Calculating vertical displacement
currFBVelocity = self.flightInfo["velX"][1]
self.FBDisplacement += float(currFBVelocity) * dt.to_sec()
(self.flightInfo["dispFB"])[1] = self.FBDisplacement
# Calculating Z displacement
currUDAcceleration = self.flightInfo["accelZ"][1]
self.UDDisplacement += float(currUDAcceleration * dt.to_sec() *
dt.to_sec())
(self.flightInfo["dispUD"])[1] = self.UDDisplacement
self.oldTime = rospy.Time.now()