def hoverAboveRoundel( drone, timeout=6000.0 ):
startTime = drone.time
maxSpeed = 0.1
maxSpeedUpDown = 0.3
frac = 0.1
scaleX, scaleY = 0.38, 0.38 # camera FOW at 1m above the ground
detectedCount = 0
goal = (0.0, 0.0) #None
goalHeading = None
while drone.time - startTime < timeout:
sx, sy, sz, sa = 0.0, 0.0, 0.0, 0.0
if drone.visionTag:
# xc[i], yc[i]: X and Y coordinates of detected tag or oriented roundel #i inside the picture,
# with (0; 0) being the top-left corner, and (1000; 1000) the right-bottom corner regardless
# the picture resolution or the source camera
x, y = drone.visionTag[0][:2]
dist = drone.visionTag[0][4]/100.
angle = math.radians(drone.visionTag[0][5] - 270) # we use circle for the body and line for the tail -> 270deg offset
detectedCount = min(100, detectedCount + 1)
# TODO verify signs and scale
dx = dist*(scaleY*(500-y)/500.0 + drone.angleFB)
dy = dist*(scaleX*(500-x)/500.0) # - drone.angleLR)
c = math.cos(drone.heading)
s = math.sin(drone.heading)
goal = (drone.coord[0] + c*dx - s*dy,
drone.coord[1] + s*dx + c*dy )
goalHeading = normalizeAnglePIPI( drone.heading + angle ) # TODO check sign
# print drone.visionTag, "%.2f %.2f %.1f %.1f" % (dx, dy, math.degrees(drone.angleFB), math.degrees(drone.angleLR))
else:
detectedCount = max(0, detectedCount - 1)
if goal:
sx,sy,sz,sa = getXYZAGoalCmd( drone, goal, goalHeading=goalHeading )
sz = 0.0
if drone.altitudeData != None:
altVision = drone.altitudeData[0]/1000.0
altSonar = drone.altitudeData[3]/1000.0
if drone.altitudeData[3] == 0:
# no sonar echo
minAlt = altVision
else:
minAlt = min(altSonar, altVision)
if minAlt > MAX_ALTITUDE:
# too high to reliably detect anything
sz = -maxSpeedUpDown
# print altSonar, altVision,
if detectedCount == 0:
# try to move up
if max(altSonar, altVision) < 2.0:
sz = maxSpeedUpDown
elif detectedCount == 100:
if minAlt > 1.0:
sz = -maxSpeedUpDown
# print "\t%.2f %.2f %.2f %.2f" % (sx, sy, sz, sa)
drone.moveXYZA( sx, sy, sz, sa )
drone.hover(0.1) # stop motion
def diff2pathType( self, dx, dy, da ):
da = normalizeAnglePIPI(da)
if (0.25 < dx < 0.48) and abs(da) < math.radians(50) and (abs(dy) < 0.25):
if abs(da) < math.radians(10):
return PATH_STRAIGHT
elif da > 0:
return PATH_TURN_LEFT
else:
return PATH_TURN_RIGHT
return None
def getXYZAGoalCmd( drone, goal, goalHeading=None, maxSpeed=0.3 ):
frac = 0.3
dxWorld = goal[0] - drone.coord[0]
dyWorld = goal[1] - drone.coord[1]
c = math.cos(drone.heading)
s = math.sin(drone.heading)
dx = c*dxWorld + s*dyWorld # inverse rotation
dy = -s*dxWorld + c*dyWorld
sx = max( -maxSpeed, min( maxSpeed, frac*(dx - drone.vx) ))
sy = max( -maxSpeed, min( maxSpeed, frac*(dy - drone.vy) ))
sa = 0.0
if goalHeading:
sa = max( -maxSpeed, min( maxSpeed, frac*(normalizeAnglePIPI(goalHeading - drone.heading)) ))
return sx, sy, 0.0, sa
def getXYZAGoalCmd(drone, goal, goalHeading=None, maxSpeed=0.3):
frac = 0.3
dxWorld = goal[0] - drone.coord[0]
dyWorld = goal[1] - drone.coord[1]
c = math.cos(drone.heading)
s = math.sin(drone.heading)
dx = c * dxWorld + s * dyWorld # inverse rotation
dy = -s * dxWorld + c * dyWorld
sx = max(-maxSpeed, min(maxSpeed, frac * (dx - drone.vx)))
sy = max(-maxSpeed, min(maxSpeed, frac * (dy - drone.vy)))
sa = 0.0
if goalHeading:
sa = max(
-maxSpeed,
min(maxSpeed,
frac * (normalizeAnglePIPI(goalHeading - drone.heading))))
return sx, sy, 0.0, sa
def updateFrame( self, pose, frameStrips, verbose=False ):
if verbose:
print pose, [str(p) for p in frameStrips]
self.pathUpdated = False
self.crossing = False
for i in xrange(len(frameStrips)):
for j in xrange(len(frameStrips)):
if i != j:
(dx,dy,da) = frameStrips[i].sub(frameStrips[j])
pt = self.diff2pathType( dx, dy, da )
if pt:
sPose = pose.add( frameStrips[i] )
if pt != PATH_STRAIGHT or self.refLine == None or \
abs(normalizeAnglePIPI( self.refLine.angle - sPose.heading )) < REF_LINE_CROSSING_ANGLE:
self.pathType = pt
self.pathPose = sPose # also j should be OK
self.pathUpdated = True
break
else:
print "SKIPPED2"
self.crossing = True
if (len(frameStrips) >= 1 and not self.pathUpdated) and self.lastStripPose != None:
for fs in frameStrips:
for lsp in self.lastStripPose:
sPose = pose.add( fs )
(dx,dy,da) = sPose.sub( lsp )
pt = self.diff2pathType( dx, dy, da )
if pt:
if pt != PATH_STRAIGHT or self.refLine == None or \
abs(normalizeAnglePIPI( self.refLine.angle - sPose.heading )) < REF_LINE_CROSSING_ANGLE:
self.pathType = pt
self.pathPose = sPose
self.pathUpdated = True
break
else:
print "SKIPPED1"
if not self.pathUpdated:
if len(frameStrips) == 1 and self.lastStripPose != None:
fs = frameStrips[0]
sPose = pose.add( fs )
for lsp in self.lastStripPose:
if self.isSameStrip( sPose, lsp ):
# the self.pathType is the same only the pose is updated
self.pathPose = sPose
self.pathUpdated = True
if not self.pathUpdated:
for lsp in self.lastStripPose:
print sPose.sub(lsp)
if len(frameStrips) > 0:
self.lastStripPose = []
for fs in frameStrips:
sPose = pose.add( fs )
if verbose and self.lastStripPose != None:
print [str(sPose.sub( lsp )) for lsp in self.lastStripPose]
self.lastStripPose.append( sPose )
if self.pathType == PATH_TURN_LEFT:
self.countLR = max( -NUM_TRANSITION_STEPS, self.countLR - 1 )
if self.pathType == PATH_TURN_RIGHT:
self.countLR = min( NUM_TRANSITION_STEPS, self.countLR + 1 )
if self.pathPose:
sPose = self.pathPose
if self.pathType == PATH_TURN_LEFT:
circCenter = sPose.add( Pose(0.0, REF_CIRCLE_RADIUS, 0 )).coord()
self.refCircle = circCenter, REF_CIRCLE_RADIUS - MAX_CIRCLE_OFFSET*self.countLR/float(NUM_TRANSITION_STEPS)
elif self.pathType == PATH_TURN_RIGHT:
circCenter = sPose.add( Pose(0.0, -REF_CIRCLE_RADIUS, 0 )).coord()
self.refCircle = circCenter, REF_CIRCLE_RADIUS + MAX_CIRCLE_OFFSET*self.countLR/float(NUM_TRANSITION_STEPS)
else:
self.refCircle = None
if self.pathType == PATH_STRAIGHT:
if self.refLine == None or abs(normalizeAnglePIPI( self.refLine.angle - sPose.heading )) < REF_LINE_CROSSING_ANGLE:
offset = Pose()
if self.countLR > 0:
offset = Pose( 0, LINE_OFFSET, 0 )
if self.countLR < 0:
offset = Pose( 0, -LINE_OFFSET, 0 )
sPose = sPose.add( offset )
self.refLine = Line( (sPose.x-0.15*math.cos(sPose.heading), sPose.y-0.15*math.sin(sPose.heading)),
(sPose.x+0.15*math.cos(sPose.heading), sPose.y+0.15*math.sin(sPose.heading)) )
else:
self.refLine = None
return self.pathUpdated
def isSameStrip( self, pose1, pose2 ):
dx,dy,da = pose1.sub(pose2)
da = normalizeAnglePIPI(da)
if abs(dx) < 0.2 and abs(dy) < 0.1 and abs(da) < math.radians(10):
return True
return False
def competeRobotemRovne( drone, desiredHeight = 1.5 ):
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
refLine = DEFAULT_REF_LINE
try:
drone.wait(1.0)
drone.setVideoChannel( front=True )
downloadOldVideo( drone, timeout=20.0 )
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
# TODO sonar/vision altitude
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), None) )
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
startTime = drone.time
sx,sy,sz,sa = 0,0,0,0
lastUpdate = None
while drone.time < startTime + 600.0:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0]/1000.0
altSonar = drone.altitudeData[3]/1000.0
altitude = (altSonar+altVision)/2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar-altVision) > 0.5:
print altSonar, altVision
altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)
sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
if altitude > 2.5:
# wind and "out of control"
sz = max( -0.5, min( 0.5, desiredHeight - altitude ))
sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
if drone.lastImageResult:
lastUpdate = drone.time
assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpVideoFrame( frameNumber, timestamp )
# keep history small
videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max( videoDelay, maxVideoDelay )
toDel = 0
for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
# print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
if rects != None and len(rects) > 0:
# note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
if oldAltitude == None:
oldAltitude = altitude
debugImg = None
if drone.replayLog != None:
debugFilename = "tmp_%04d.jpg" % (frameNumber/FRAMES_PER_INDEX)
debugImg = cv2.imread( debugFilename )
line = chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude,
heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg )
if line:
rejected = False
if refLine != None:
print "%.1fdeg %.2fm" % (math.degrees(normalizeAnglePIPI(refLine.angle - line.angle)), refLine.signedDistance( line.start ))
if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
refLine = line
else:
rejected = True
else:
refLine = line
viewlog.dumpBeacon( line.start, index=3 )
viewlog.dumpBeacon( line.end, index=3 )
if drone.replayLog != None:
if rejected:
# redraw image with yellow color
chooseBestWidth( rects, coord=oldPose[:2], height=oldAltitude,
heading=oldPose[2], angleFB=oldAngles[0], angleLR=oldAngles[1], debugImg=debugImg, color=(0,255,255) )
cv2.imwrite( debugFilename, debugImg )
else:
print rects
desiredSpeed = MAX_ALLOWED_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance( drone.coord )
errA = normalizeAnglePIPI( drone.heading - refLine.angle )
# get the height first
if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
sx = 0.0
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA( sx, sy, sz, sa )
maxControlGap = max( drone.time - prevTime, maxControlGap )
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR), altitude) )
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel( front=True )
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def hoverAboveRoundel(drone, timeout=6000.0):
startTime = drone.time
maxSpeed = 0.1
maxSpeedUpDown = 0.3
frac = 0.1
scaleX, scaleY = 0.38, 0.38 # camera FOW at 1m above the ground
detectedCount = 0
goal = (0.0, 0.0) #None
goalHeading = None
while drone.time - startTime < timeout:
sx, sy, sz, sa = 0.0, 0.0, 0.0, 0.0
if drone.visionTag:
# xc[i], yc[i]: X and Y coordinates of detected tag or oriented roundel #i inside the picture,
# with (0; 0) being the top-left corner, and (1000; 1000) the right-bottom corner regardless
# the picture resolution or the source camera
x, y = drone.visionTag[0][:2]
dist = drone.visionTag[0][4] / 100.
angle = math.radians(
drone.visionTag[0][5] - 270
) # we use circle for the body and line for the tail -> 270deg offset
detectedCount = min(100, detectedCount + 1)
# TODO verify signs and scale
dx = dist * (scaleY * (500 - y) / 500.0 + drone.angleFB)
dy = dist * (scaleX * (500 - x) / 500.0) # - drone.angleLR)
c = math.cos(drone.heading)
s = math.sin(drone.heading)
goal = (drone.coord[0] + c * dx - s * dy,
drone.coord[1] + s * dx + c * dy)
goalHeading = normalizeAnglePIPI(drone.heading +
angle) # TODO check sign
# print drone.visionTag, "%.2f %.2f %.1f %.1f" % (dx, dy, math.degrees(drone.angleFB), math.degrees(drone.angleLR))
else:
detectedCount = max(0, detectedCount - 1)
if goal:
sx, sy, sz, sa = getXYZAGoalCmd(drone,
goal,
goalHeading=goalHeading)
sz = 0.0
if drone.altitudeData != None:
altVision = drone.altitudeData[0] / 1000.0
altSonar = drone.altitudeData[3] / 1000.0
if drone.altitudeData[3] == 0:
# no sonar echo
minAlt = altVision
else:
minAlt = min(altSonar, altVision)
if minAlt > MAX_ALTITUDE:
# too high to reliably detect anything
sz = -maxSpeedUpDown
# print altSonar, altVision,
if detectedCount == 0:
# try to move up
if max(altSonar, altVision) < 2.0:
sz = maxSpeedUpDown
elif detectedCount == 100:
if minAlt > 1.0:
sz = -maxSpeedUpDown
# print "\t%.2f %.2f %.2f %.2f" % (sx, sy, sz, sa)
drone.moveXYZA(sx, sy, sz, sa)
drone.hover(0.1) # stop motion
def followRow(drone, desiredHeight=1.5, timeout=10.0):
maxControlGap = 0.0
maxVideoDelay = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
startTime = drone.time
sx, sy, sz, sa = 0, 0, 0, 0
lastUpdate = None
refLine = None
while drone.time < startTime + timeout:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0] / 1000.0
altSonar = drone.altitudeData[3] / 1000.0
altitude = (altSonar + altVision) / 2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar - altVision) > 0.5:
# print altSonar, altVision
altitude = max(altSonar,
altVision) # sonar is 0.0 sometimes (no ECHO)
sz = max(-0.2, min(0.2, desiredHeight - altitude))
if altitude > 2.5:
# wind and "out of control"
sz = max(-0.5, min(0.5, desiredHeight - altitude))
if drone.lastImageResult:
lastUpdate = drone.time
assert len(drone.lastImageResult) == 2 and len(
drone.lastImageResult[0]) == 2, drone.lastImageResult
(frameNumber, timestamp), rowTopBottom = drone.lastImageResult
viewlog.dumpVideoFrame(frameNumber, timestamp)
# keep history small
videoTime = correctTimePeriod(timestamp / 1000., ref=drone.time)
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max(videoDelay, maxVideoDelay)
toDel = 0
for oldTime, oldPose, oldAngles in drone.poseHistory:
toDel += 1
if oldTime >= videoTime:
break
drone.poseHistory = drone.poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight * oldAngles[0],
desiredHeight * oldAngles[1],
0) # TODO real height?
validRow = evalRowData(rowTopBottom)
print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
oldAngles[0]), math.degrees(oldAngles[1])), validRow
if validRow:
sp = groundPose(*rowTopBottom,
scale=ROW_WIDTH /
((validRow[0] + validRow[1]) / 2.0))
sPose = Pose(*oldPose).add(tiltCompensation).add(sp)
refLine = Line((sPose.x, sPose.y),
(sPose.x + math.cos(sPose.heading),
sPose.y + math.sin(sPose.heading)))
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance(drone.coord)
errA = normalizeAnglePIPI(drone.heading - refLine.angle)
sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (desiredSpeed / 0.4)
prevTime = drone.time
drone.moveXYZA(sx, sy, sz, sa)
drone.poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR)))
maxControlGap = max(drone.time - prevTime, maxControlGap)
return maxControlGap
def competeRobotemRovne(drone, desiredHeight=1.5):
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
refLine = DEFAULT_REF_LINE
try:
drone.wait(1.0)
drone.setVideoChannel(front=True)
downloadOldVideo(drone, timeout=20.0)
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
# TODO sonar/vision altitude
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR), None))
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
startTime = drone.time
sx, sy, sz, sa = 0, 0, 0, 0
lastUpdate = None
while drone.time < startTime + 600.0:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0] / 1000.0
altSonar = drone.altitudeData[3] / 1000.0
altitude = (altSonar + altVision) / 2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar - altVision) > 0.5:
print altSonar, altVision
altitude = max(
altSonar,
altVision) # sonar is 0.0 sometimes (no ECHO)
sz = max(-0.2, min(0.2, desiredHeight - altitude))
if altitude > 2.5:
# wind and "out of control"
sz = max(-0.5, min(0.5, desiredHeight - altitude))
sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
if drone.lastImageResult:
lastUpdate = drone.time
assert len(drone.lastImageResult) == 2 and len(
drone.lastImageResult[0]) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpVideoFrame(frameNumber, timestamp)
# keep history small
videoTime = correctTimePeriod(timestamp / 1000.,
ref=drone.time)
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max(videoDelay, maxVideoDelay)
toDel = 0
for oldTime, oldPose, oldAngles, oldAltitude in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight * oldAngles[0],
desiredHeight * oldAngles[1],
0) # TODO real height?
print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
oldAngles[0]), math.degrees(oldAngles[1])),
# print "angle", math.degrees(drone.angleFB-oldAngles[0]), math.degrees(drone.angleLR-oldAngles[1])
if rects != None and len(rects) > 0:
# note, that "rects" could be None, when video processing is OFF (g_processingEnabled==False)
if oldAltitude == None:
oldAltitude = altitude
debugImg = None
if drone.replayLog != None:
debugFilename = "tmp_%04d.jpg" % (frameNumber /
FRAMES_PER_INDEX)
debugImg = cv2.imread(debugFilename)
line = chooseBestWidth(rects,
coord=oldPose[:2],
height=oldAltitude,
heading=oldPose[2],
angleFB=oldAngles[0],
angleLR=oldAngles[1],
debugImg=debugImg)
if line:
rejected = False
if refLine != None:
print "%.1fdeg %.2fm" % (math.degrees(
normalizeAnglePIPI(refLine.angle - line.angle)
), refLine.signedDistance(line.start))
if abs(normalizeAnglePIPI(refLine.angle - line.angle)) < MAX_REF_LINE_ANGLE and \
abs(refLine.signedDistance( line.start )) < MAX_REF_LINE_DIST:
refLine = line
else:
rejected = True
else:
refLine = line
viewlog.dumpBeacon(line.start, index=3)
viewlog.dumpBeacon(line.end, index=3)
if drone.replayLog != None:
if rejected:
# redraw image with yellow color
chooseBestWidth(rects,
coord=oldPose[:2],
height=oldAltitude,
heading=oldPose[2],
angleFB=oldAngles[0],
angleLR=oldAngles[1],
debugImg=debugImg,
color=(0, 255, 255))
cv2.imwrite(debugFilename, debugImg)
else:
print rects
desiredSpeed = MAX_ALLOWED_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance(drone.coord)
errA = normalizeAnglePIPI(drone.heading - refLine.angle)
# get the height first
if drone.coord[
2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
sx = 0.0
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
desiredSpeed / 0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA(sx, sy, sz, sa)
maxControlGap = max(drone.time - prevTime, maxControlGap)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR), altitude))
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel(front=True)
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def followRow( drone, desiredHeight = 1.5, timeout = 10.0 ):
maxControlGap = 0.0
maxVideoDelay = 0.0
desiredSpeed = MAX_ALLOWED_SPEED
startTime = drone.time
sx,sy,sz,sa = 0,0,0,0
lastUpdate = None
refLine = None
while drone.time < startTime + timeout:
altitude = desiredHeight
if drone.altitudeData != None:
altVision = drone.altitudeData[0]/1000.0
altSonar = drone.altitudeData[3]/1000.0
altitude = (altSonar+altVision)/2.0
# TODO selection based on history? panic when min/max too far??
if abs(altSonar-altVision) > 0.5:
# print altSonar, altVision
altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)
sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
if altitude > 2.5:
# wind and "out of control"
sz = max( -0.5, min( 0.5, desiredHeight - altitude ))
if drone.lastImageResult:
lastUpdate = drone.time
assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
(frameNumber, timestamp), rowTopBottom = drone.lastImageResult
viewlog.dumpVideoFrame( frameNumber, timestamp )
# keep history small
videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max( videoDelay, maxVideoDelay )
toDel = 0
for oldTime, oldPose, oldAngles in drone.poseHistory:
toDel += 1
if oldTime >= videoTime:
break
drone.poseHistory = drone.poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
validRow = evalRowData( rowTopBottom )
print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])), validRow
if validRow:
sp = groundPose( *rowTopBottom, scale=ROW_WIDTH/((validRow[0]+validRow[1])/2.0))
sPose = Pose( *oldPose ).add(tiltCompensation).add( sp )
refLine = Line( (sPose.x,sPose.y), (sPose.x + math.cos(sPose.heading), sPose.y + math.sin(sPose.heading)) )
errY, errA = 0.0, 0.0
if refLine:
errY = refLine.signedDistance( drone.coord )
errA = normalizeAnglePIPI( drone.heading - refLine.angle )
sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4)
prevTime = drone.time
drone.moveXYZA( sx, sy, sz, sa )
drone.poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
maxControlGap = max( drone.time - prevTime, maxControlGap )
return maxControlGap
def competeAirRace(drone, desiredHeight=1.7):
loops = []
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
loc = StripsLocalisation()
remainingFastStrips = 0
desiredSpeed = TRANSITION_SPEED
updateFailedCount = 0
try:
drone.wait(1.0)
drone.setVideoChannel(front=False)
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR)))
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
pathType = PATH_TURN_LEFT
virtualRefCircle = None
startTime = drone.time
sx, sy, sz, sa = 0, 0, 0, 0
lastUpdate = None
while drone.time < startTime + 600.0:
sz = max(-0.2, min(0.2, desiredHeight - drone.coord[2]))
sx = max(0, min(drone.speed, desiredSpeed - drone.vx))
if drone.lastImageResult:
lastUpdate = drone.time
assert len(drone.lastImageResult) == 2 and len(
drone.lastImageResult[0]) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpCamera("tmp_%04d.jpg" % (frameNumber / 15, ), 0)
# keep history small
videoTime = correctTimePeriod(timestamp / 1000.,
ref=drone.time)
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max(videoDelay, maxVideoDelay)
toDel = 0
for oldTime, oldPose, oldAngles in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight * oldAngles[0],
desiredHeight * oldAngles[1],
0) # TODO real height?
print "FRAME", frameNumber / 15, "[%.1f %.1f]" % (math.degrees(
oldAngles[0]), math.degrees(oldAngles[1])),
loc.updateFrame(
Pose(*oldPose).add(tiltCompensation),
allStripPoses(rects,
highResolution=drone.videoHighResolution))
if loc.pathType != pathType:
print "TRANS", pathType, "->", loc.pathType
if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
if len(loops) > 0:
print "Loop %d, time %d" % (len(loops),
drone.time - loops[-1])
print "-----------------------------------------------"
loops.append(drone.time)
if drone.magneto[:3] == magnetoOnStart:
print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
pathType = loc.pathType
print "----"
remainingFastStrips = NUM_FAST_STRIPS
if loc.crossing:
print "X", True, remainingFastStrips
else:
print pathType, loc.pathUpdated, remainingFastStrips
if not loc.pathUpdated:
updateFailedCount += 1
if updateFailedCount > 1:
print "UPDATE FAILED", updateFailedCount
else:
updateFailedCount = 0
if remainingFastStrips > 0:
remainingFastStrips -= 1
desiredSpeed = MAX_ALLOWED_SPEED
if not loc.pathUpdated and not loc.crossing:
desiredSpeed = STRIPS_FAILURE_SPEED
else:
desiredSpeed = TRANSITION_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
for sp in allStripPoses(
rects, highResolution=drone.videoHighResolution):
sPose = Pose(*oldPose).add(tiltCompensation).add(sp)
viewlog.dumpBeacon(sPose.coord(), index=3)
viewlog.dumpObstacles([[
(sPose.x - 0.15 * math.cos(sPose.heading),
sPose.y - 0.15 * math.sin(sPose.heading)),
(sPose.x + 0.15 * math.cos(sPose.heading),
sPose.y + 0.15 * math.sin(sPose.heading))
]])
refCircle, refLine = loc.getRefCircleLine(
Pose(drone.coord[0], drone.coord[1], drone.heading))
if refCircle == None and refLine == None and virtualRefCircle != None:
refCircle = virtualRefCircle
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
assert refCircle == None or refLine == None # they cannot be both active at the same time
if refCircle:
if pathType == PATH_TURN_LEFT:
errY = refCircle[1] - math.hypot(
drone.coord[0] - refCircle[0][0],
drone.coord[1] - refCircle[0][1])
errA = normalizeAnglePIPI(
-math.atan2(refCircle[0][1] - drone.coord[1],
refCircle[0][0] - drone.coord[0]) -
math.radians(-90) + drone.heading)
if pathType == PATH_TURN_RIGHT:
errY = math.hypot(
drone.coord[0] - refCircle[0][0],
drone.coord[1] - refCircle[0][1]) - refCircle[1]
errA = normalizeAnglePIPI(
-math.atan2(refCircle[0][1] - drone.coord[1],
refCircle[0][0] - drone.coord[0]) -
math.radians(90) + drone.heading)
if refLine:
errY = refLine.signedDistance(drone.coord)
errA = normalizeAnglePIPI(drone.heading - refLine.angle)
# get the height first
if drone.coord[
2] < desiredHeight - 0.1 and drone.time - startTime < 5.0:
sx = 0.0
if refCircle == None and refLine == None and virtualRefCircle == None:
sx = 0.0 # wait for Z-up
if drone.coord[2] > desiredHeight - 0.1:
print "USING VIRTUAL LEFT TURN CIRCLE!"
circCenter = Pose(drone.coord[0], drone.coord[1],
drone.heading).add(
Pose(0.0, REF_CIRCLE_RADIUS,
0)).coord()
viewlog.dumpBeacon(circCenter, index=0)
virtualRefCircle = circCenter, REF_CIRCLE_RADIUS
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max(-0.2, min(0.2, -errY - drone.vy)) / 2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max(-0.1, min(0.1, -errA / 2.0)) * 1.35 * (
desiredSpeed / 0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA(sx, sy, sz, sa)
maxControlGap = max(drone.time - prevTime, maxControlGap)
poseHistory.append(
(drone.time, (drone.coord[0], drone.coord[1], drone.heading),
(drone.angleFB, drone.angleLR)))
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel(front=True)
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def competeAirRace( drone, desiredHeight = 1.7 ):
loops = []
drone.speed = 0.1
maxVideoDelay = 0.0
maxControlGap = 0.0
loc = StripsLocalisation()
remainingFastStrips = 0
desiredSpeed = TRANSITION_SPEED
updateFailedCount = 0
try:
drone.wait(1.0)
drone.setVideoChannel( front=False )
drone.takeoff()
poseHistory = []
startTime = drone.time
while drone.time < startTime + 1.0:
drone.update("AT*PCMD=%i,0,0,0,0,0\r") # drone.hover(1.0)
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
magnetoOnStart = drone.magneto[:3]
print "NAVI-ON"
pathType = PATH_TURN_LEFT
virtualRefCircle = None
startTime = drone.time
sx,sy,sz,sa = 0,0,0,0
lastUpdate = None
while drone.time < startTime + 600.0:
sz = max( -0.2, min( 0.2, desiredHeight - drone.coord[2] ))
sx = max( 0, min( drone.speed, desiredSpeed - drone.vx ))
if drone.lastImageResult:
lastUpdate = drone.time
assert len( drone.lastImageResult ) == 2 and len( drone.lastImageResult[0] ) == 2, drone.lastImageResult
(frameNumber, timestamp), rects = drone.lastImageResult
viewlog.dumpCamera( "tmp_%04d.jpg" % (frameNumber/15,), 0 )
# keep history small
videoTime = correctTimePeriod( timestamp/1000., ref=drone.time )
videoDelay = drone.time - videoTime
if videoDelay > 1.0:
print "!DANGER! - video delay", videoDelay
maxVideoDelay = max( videoDelay, maxVideoDelay )
toDel = 0
for oldTime, oldPose, oldAngles in poseHistory:
toDel += 1
if oldTime >= videoTime:
break
poseHistory = poseHistory[:toDel]
tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),
loc.updateFrame( Pose( *oldPose ).add(tiltCompensation), allStripPoses( rects, highResolution=drone.videoHighResolution ) )
if loc.pathType != pathType:
print "TRANS", pathType, "->", loc.pathType
if pathType == PATH_TURN_LEFT and loc.pathType == PATH_STRAIGHT:
if len(loops) > 0:
print "Loop %d, time %d" % (len(loops), drone.time-loops[-1])
print "-----------------------------------------------"
loops.append( drone.time )
if drone.magneto[:3] == magnetoOnStart:
print "!!!!!!!! COMPASS FAILURE !!!!!!!!"
pathType = loc.pathType
print "----"
remainingFastStrips = NUM_FAST_STRIPS
if loc.crossing:
print "X", True, remainingFastStrips
else:
print pathType, loc.pathUpdated, remainingFastStrips
if not loc.pathUpdated:
updateFailedCount += 1
if updateFailedCount > 1:
print "UPDATE FAILED", updateFailedCount
else:
updateFailedCount = 0
if remainingFastStrips > 0:
remainingFastStrips -= 1
desiredSpeed = MAX_ALLOWED_SPEED
if not loc.pathUpdated and not loc.crossing:
desiredSpeed = STRIPS_FAILURE_SPEED
else:
desiredSpeed = TRANSITION_SPEED
if videoDelay > MAX_ALLOWED_VIDEO_DELAY:
desiredSpeed = 0.0
if drone.battery < 10:
print "BATTERY LOW!", drone.battery
# height debugging
#print "HEIGHT\t%d\t%d\t%.2f\t%d\t%d\t%d\t%d\t%d\t%d" % tuple([max([0]+[w for ((x,y),(w,h),a) in rects])] + list(drone.altitudeData[:4]) + list(drone.pressure) )
for sp in allStripPoses( rects, highResolution=drone.videoHighResolution ):
sPose = Pose( *oldPose ).add(tiltCompensation).add( sp )
viewlog.dumpBeacon( sPose.coord(), index=3 )
viewlog.dumpObstacles( [[(sPose.x-0.15*math.cos(sPose.heading), sPose.y-0.15*math.sin(sPose.heading)),
(sPose.x+0.15*math.cos(sPose.heading), sPose.y+0.15*math.sin(sPose.heading))]] )
refCircle,refLine = loc.getRefCircleLine( Pose(drone.coord[0], drone.coord[1], drone.heading) )
if refCircle == None and refLine == None and virtualRefCircle != None:
refCircle = virtualRefCircle
# error definition ... if you substract that you get desired position or angle
# error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
errY, errA = 0.0, 0.0
assert refCircle == None or refLine == None # they cannot be both active at the same time
if refCircle:
if pathType == PATH_TURN_LEFT:
errY = refCircle[1] - math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] )
errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] )
- math.radians(-90) + drone.heading )
if pathType == PATH_TURN_RIGHT:
errY = math.hypot( drone.coord[0]-refCircle[0][0], drone.coord[1]-refCircle[0][1] ) - refCircle[1]
errA = normalizeAnglePIPI( - math.atan2( refCircle[0][1] - drone.coord[1], refCircle[0][0] - drone.coord[0] )
- math.radians(90) + drone.heading )
if refLine:
errY = refLine.signedDistance( drone.coord )
errA = normalizeAnglePIPI( drone.heading - refLine.angle )
# get the height first
if drone.coord[2] < desiredHeight - 0.1 and drone.time-startTime < 5.0:
sx = 0.0
if refCircle == None and refLine == None and virtualRefCircle == None:
sx = 0.0 # wait for Z-up
if drone.coord[2] > desiredHeight - 0.1:
print "USING VIRTUAL LEFT TURN CIRCLE!"
circCenter = Pose( drone.coord[0], drone.coord[1], drone.heading ).add( Pose(0.0, REF_CIRCLE_RADIUS, 0 )).coord()
viewlog.dumpBeacon( circCenter, index=0 )
virtualRefCircle = circCenter, REF_CIRCLE_RADIUS
# error correction
# the goal is to have errY and errA zero in 1 second -> errY defines desired speed at given distance from path
sy = max( -0.2, min( 0.2, -errY-drone.vy ))/2.0
# there is no drone.va (i.e. derivative of heading) available at the moment ...
sa = max( -0.1, min( 0.1, -errA/2.0 ))*1.35*(desiredSpeed/0.4) # originally set for 0.4=OK
# print "%0.2f\t%d\t%0.2f\t%0.2f\t%0.2f" % (errY, int(math.degrees(errA)), drone.vy, sy, sa)
prevTime = drone.time
drone.moveXYZA( sx, sy, sz, sa )
maxControlGap = max( drone.time - prevTime, maxControlGap )
poseHistory.append( (drone.time, (drone.coord[0], drone.coord[1], drone.heading), (drone.angleFB, drone.angleLR)) )
print "NAVI-OFF", drone.time - startTime
drone.hover(0.5)
drone.land()
drone.setVideoChannel( front=True )
except ManualControlException, e:
print "ManualControlException"
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
