def moveHead():
alphaDEG = toDEG(getHead()[0])
betaDEG = toDEG(getHead()[1])
if(alphaDEG < 4 and alphaDEG > -4):
motion_poseInit.setHeadMotion(30, betaDEG)
#left
elif(alphaDEG > 0):
if(alphaDEG <= 90):
motion_poseInit.setHeadMotion(alphaDEG+30, betaDEG)
return False
else:
motion_poseInit.setHeadMotion(-30, betaDEG)
return False
#right
elif(alphaDEG < 0):
if(alphaDEG >= -90):
motion_poseInit.setHeadMotion(alphaDEG-30, betaDEG)
return False
else:
motion_poseInit.setHeadMotion(0, betaDEG)
return False
def findNXT(NXTColor):
# set the initial head position for NAO
motion_poseInit.setHeadMotion(0, 0)
# this are the current intervals in degree in which the NAO looks for the nxt
pitchIntervals = [10, 20, 0]
yawIntervals = [0, -30, -60, 0, 30, 60]
# in every pitch motion interval is a yaw motion of the NXT
for g in range(0, len(pitchIntervals)):
# move
for h in range(0, len(yawIntervals)):
# set the head position to the current yaw and pitch interval
motion_poseInit.setHeadMotion(yawIntervals[h], pitchIntervals[g])
allDetectedMarker = [[[],[],[],[], []], [[],[],[],[], []], [[],[],[],[], []], [[],[],[],[], []], [[],[],[],[], []], [[],[],[],[], []]]
#make 5 measurements to be sure that there is / is no marker detected
for i in range(0, 5):
# sleep time is necessary otherwise the NAO has no chance to realize marker because of head movement
time.sleep(0.8)
val = memoryProxy.getData(memValue)
# same like in detectMarkerAndCalcDist
if(len(val)>=2):
numberOfMarker = len(val[1])
else:
numberOfMarker = 0
# Check whether we found some markers
if(numberOfMarker >= 1):
# fill array with all possible makers
for j in range(0, numberOfMarker):
try:
# First Field = Data field
markerDataField = val[1][j][0]
allDetectedMarker[j][0].append(markerDataField[1]) #insert the alphaValue
allDetectedMarker[j][1].append(markerDataField[2]) #insert the betaValue
# Second Field = Extra info (ie, mark ID)
markerID = val[1][j][1][0]
allDetectedMarker[j][3].append(markerID) #insert the markerID
except Exception, e:
print "Naomarks detected, but it seems getData is invalid. ALValue = "
print val
print "Error msg %s" % (str(e))
# if could find minimal one marker
if(allDetectedMarker[0][0] != []):
# calculate the averages for all detected markers and the color of the marker
for i in range(0,6):
# only calculate if there is one marker
if(allDetectedMarker[i][0] != []):
avgID = mostFrequent(allDetectedMarker[i][3]) #avgID
print allDetectedMarker
# center the head of the nao to the nxt marker if the right Marker was found
print markerPosition[0]
if(avgID in markerPosition[0]):
# TODO: make color recognition of this marker
avgAlpha = calculateAVG(allDetectedMarker[i][0]) #avgAlphaArray
avgBeta = calculateAVG(allDetectedMarker[i][1]) #avgBetaArray
motion_poseInit.setHeadMotion(toDEG(getHead()[0]+avgAlpha), toDEG(getHead()[1]+avgBeta))
tts = ALProxy("ALTextToSpeech")
tts.say('NXT with color ' + str(NXTColor) + ' found!')
return True
