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