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