def go( metalog, assertWrite, ipPair ):
print "Go straight ..."
if metalog is None:
metalog = MetaLog()
robot = HuskyROS( filename=metalog.getLog("node"), replay=metalog.replay, ipPair=ipPair )
print "started"
scanner = ScannerThread()
scanner.start()
scannerFn = SourceLogger( sourceGet=scanner.get, filename=metalog.getLog("scanner") ).get
else:
scanner = DummyScanner() # only for access to scanner saveImages
robot = HuskyROS( filename=metalog.getLog("node"), replay=True, assertWrite=assertWrite, ipPair=ipPair ) # TODO move assert to metalog
scannerFn = SourceLogger( sourceGet=None, filename=metalog.getLog("scanner") ).get
maxSpeed = 0.25
safeDist = 0.5
limitDist = 1.5
index = 0
prev = None
obstacleDir = 0.0
personDetected = False
pauseCount = 0 # TODO use time instead
while True:
minDist = scannerFn()
scanner.saveImages = not robot.emergencyStopPressed
if minDist is not None:
prev = minDist[0]/1000.0
turnDiff = 160-minDist[1] # only flip
obstacleDir = -math.radians(turnDiff)/4.0 # TODO calibration via FOV
print prev, turnDiff, robot.power, robot.emergencyStopPressed
if robot.emergencyStopPressed:
pauseCount = 10
else:
pauseCount = max(0, pauseCount-1)
if prev is None or prev < safeDist or pauseCount > 0:
robot.setSpeedPxPa( 0, 0 )
elif prev < safeDist*2:
# turn in place
if obstacleDir > 0: # i.e. on the left
robot.setSpeedPxPa( 0.0, math.radians(-10) )
else:
robot.setSpeedPxPa( 0.0, math.radians(10) )
# robot.setSpeedPxPa( maxSpeed*(prev-safeDist)/safeDist, turn/2 )
# elif prev < limitDist:
# robot.setSpeedPxPa( maxSpeed, turn )
else:
robot.setSpeedPxPa( maxSpeed, 0 )
robot.update()
robot.setSpeedPxPa( 0, 0 )
robot.update()
log.write("%d\n" % index )
log.close()
if not metalog.replay:
scanner.requestStop()
scanner.join()
def battery( metalog, assertWrite, ipPair ):
if metalog is None:
metalog = MetaLog()
robot = HuskyROS( filename=metalog.getLog("node"), replay=metalog.replay, ipPair=ipPair )
else:
robot = HuskyROS( filename=metalog.getLog("node"), replay=True, assertWrite=assertWrite, ipPair=ipPair ) # TODO move assert to metalog
scannerFn = SourceLogger( sourceGet=None, filename=metalog.getLog("scanner") ).get
robot.setSpeedPxPa( 0, 0 )
for i in xrange(10):
robot.update()
print "Battery: %.3f" % robot.power
def __init__(self, metalog=None):
if metalog is None:
metalog = MetaLog()
self.soc = metalog.createLoggedSocket("velodyne", headerFormat="<BBBI")
self.soc.bind( ('',PORT) )
self.metalog = metalog
self.buf = ""
self.time = None
self.last_blocked = None
self.dist = np.zeros( (360, NUM_LASERS), dtype=np.uint16)
self.scan_index = 0
self.prev_azimuth = None
self.safe_dist = None
self.dist_index = None
def __init__( self, metalog=None ):
if metalog is None:
self._discovery()
metalog = MetaLog()
self.navdata = metalog.createLoggedSocket( "navdata", headerFormat="<BBBI" )
self.navdata.bind( ('',NAVDATA_PORT) )
self.command = metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" )
self.console = metalog.createLoggedInput( "console", myKbhit ).get
self.metalog = metalog
self.buf = ""
self.battery = None
self.flyingState = None
self.flatTrimCompleted = False
self.manualControl = False
def battery(metalog, assertWrite, ipPair):
if metalog is None:
metalog = MetaLog()
robot = HuskyROS(filename=metalog.getLog("node"),
replay=metalog.replay,
ipPair=ipPair)
else:
robot = HuskyROS(filename=metalog.getLog("node"),
replay=True,
assertWrite=assertWrite,
ipPair=ipPair) # TODO move assert to metalog
scannerFn = SourceLogger(sourceGet=None,
filename=metalog.getLog("scanner")).get
robot.setSpeedPxPa(0, 0)
for i in xrange(10):
robot.update()
print "Battery: %.3f" % robot.power
def __init__( self, metalog=None, onlyIFrames=True ):
if metalog is None:
self._discovery()
metalog = MetaLog()
self.navdata = metalog.createLoggedSocket( "navdata", headerFormat="<BBBI" )
self.navdata.bind( ('',NAVDATA_PORT) )
if metalog.replay:
self.commandSender = CommandSenderReplay(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT), checkAsserts=metalog.areAssertsEnabled())
else:
self.commandSender = CommandSender(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT))
self.console = metalog.createLoggedInput( "console", myKbhit ).get
self.metalog = metalog
self.buf = ""
self.videoFrameProcessor = VideoFrames( onlyIFrames=onlyIFrames, verbose=False )
self.videoCbk = None
self.videoCbkResults = None
self.battery = None
self.flyingState = None
self.flatTrimCompleted = False
self.manualControl = False
self.time = None
self.altitude = None
self.position = (0,0,0)
self.speed = (0,0,0)
self.positionGPS = None
self.cameraTilt, self.cameraPan = 0,0
self.lastImageResult = None
self.navigateHomeState = None
self.config()
self.commandSender.start()
def __init__( self, host,navdata_port,speed_limits, metalog=None, onlyIFrames=True):
self.host = host
self.navdata_port = navdata_port
if metalog is None:
self._discovery()
metalog = MetaLog()
self.navdata = metalog.createLoggedSocket( "navdata", headerFormat="<BBBI" )
self.navdata.bind(('',self.navdata_port))
if metalog.replay:
self.commandSender = CommandSenderReplay(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(self.host, COMMAND_PORT), checkAsserts=metalog.areAssertsEnabled())
else:
self.commandSender = CommandSender(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(self.host, COMMAND_PORT))
#self.console = metalog.createLoggedInput( "console", myKbhit ).get #takahashi changed
self.metalog = metalog
self.buf = ""
self.videoFrameProcessor = VideoFrames( onlyIFrames=onlyIFrames, verbose=False )
self.videoCbk = None
self.videoCbkResults = None
self.battery = None
self.flyingState = None
self.flatTrimCompleted = False
self.manualControl = True# takahashi changed
self.time = None
self.altitude = None
self.position = (0,0,0)
self.speed = (0,0,0)
self.positionGPS = None
self.cameraTilt, self.cameraPan = 0,0
self.lastImageResult = None
self.navigateHomeState = None
self.config(speed_limits[0],speed_limits[1])#speed_limits[0] is linear velocity limit, speed_limits[1] is angular velocity limit
self.commandSender.start()
# takahashi added
self.roll = 0
self.pitch = 0
self.yaw = 0
def __init__( self, metalog=None, onlyIFrames=True, jpegStream=False, fps = 0 ):
if metalog is None:
self._discovery()
metalog = MetaLog()
self.navdata = metalog.createLoggedSocket( "navdata", headerFormat="<BBBI" )
self.navdata.bind( ('',NAVDATA_PORT) )
if metalog.replay:
self.commandSender = CommandSenderReplay(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT), checkAsserts=metalog.areAssertsEnabled())
else:
self.commandSender = CommandSender(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT))
self.console = metalog.createLoggedInput( "console", myKbhit ).get
self.metalog = metalog
self.buf = ""
self.jpegStream = jpegStream
self.fps = fps
self.videoCbk = None
self.videoCbkResults = None
self.battery = None
self.flyingState = None
self.flatTrimCompleted = False
self.manualControl = False
self.time = None
self.altitude = None
self.position = (0,0,0)
self.speed = (0,0,0)
self.positionGPS = None
self.cameraTilt, self.cameraPan = 0,0
self.lastImageResult = None
self.navigateHomeState = None
self.config()
self.commandSender.start()
# Video variables
self.frameWidth = 0
self.frameHeight = 0
self.pictureBoolean = False
self.barcodes = []
# Sphero tracking variables
self.findSphero = False
self.sinceLastSphero = 0
self.lastFrame = None
self.thisFrame = None
self.moveScaler = 0
self.objectCenterX = 0
self.objectCenterY = 0
self.minEdgeVal = 0
self.maxEdgeVal = 0
self.minCircleRadius = 0
self.maxCircleRadius = 0
if self.jpegStream:
self.videoFrameProcessor = VideoFrames(onlyIFrames=False, verbose=False)
self.reader = JpegReader(self, self.fps)
self.reader.setDaemon(True)
self.reader.start()
else:
self.videoFrameProcessor = VideoFrames(onlyIFrames=onlyIFrames, verbose=False)
def __init__( self, metalog=None, onlyIFrames=True ):
if metalog is None:
self._discovery()
metalog = MetaLog()
self.navdata = metalog.createLoggedSocket( "navdata", headerFormat="<BBBI" )
self.navdata.bind( ('',NAVDATA_PORT) )
if metalog.replay:
self.commandSender = CommandSenderReplay(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT), checkAsserts=metalog.areAssertsEnabled())
else:
self.commandSender = CommandSender(metalog.createLoggedSocket( "cmd", headerFormat="<BBBI" ),
hostPortPair=(HOST, COMMAND_PORT))
self.console = metalog.createLoggedInput( "console", myKbhit ).get
self.metalog = metalog
self.buf = ""
self.videoFrameProcessor = VideoFrames( onlyIFrames=onlyIFrames, verbose=False )
self.videoCbk = None
self.videoCbkResults = None
self.battery = None
self.flyingState = None
self.flatTrimCompleted = False
self.manualControl = False
self.time = None
self.altitude = None
self.position = (0,0,0)
self.angle = (0,0,0)
self.speed = (0,0,0)
self.positionGPS = None
self.cameraTilt, self.cameraPan = 0,0
self.lastImageResult = None
self.navigateHomeState = None
self.config()
self.commandSender.start()
def followme( metalog, assertWrite, ipPair ):
print "Follow Me"
if metalog is None:
metalog = MetaLog()
robot = HuskyROS( filename=metalog.getLog("node"), replay=metalog.replay, ipPair=ipPair )
print "started"
scanner = ScannerThread()
scanner.start()
scannerFn = SourceLogger( sourceGet=scanner.get, filename=metalog.getLog("scanner") ).get
else:
robot = HuskyROS( filename=metalog.getLog("node"), replay=True, assertWrite=assertWrite, ipPair=ipPair ) # TODO move assert to metalog
scannerFn = SourceLogger( sourceGet=None, filename=metalog.getLog("scanner") ).get
maxSpeed = 0.5
safeDist = 0.5
limitDist = 1.5
index = 0
prev = None
turn = 0.0
personDetected = False
while True:
minDist = scannerFn()
if minDist is not None:
prev = minDist[0]/1000.0
turnDiff = 160-minDist[1]
turn = -math.radians(turnDiff)/4.0
print prev, turnDiff
if prev is None or prev < safeDist:
robot.setSpeedPxPa( 0, turn/4 )
elif prev < safeDist*2:
robot.setSpeedPxPa( maxSpeed*(prev-safeDist)/safeDist, turn/2 )
elif prev < limitDist:
robot.setSpeedPxPa( maxSpeed, turn )
else:
robot.setSpeedPxPa( 0, 0 )
robot.update()
robot.setSpeedPxPa( 0, 0 )
robot.update()
log.write("%d\n" % index )
log.close()
if not metalog.replay:
scanner.requestStop()
scanner.join()
robot.canproxy.set_turn_raw(0)
go_straight(robot, distance=1.0, speed=0.3, with_stop=True)
for i in xrange(3):
turn(robot, math.radians(-45), radius=2.6, speed=0.5)
turn(robot, math.radians(-45), radius=2.6, speed=-0.5)
robot.canproxy.stop_turn()
robot.wait(3.0)
if __name__ == "__main__":
if len(sys.argv) < 2:
print __doc__
sys.exit(2)
metalog = None
if 'meta_' in sys.argv[1]:
metalog = MetaLog(filename=sys.argv[1])
elif len(sys.argv) > 2:
metalog = MetaLog(filename=sys.argv[2])
if len(sys.argv) > 2 and sys.argv[-1] == 'F':
disableAsserts()
if metalog is None:
metalog = MetaLog()
driver_self_test(Driver(), metalog)
# vim: expandtab sw=4 ts=4
def parse_and_launch():
"""parse sys.argv arguments and return initialized robot"""
parser = argparse.ArgumentParser(
description='Navigate given pattern in selected area')
subparsers = parser.add_subparsers(help='sub-command help', dest='command')
subparsers.required = True
parser_run = subparsers.add_parser('run', help='run on real HW')
parser_run.add_argument('config', help='configuration file')
parser_run.add_argument('--note', help='add description')
parser_replay = subparsers.add_parser('replay', help='replay from logfile')
parser_replay.add_argument('logfile', help='recorded log file')
parser_replay.add_argument('--view',
dest='view',
action='store_true',
help='view parsed log')
parser_replay.add_argument(
'--force',
'-F',
dest='force',
action='store_true',
help='force replay even for failing output asserts')
parser_replay.add_argument('--config',
dest='config',
help='use different configuration file')
args = parser.parse_args()
conf = None
if args.config is not None:
conf = Config.load(args.config)
viewer = None
if args.command == 'replay':
metalog = MetaLog(args.logfile)
if conf is None and args.logfile.endswith('.zip'):
if DEFAULT_ZIP_CONFIG in zipfile.ZipFile(args.logfile).namelist():
s = str(
zipfile.ZipFile(args.logfile).read(DEFAULT_ZIP_CONFIG),
'utf-8')
conf = Config.loads(s)
if args.view:
global g_img_dir
from tools.viewer import main as viewer_main
from tools.viewer import getCombinedPose
viewer = viewer_main
if args.logfile.endswith('.zip'):
g_img_dir = args.logfile
else:
g_img_dir = os.path.dirname(args.logfile)
if args.force:
disableAsserts()
elif args.command == 'run':
metalog = MetaLog()
else:
assert False, args.command # unsupported command
robot = create_robot(metalog, conf)
for sensor_name in ['gps', 'laser', 'camera']:
attach_sensor(robot, sensor_name, metalog)
if viewer is not None:
robot.extensions.append(('launcher_viewer', launcher_viewer_extension))
try:
robot.extensions.append(('emergency_stop', emergency_stop_extension))
yield robot, metalog, conf, viewer
except EmergencyStopException:
print("Emergency STOP Exception!")
robot.extensions = [] # hack
# TODO make this code conditional based on the state of finished application
robot.canproxy.stop()
robot.canproxy.stop_turn()
robot.wait(3.0)
detach_all_sensors(robot)
if viewer is not None:
viewer(filename=None, posesScanSet=viewer_data)
drone.land()
except ManualControlException, e:
print
print "ManualControlException"
if drone.flyingState is None or drone.flyingState == 1: # taking off
drone.emergency()
drone.land()
if __name__ == "__main__":
if len(sys.argv) < 3:
print __doc__
sys.exit(2)
metalog = None
if len(sys.argv) > 3:
metalog = MetaLog(filename=sys.argv[3])
if len(sys.argv) > 4 and sys.argv[4] == 'F':
disableAsserts()
drone = Bebop(metalog=metalog)
if sys.argv[2] == "fly":
print "Start flight program"
fly(drone)
if sys.argv[2] == "manual":
print "Start manual program"
manual(drone)
print "Battery:", drone.battery
# vim: expandtab sw=4 ts=4
def go(metalog, assertWrite, ipPair):
print "Go straight ..."
if metalog is None:
metalog = MetaLog()
robot = HuskyROS(filename=metalog.getLog("node"),
replay=metalog.replay,
ipPair=ipPair)
print "started"
scanner = ScannerThread()
scanner.start()
scannerFn = SourceLogger(sourceGet=scanner.get,
filename=metalog.getLog("scanner")).get
else:
scanner = DummyScanner() # only for access to scanner saveImages
robot = HuskyROS(filename=metalog.getLog("node"),
replay=True,
assertWrite=assertWrite,
ipPair=ipPair) # TODO move assert to metalog
scannerFn = SourceLogger(sourceGet=None,
filename=metalog.getLog("scanner")).get
maxSpeed = 0.25
safeDist = 0.5
limitDist = 1.5
index = 0
prev = None
obstacleDir = 0.0
personDetected = False
pauseCount = 0 # TODO use time instead
while True:
minDist = scannerFn()
scanner.saveImages = not robot.emergencyStopPressed
if minDist is not None:
prev = minDist[0] / 1000.0
turnDiff = 160 - minDist[1] # only flip
obstacleDir = -math.radians(
turnDiff) / 4.0 # TODO calibration via FOV
print prev, turnDiff, robot.power, robot.emergencyStopPressed
if robot.emergencyStopPressed:
pauseCount = 10
else:
pauseCount = max(0, pauseCount - 1)
if prev is None or prev < safeDist or pauseCount > 0:
robot.setSpeedPxPa(0, 0)
elif prev < safeDist * 2:
# turn in place
if obstacleDir > 0: # i.e. on the left
robot.setSpeedPxPa(0.0, math.radians(-10))
else:
robot.setSpeedPxPa(0.0, math.radians(10))
# robot.setSpeedPxPa( maxSpeed*(prev-safeDist)/safeDist, turn/2 )
# elif prev < limitDist:
# robot.setSpeedPxPa( maxSpeed, turn )
else:
robot.setSpeedPxPa(maxSpeed, 0)
robot.update()
robot.setSpeedPxPa(0, 0)
robot.update()
log.write("%d\n" % index)
log.close()
if not metalog.replay:
scanner.requestStop()
scanner.join()
'--force',
'-F',
dest='force',
action='store_true',
help='force replay even for failing output asserts')
parser_replay.add_argument('--config',
dest='config',
help='use different configuration file')
args = parser.parse_args()
conf = None
if args.config is not None:
conf = Config.load(args.config)
viewer = None
if args.command == 'replay':
metalog = MetaLog(args.logfile)
if args.view:
from tools.viewer import main as viewer_main
from tools.viewer import getCombinedPose
viewer = viewer_main
if args.logfile.endswith('.zip'):
g_img_dir = args.logfile
else:
g_img_dir = os.path.dirname(args.logfile)
if args.force:
disableAsserts()
elif args.command == 'run':
metalog = MetaLog()
else:
