def launch(cmd_args, robotFactory, task, configFn = None, canArgs={}):
'''
Launches a robot to the given file.
Parameters:
cmd_args ... List of parameters in the "configFile [--file logfile [F|FF]]" form.
robotFactory ... Function (or class) which can be called with the following named
parameters and creates an instance of the robot:
robot = robotFactory(can=..., replyLog=...)
task ... Factory which called as "t = task(robot, configFileName, verbose=...)"
creates a runable instance. Ie. "t()" runs the show.
configFn ... A method called in the robot's preconfigration mode.
canArgs ... Named arguments passed down to the CAN.
'''
if len(cmd_args) < 2:
print __doc__
# sys.exit(2)
return
# TODO unified launcher, similar to Eduro
if len(cmd_args) > 3 and cmd_args[3] in ['F','FL']:
ardrone2.g_checkAssert = False
if len(cmd_args) > 3 and cmd_args[3] == 'FL':
ardrone2.g_relogCmd = True
replayLog = None
metaLog = None
console = None
if len(cmd_args) > 2:
if "meta" in cmd_args[2]:
metaLog = MetaLog( cmd_args[2] )
replayLog = metaLog.getLog( "navdata:" )
else:
replayLog=cmd_args[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(cmd_args) + "\n" )
metaLog.flush()
if sys.platform == 'linux2':
pygame.init()
screen = pygame.display.set_mode((100,100))
console = linuxKbHit
if cmd_args[1] == "replay":
for replayLog in cmd_args[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:
task( robotFactory( replayLog=replayLog, metaLog=metaLog, console=console ) )
def dumpLaserGeometry(self, pose=None ):
if pose == None:
geo = [(0,0,math.radians(angle)) for angle in range(-135, 136, self.degStep)]
else:
(x,y,z),(a,b,c) = pose
geo = [(x,y,a+math.radians(angle)) for angle in range(-135, 136, self.degStep)]
assert( b == 0 or b == math.radians(180) )
if b == math.radians(180):
geo.reverse()
viewlog.dumpSharpsGeometry( geo )
def dumpLaserGeometry(self, pose=None):
if pose == None:
geo = [(0, 0, math.radians(angle))
for angle in range(-135, 136, self.degStep)]
else:
(x, y, z), (a, b, c) = pose
geo = [(x, y, a + math.radians(angle))
for angle in range(-135, 136, self.degStep)]
assert (b == 0 or b == math.radians(180))
if b == math.radians(180):
geo.reverse()
viewlog.dumpSharpsGeometry(geo)
# TODO unified launcher, similar to Eduro
if len(sys.argv) > 3 and sys.argv[3] == 'F':
g_checkAssert = False
replayLog = None
metaLog = None
if len(sys.argv) > 2:
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(
def dumpLaserGeometry():
# TODO len(data) a step
geo = [(0, 0, math.radians(angle)) for angle in range(-135, 136, 5)]
print "geo", len(geo)
viewlog.dumpSharpsGeometry(geo)
def dumpLaserGeometry(): # TODO len(data) a step geo = [(0,0,math.radians(angle)) for angle in range(-135, 136, 5)] print "geo", len(geo) viewlog.dumpSharpsGeometry( geo )
# TODO unified launcher, similar to Eduro
if len(sys.argv) > 3 and sys.argv[3] == 'F':
g_checkAssert = False
replayLog = None
metaLog = None
if len(sys.argv) > 2:
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:
