def __init__(self,
replayLog=None,
speed=0.2,
skipConfigure=False,
metaLog=None,
console=None):
self.loggedVideoResult = None
self.lastImageResult = None
self.videoHighResolution = False
ARDrone2.__init__(self, replayLog, speed, skipConfigure, metaLog,
console)
if replayLog == None:
name = timeName("logs/src_cv2_", "log")
metaLog.write("cv2: " + name + '\n')
self.loggedVideoResult = SourceLogger(getOrNone, name).get
self.startVideo(wrapper,
g_queueResults,
record=True,
highResolution=self.videoHighResolution)
else:
assert metaLog
self.loggedVideoResult = SourceLogger(None,
metaLog.getLog("cv2:")).get
self.startVideo(record=True,
highResolution=self.videoHighResolution)
def __init__( self, replayLog=None, speed = 0.2, skipConfigure=False, metaLog=None, console=None ):
self.loggedVideoResult = None
self.lastImageResult = None
self.videoHighResolution = False
ARDrone2.__init__( self, replayLog, speed, skipConfigure, metaLog, console )
if replayLog == None:
name = timeName( "logs/src_cv2_", "log" )
metaLog.write("cv2: "+name+'\n' )
self.loggedVideoResult = SourceLogger( getOrNone, name ).get
self.startVideo( wrapper, g_queueResults, record=True, highResolution=self.videoHighResolution )
else:
assert metaLog
self.loggedVideoResult = SourceLogger( None, metaLog.getLog("cv2:") ).get
self.startVideo( record=True, highResolution=self.videoHighResolution )
def h264drone(replayLog, metaLog, desiredSpeed=1.0, timeout=5.0):
drone = ARDrone2(replayLog, metaLog=metaLog)
if replayLog:
for line in metaLog: # TODO refactoring
print "XXLINE", line.strip()
if line.startswith("h264:"):
loggedResult = SourceLogger(None, line.split()[1].strip()).get
break
drone.startVideo(record=False)
else:
name = timeName("logs/src_h264_", "log")
metaLog.write("h264: " + name + '\n')
loggedResult = SourceLogger(getOrNone, name).get
drone.startVideo(wrapper, queueResults, record=False)
if drone.userEmergencyLanding:
drone.reset()
try:
drone.wait(1.0)
drone.takeoff(enabledCorrections=False)
# TODO some flying
startTime = drone.time
vz = 0.0
while drone.time - startTime < timeout:
# print "SPEED", drone.vx
if drone.vx > desiredSpeed:
drone.moveXYZA(0.0, 0.0, vz, 0.0)
else:
drone.moveXYZA(drone.speed, 0.0, vz, 0.0)
tmp = loggedResult()
if tmp != None:
print "QUEUE", drone.time, tmp
frameNum, timestamp, (left, right, up, down) = tmp
vz = 0.0
if left + right < MAX_COUNT: # limited max number of movements in the whole image
if down > up + MIN_STEP:
# move up
vz = drone.speed
if up > down + MIN_STEP:
# move down
vz = -drone.speed
drone.land()
drone.wait(1.0)
except ManualControlException, e:
print "ManualControlException"
manualControl(drone)
if drone.ctrlState == 3: # CTRL_FLYING=3 ... i.e. stop the current motion
drone.hover(0.1)
drone.land()
def update( self, cmd="AT*COMWDG=%i,\r" ):
ARDrone2.update( self, cmd )
if self.loggedVideoResult != None:
self.lastImageResult = self.loggedVideoResult()
if "meta" in sys.argv[2]:
metaLog = open(sys.argv[2])
for line in metaLog:
if line.startswith("navdata:"):
replayLog = line.split()[1].strip()
break
else:
replayLog = sys.argv[2]
viewlog.viewLogFile = open("view.log", "w")
viewlog.dumpSharpsGeometry([(0.18, 0.0, 0.0)]) # front sonar
else: # create new metaLog
metaLog = open(
datetime.datetime.now().strftime("logs/meta_%y%m%d_%H%M%S.log"),
"w")
metaLog.write(str(sys.argv) + "\n")
metaLog.flush()
if sys.argv[1] == "replay":
for replayLog in sys.argv[2:]:
drone = ARDrone2(replayLog, skipConfigure=True)
try:
while True:
drone.update()
if drone.altitudeData:
print "%d\t%.3f\t" % (
drone.ctrlState, drone.coord[2]) + "\t".join(
[str(x) for x in drone.altitudeData])
except EOFError:
pass
else:
h264drone(replayLog=replayLog, metaLog=metaLog)
"""
Lesson 2 - manual emergency stop
"""
import sys
sys.path.append('..') # access to drone source without installation
from ardrone2 import ARDrone2, ManualControlException
drone = ARDrone2()
try:
drone.takeoff()
drone.hover(3.0)
except ManualControlException, e:
print "ManualControlException"
drone.land()
# vim: expandtab sw=4 ts=4
def update(self, cmd="AT*COMWDG=%i,\r"):
ARDrone2.update(self, cmd)
if self.loggedVideoResult != None:
self.lastImageResult = self.loggedVideoResult()
