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 turnToHeadStraight(self,markData):
motionProxy.moveInit()
time.sleep(1)
global curAngle
deltaAngle,dontcare = NaoMarkModule.getMarkAngles(markData)
angle = motionProxy.getAngles("HeadYaw",False)
print angle[0]
motionProxy.moveTo(0, 0, angle[0])
print "Moved to head straight"
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
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()
#Motion code snippet for continuously centering head to a visible nao mark (tests naoMarkModule getMarkAngles function)
#Mitch Deplazes
#2/2/16
from naoqi import ALProxy
import NaoMarkModule
import time
#IP of the Robot
IP = "10.0.0.6"
#Port Number of the Robot
PORT = 9559
#create a motion proxy
motionProxy = ALProxy("ALMotion", IP, PORT)
while True:
#Get angle of the mark
wzCamera, wyCamera = NaoMarkModule.getMarkAngles(IP, PORT)
#Update head postion to center naomark
motionProxy.setStiffnesses("Body", 1.0)
motionProxy.changeAngles("HeadYaw", wzCamera, 0.1)
motionProxy.changeAngles("HeadPitch", wyCamera, 0.1)
time.sleep(1)
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)
# File for test NaoMarkModule getMarkNumber function
from naoqi import ALProxy
import NaoMarkModule
# IP of the Robot
IP = "10.0.0.7"
# Port Number of the Robot
PORT = 9559
# Make proxy to speech
speechProxy = ALProxy("ALTextToSpeech", IP, PORT)
markData = NaoMarkModule.getMarkData(IP, PORT)
while markData is None or len(markData) == 0:
markData = NaoMarkModule.getMarkData(IP, PORT)
# Get mark number
markID = NaoMarkModule.getMarkNumber(markData)
# Say mark number with speech
speechProxy.say("The Nao Mark I see is " + str(markID))
print markID
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()
