def detectMarkSearchForward(self, number): markD = None searching = True global naomarkSize global robotIP global PORT while(searching): markD = customMotions.lookAroundForMarkMoving(self, number) if(not (markD is None or len(markD) ==0)): print "found something" searching = False else : print "tried turn" customMotions.moveForward(self, .02) x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) customMotions.moveForwardY(self,x, y)
def detectMarkAndMoveTo(self, markNumPR=None, maxAttemptsPR=4, stoppingDistancePR=.25, lateralOffsetPR=0, walkStraightPR=True): markData = self.lookAroundForMark(markNumPR) if markData is None: print "Mark not found!" return False if walkStraightPR: while abs(self.getLookAngle()) > math.pi/12: markData = self.lookAroundForMark(markNumPR, maxAttemptsPR) self.turnToLookAngle() x, y, z = NaoMarkModule.getMarkXYZ(self.motionProxy, markData, self.naoMarkSize) print "Mark detected at x:{}, y:{} meters from NAO position"\ .format(x, y) self.walkTo(x - stoppingDistancePR, y + lateralOffsetPR) return True
#Testing file for NaoMarkModule getMark XYZ function from naoqi import ALProxy import NaoMarkModule #IP of the Robot IP = "10.0.0.7" #Port Number of the Robot PORT = 9559 #Get the x,y,z in meters markData = NaoMarkModule.getMarkData(IP, PORT) #Get data until face is found while (markData is None or len(markData) == 0): markData = NaoMarkModule.getMarkData(IP, PORT) x, y, z = NaoMarkModule.getMarkXYZ(IP, PORT, markData, 0.04) #Print information print "x " + str(x) + " (in meters)" print "y " + str(y) + " (in meters)" print "z " + str(z) + " (in meters)"
def detectMarkAndMoveTo(self): markD = customMotions.lookAroundForMark() x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, .08) customMotions.moveForwardY(x, y)
def detectMarkWalkStraight(self): markD = customMotions.lookAroundForMark() x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, .08) customMotions.turnToHeadStraight(markD) customMotions.detectMarkAndMoveTo()
def detectMarkAndMoveToLeft(self, number): markD = customMotions.lookAroundForMark(self,number) x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) customMotions.moveForwardY(self,x, y +.30)
def detectMarkAndMoveToRight(self): markD = customMotions.lookAroundForMark(self) x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) customMotions.moveForwardY(self,x, y-.35)
def detectMarkWalkStraight(self): markD = customMotions.lookAroundForMark(self) global naomarkSize x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) customMotions.turnToHeadStraight(self,markD) customMotions.detectMarkAndMoveTo(self)
def detectMarkAndMoveToLeft(): markD = lookAroundForMark() x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) moveForwardY(x, y +.25)
def detectMarkWalkStraight(): markD = lookAroundForMark() global naomarkSize x,y,z = NaoMarkModule.getMarkXYZ(robotIP, PORT, markD, naomarkSize) turnToHeadStraight(markD) detectMarkAndMoveTo()