def lookAroundForMark(self, markNumPR=None, maxAttemptsPR=4): self.lookForward() headAngle = 0 back = False attempts = 0 incrementAngle = math.pi/12 maxAngle = math.pi/2 markData = None turnSpeed = .75 while not attempts >= maxAttemptsPR: time.sleep(0.35) markData = NaoMarkModule.getMarkData(self.memoryProxy, self.landmarkProxy) if not markData or len(markData) == 0: markData = None if markData is not None: if markNumPR is None or\ (NaoMarkModule.getMarkNumber(markData) == markNumPR): break if abs(headAngle) >= maxAngle: headAngle = max(min(headAngle, maxAngle), -maxAngle) back = ~back attempts += 1 if back: headAngle -= incrementAngle else: headAngle += incrementAngle self.motionProxy.angleInterpolation("HeadYaw", headAngle, turnSpeed, True) #while not reached maximum attempts return markData
def lookAroundForMarkMoving(self, number): motionProxy.moveInit() time.sleep(1) names = "HeadYaw" markFound = False angleLists = .25 back = False markData = NaoMarkModule.getMarkData(robotIP, PORT) first = True attempts = 0; while(not markFound): if(attempts >=2): return None if (back): angleLists = angleLists -.125 else: angleLists = angleLists + .125 if(angleLists > 1.0): angleLists = 1.0 back = True attempts = attempts + 1 else: if(angleLists < -1.0): angleLists = -1.0 back = False times = .15 if(first): times = 1.0 isAbsolute = True motionProxy.angleInterpolation(names, angleLists, times, isAbsolute) markData = NaoMarkModule.getMarkData(robotIP, PORT) if(not (markData is None or len(markData) ==0)): if(NaoMarkModule.getMarkNumber(markData) == number): markFound =True first = False return markData
def lookAroundForMark(self): motionProxy.moveInit() time.sleep(1) names = "HeadYaw" markFound = False first = True angleLists = .25 back = False markData = NaoMarkModule.getMarkData(robotIP, PORT) while(not markFound): if (back): angleLists = angleLists -.125 else: angleLists = angleLists + .125 if(angleLists > 1.0): angleLists = 1.0 back = True else: if(angleLists < -1.0): angleLists = -1.0 back = False times = .05 if(first): times = 2.0 print times isAbsolute = True motionProxy.angleInterpolation(names, angleLists, times, isAbsolute) markData = NaoMarkModule.getMarkData(robotIP, PORT) if(not (markData is None or len(markData) ==0)): markFound =True first = False return markData
#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)"