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