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 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 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 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()
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()
