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