Python ALProxy.startTracker方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class FaceTracker():
    
    def __init__(self, use_nao, ip = "10.0.1.3", port = 9559):
        self.use_nao = use_nao
        if use_nao:
            self.faceProxy = ALProxy("ALFaceTracker", ip, port)
            self.motion = ALProxy("ALMotion", ip, port)
        
    def start_tracking(self):
        if self.use_nao:
            print "StartTracking"
            self.motion.setStiffnesses("Head", 1.0)
            self.faceProxy.startTracker()
            self.faceProxy.setWholeBodyOn(True)
    
    def stop_tracking(self):
        if self.use_nao:
            self.faceProxy.stopTracker()
            self.motion.setStiffnesses("Head", 0.0)
            self.to_default_pos()
            print "Tracking stopped"
    
    def to_default_pos(self):
        if self.use_nao:
            self.motion.setStiffnesses("Head", 0.5)
            self.motion.setAngles("HeadYaw", 0.0, 0.1)
            self.motion.setAngles("HeadPitch", -0.25, 0.1)
#            self.motion.setStiffnesses("Head", 0)
    
    def shake_no(self):
        if self.use_nao:
            names = "HeadYaw"
            currentAngle = self.motion.getAngles("Head", True)[0]
    
            angles = [0.25, 0, -0.25, 0, 0.25, currentAngle]
            angles = [currentAngle+0.25, currentAngle, currentAngle-0.25, currentAngle, currentAngle+0.25, currentAngle]
            times = [(i/len(angles))+0.2 for i in np.arange(1, len(angles)+1)]
                    
            self.faceProxy.stopTracker()
            print "no"
            self.motion.setStiffnesses("Head", 1.0)
            self.motion.angleInterpolation(names, angles, times, True)
            self.motion.setStiffnesses("Head", 0.0)
            self.start_tracking()
    
    def shake_yes(self):
        if self.use_nao:
            names = "HeadPitch"        
            currentAngle = self.motion.getAngles("Head", False)[1]
            angles = [currentAngle+0.15, currentAngle]
            times = [i/len(angles)*0.5 for i in np.arange(1, len(angles)+1)]
            
            self.faceProxy.stopTracker()
            self.motion.setStiffnesses("Head", 1.0)
            self.motion.angleInterpolation(names, angles, times, True)
            self.motion.setStiffnesses("Head", 0.0)
            self.start_tracking()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class ObjectTrackerModule(ALModule):
    def __init__(self, name):
        ALModule.__init__(self, name)
        self.tts = ALProxy("ALTextToSpeech")
        self.gestureProxy = ALProxy("NAOObjectGesture", myBroker)
        self.motionProxy = ALProxy("ALMotion", myBroker)
        self.memProxy = ALProxy("ALMemory", myBroker)

        self.motionProxy.setStiffnesses("Head", 1.0)
        self.gestureProxy.startTracker(15, 0)

        self.gestureProxy.addGesture("Drink", [2,6])
        self.gestureProxy.addGesture("FrogL", [1,0,7])
        self.gestureProxy.addGesture("FrogR", [3,4,5])

    def startTracker(self, camId):
        self.gestureProxy.startTracker(15, camId)
        self.gestureProxy.focusObject(-1)

    def stopTracker(self):
        self.gestureProxy.stopTracker()
        self.gestureProxy.stopFocus()

    def load(self, path, name):
        self.gestureProxy.loadDataset(path)
        self.gestureProxy.trackObject(name, -len(self.kindNames))
        self.memProxy.subscribeToMicroEvent(name, "ObjectTracker", name, "onObjGet")

    def onObjGet(self, key, value, message):
        id = -1
        if (key in self.kindNames):
            id = self.kindNames.index(key)
        else:
            return
        if (value != None):
            if (value[0] != 0):
                if (value[5]!=None):
                    print (value[5])
            else:
                if (value[1]!=None):
                    print (value[1])

    def unload(self):
        self.gestureProxy.stopTracker()
        for i in range(0, len(self.exists)):
            self.gestureProxy.removeObjectKind(0)
            self.gestureProxy.removeEvent(self.kindNames[i])
        self.gestureProxy.removeGesture("Drink")
        self.gestureProxy.removeGesture("FrogL")
        self.gestureProxy.removeGesture("FrogR")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class BallTracker:
        
    def __init__(self, glade_file_path=c.GLADE_FILE_PATH):
	self.glade_file_path = glade_file_path
            
	# Gtk Builder Init
	self.builder = Gtk.Builder()
	self.builder.add_from_file(self.glade_file_path)
	self.builder.connect_signals(self)

	# Add UI Components
	self.window = self.builder.get_object("ballWindow")

	# Show UI
	self.window.show_all()

	self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.0.2.3",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera(self)
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []

    ### Destroy GUI
    def destroy(self, widget):
	print "destroyed"
	Gtk.main_quit()


    def __del__(self, widget):
	self.tracker.stopTracker(self)
	pass

    # If Nao has ball returns True
    def hasBall(self, widget):
        self.checkForBall(self)
        time.sleep(0.5)
        if self.checkForBall(self):
	    print "I see the Ball"
            return True
        else:
	    print "Sorry I cant find the ball"
            return False

    # Nao scans around for the redball
    def searchBall(self, widget):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall(self):
                self.turnToBall(self)
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall(self):
                        self.turnToBall(self)
                        return


    # Nao walks close to ball
    def walkToBall(self, widget):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall(self)
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall(self)
                self.walkToBall(self)
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]

#.........这里部分代码省略.........
		"""

		if color in self.colors:
			color = self.colors[color]

		self.leds.fadeRGB("FaceLeds", color, fade_duration)

	def getHeadAngles(self):
		"""
		Returns current robot head angles as a list of yaw, pitch.
		For yaw, from the robot's POV, left is positive and right is negative. For pitch, up is positive and down is negative.
		See http://doc.aldebaran.com/2-1/family/robots/joints_robot.html for info on the range of its yaw and pitch.
		"""

		robot_head_yaw, robot_head_pitch = self.motion.getAngles("Head", False)

		# return adjusted robot head angles
		return [robot_head_yaw, -robot_head_pitch]

	def resetEyes(self):
		"""
		Turns eye LEDs white.
		"""

		self.leds.on("FaceLeds")		

	def trackFace(self):
		"""
		Sets face tracker to just head and starts.
		"""

		# start face tracker
		self.track.setWholeBodyOn(False)
		self.track.startTracker()

	def stopTrackingFace(self):
		"""
		Stops face tracker.
		"""

		self.track.stopTracker()

	def subscribeGaze(self):
		"""
		Subscribes to gaze analysis module so that robot starts writing gaze data to memory.
		Also sets the highest tolerance for determining if people are looking at the robot because those people's IDs are the only ones stored.
		"""

		self.gaze.subscribe("_")
		self.gaze.setTolerance(1)

	def getPeopleIDs(self):
		"""
		Retrieves people IDs from robot memory. If list of IDs was empty, return None.
		"""

		people_ids = self.mem.getData("GazeAnalysis/PeopleLookingAtRobot")

		if people_ids is None or len(people_ids) == 0:
			return None

		return people_ids

	def getRawPersonGaze(self, person_id):
		"""
		Returns person's gaze as a list of yaw (left -, right +) and pitch (up pi, down 0) in radians, respectively.

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
    print "Error was: ", e
    
try:
    memoryProxy = ALProxy("ALMemory",nao_ip, nao_port)
except Exception, e:
    print "Could not create proxy to ALMemory"
    print "Error was: ", e
    
 
motionProxy.setStiffnesses("Body", 1.0)

# First, set Head Stiffness to ON.
motionProxy.setStiffnesses("Head", 1.0)

# Then, start tracker.
redBallTracker.startTracker()    

# Will go to 1.0 then 0 radian
# in two seconds


#RIGHT FOOT
maxStepXRight = ["MaxStepX", 0.07]
maxStepYRight = ["MaxStepY", 0.065]
maxStepThetaRight = ["MaxStepTheta", 0.349]
maxStepFrequencyRight = ["MaxStepFrequency", 1]
stepHeightRight = ["StepHeight", 0.015]
torsoWxRight = ["TorsoWx", 0.0] 
torsoWyRight = ["TorsoWy", 0.0]

#LEFT FOOT

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class maThread(threading.Thread):
    def __init__(self, *args):
        """Initialisation de la thread, partie qui ne se joue qu'une seule fois."""
        threading.Thread.__init__(self)
        self.running = True
        self.lost = False
        self.countLost = 0
        global memory
        memory = ALProxy("ALMemory")
        self.motionProxy = ALProxy("ALMotion", NAO_IP, NAO_PORT)
        self.postureProxy= ALProxy("ALRobotPosture", NAO_IP, NAO_PORT)
        self.redBallTracker = ALProxy("ALRedBallTracker", NAO_IP, NAO_PORT)


        

    def run(self):
        """Partie de programme qui se répète."""
        global memory
        self.postureProxy.applyPosture("Sit",0.5)
        self.motionProxy.setStiffnesses("Head", 1.0)
        self.motionProxy.setAngles("HeadYaw", 0, 0.3)
        self.motionProxy.setAngles("HeadPitch", 0, 0.3)
        self.redBallTracker.startTracker()
        while self.running == True:
            #print "Bam : "+str(self.lost)
            if self.lost == False:

                #data = memory.getData("redBallDetected",1)
                
                if self.redBallTracker.isNewData():
                    position = self.redBallTracker.getPosition()
                    self.lost = False
                    #print "Position : "
                    #print "   x = "+str(position[0])+" y = "+str(position[1])+" z = "+str(position[2])
                    self.countLost = 0
                else:
                    # S'il n'y a pas de nouvelles données, la balle est perdue de vu et on attends 10 itérations histoire d'être sur
                    self.countLost=self.countLost+1
                    #print "CountLost = "+str(self.countLost)
                    time.sleep(1.0)

                    if (self.countLost > 5):
                        print "Balle perdue"
                        self.lost = True
                        self.countLost = 0


            if self.lost == True:
            		# Dans le cas ou la balle est perdue de vue, on fait des mouvement de la tête pour la retrouver
                    #TODO : faire un mouvement de la tête et des bras cool
                    

                    self.motionProxy.setAngles("HeadYaw", 0, 0.3)
                    self.motionProxy.setAngles("HeadPitch", 0, 0.3)
                    time.sleep(1.0)



                    self.motionProxy.setAngles("HeadPitch",0.5, 0.3)
                    time.sleep(0.5)
                    self.motionProxy.setAngles("HeadYaw", 1, 0.5)
                    time.sleep(1.2)
                    if self.redBallTracker.isNewData():
                        self.lost = False
                    else:
                        self.motionProxy.setAngles("HeadYaw", -0.70, 0.5)
                        time.sleep(1.0)
                    
                        if self.redBallTracker.isNewData():
                            self.lost = False
                        else:

                            self.motionProxy.setAngles("HeadPitch",-0.5, 0.3)
                            time.sleep(0.5)
                            self.motionProxy.setAngles("HeadYaw", 0.70, 0.5)
                            time.sleep(1.0)

                            if self.redBallTracker.isNewData():
                                self.lost = False
                            else:
                                self.motionProxy.setAngles("HeadYaw", 0, 0.3)
                                self.motionProxy.post.setAngles("HeadPitch", 0, 0.3)
                                time.sleep(1.0)


    def stop(self):
        """Permet un arrêt propre de la thread."""
        
        self.redBallTracker.stopTracker()
        self.redBallTracker = None
        self.running = False

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class NaoWalk():
    # INCOMPLETE AT: walkUpToBall(), include MCC-CALL and delete walkToPosition()

    # Usage description:
    # When the NaoWalk Phase started, call
    #       hasBall()
    #
    # to determine if NAO has the Redball in vision.
    # If not, call
    #       retrieveBall()
    #
    # to find and turn to the ball
    # If NAO has a ball in vision, call
    #       walkUpToBall()
    #
    # NAO will walk up closely to the Redball and call the MCC to
    # request the NXT to move to his next position
    # After the NXT left his initial Position, call
    #       walkToPosition()
    #
    # and NAO makes his few last steps.
    #
    # Repeat this progress until NAO has reached
    # the target area.

    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"194.95.174.146",9559)
        self.motion = ALProxy("ALMotion")
        self.tracker = ALProxy("ALRedBallTracker")
        self.vision = ALProxy("ALVideoDevice")
        self.tts = ALProxy("ALTextToSpeech")
        self.currentCam = 0
        self.__setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []
        # ALModule.__init__(self,name)

    def __del__(self):
        self.tracker.stopTracker()
        pass

    # determines whether NAO sees a redball or not
    # returns true is he sees one, false if not
    def hasBall(self):
        self.__checkForBall()
        time.sleep(0.5)
        if self.__checkForBall():
            return True
        else :
            return False

    # NAO scans his environment for the redball
    # after calling, NAO either has a redball in vision or there is none in
    # his range of sight.
    # Maybe include MCC-CALL after failure
    def retrieveBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.__turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.__turnToBall()
                        return

    # lets the nao walk close up to the ball
    # Redball in vision is mandatory to call this method!
    # !! NEED TO INCLUDE MCC-CALL TO MOVE NXT TO NEXT POSITION IN LINE 85 !!
    def walkUpToBall(self):
        ballPosi = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosi[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.__turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.__turnToBall()
                self.walkUpToBall()
                break
            dist = self.__getDistance()
            if dist < 0.65:
                self.motion.stopWalk()
                self.__turnToBall()
                self.__safePosition()
#                    # !!
#                    # meldung ans mcc dass nxt weiterlaufen soll
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
    motion = ALProxy("ALMotion", "127.0.0.1", robot_ID)
    motion.setStiffnesses("Body",1.0)
    
    # get robot coordinates
    x = 3.55122# value taken from the translation coordinate for x on webots
    y = 0.321583 # y is actually z in the translaion of the robot on webots
    r1_loc = [x,y]
    print_coordinates("Robot 1's Location is: ",r1_loc)
    
    #set robot posture to prepare for walking - increased stability
    postureProxy = ALProxy("ALRobotPosture", robot_IP, robot_ID)
    postureProxy.goToPosture("StandInit", 0.5)

    #get ball coordinates
    ball = ALProxy("ALRedBallTracker", robot_IP, robot_ID)
    ball.startTracker()
    time.sleep(1)
    ball_loc = ball.getPosition()
    ball.stopTracker()
    print_coordinates('Ball Location is: ',ball_loc)
    
    #get goal location: hard-coded for now
    goal_loc = [4.5,0.0]
    print_coordinates("Goal Location is: ", goal_loc)
    
    '''go to the ball
    call chasing module here... hard-coded to start close enough to the ball for now
    ***ENDS GETTING TO BALL*****'''
    
    
    # get Robot's target location

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
print "Connecting to", IP, "with port", PORT

motion = ALProxy("ALMotion", IP, PORT)
faceTracker = ALProxy("ALFaceTracker", IP, PORT)
posture = ALProxy("ALRobotPosture", IP, PORT)

# change tracking from just head to whole body movement
faceTracker.setWholeBodyOn(True)

# stiffen body
motion.setStiffnesses("Body", 1.0)

# stand up in balanced stance
posture.goToPosture("StandInit", 0.5)

# Then, start tracker.
faceTracker.startTracker()

wait = 20

print "ALFaceTracker successfully started, now show your face to Nao!"
print "ALFaceTracker will be stopped in", wait, "seconds."

time.sleep(wait)

# Stop tracker, sit down, remove stiffness
faceTracker.stopTracker()
posture.goToPosture("Crouch", 0.2)
motion.setStiffnesses("Body", 0.0)

print "ALFaceTracker stopped."

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
# John Dulin
# July 9, 2012

import sys
import time
from naoqi import ALProxy

IP = "127.0.0.1"
PORT = "9559"

speech = ALProxy("ALTextToSpeech", IP, PORT)
motion = ALProxy("ALMotion", IP, PORT)
tracker = ALProxy("ALRedBallTracker", IP, PORT)

# First, set the head stiffness to on.
motion.setStiffnesses("Head", 1.0)

# Then, start the tracker.
tracker.startTracker()

print "Tracker started successfully, now show a ball to Nao!"
speech.say("Show a ball to me!")
print "The tracker will stop in 60 seconds"

time.sleep(60)

print "Tracker is stopping..."
speech.say("Alright, I'm tired of that.")
tracker.stopTracker()
motion.setStiffnesses("Head", 0.0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]
class Nao():

    # Usage
    # Nao().searchBall()
    # Nao().hasBall()
    # Nao().walkToBall()
    # Nao().protectionOff()  should call before moves
    
    def __init__(self):
        self.myBroker = ALBroker("myBroker","0.0.0.0",0,"10.51.5.167",9559)
        self.motion   = ALProxy("ALMotion")
        self.tracker  = ALProxy("ALRedBallTracker")
        self.vision   = ALProxy("ALVideoDevice")
        self.tts      = ALProxy("ALTextToSpeech")
        self.currentCamera = 0
        self.setTopCamera()
        self.tracker.setWholeBodyOn(False)
        self.tracker.startTracker()
        self.ballPosition = []
        self.targetPosition = []


    def __del__(self):
        self.tracker.stopTracker()
        pass


    # If Nao has ball returns True
    def hasBall(self):
        self.checkForBall()
        time.sleep(0.5)
        if self.checkForBall():
            return True
        else:
            return False


    # Nao scans around for the redball
    def searchBall(self):
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        for turnAngle in [0,math.pi/1.8,math.pi/1.8,math.pi/1.8]:
            if turnAngle > 0:
                self.motion.walkTo(0,0,turnAngle)
            if self.hasBall():
                self.turnToBall()
                return
            for headPitchAngle in [((math.pi*29)/180),((math.pi*12)/180)]:
                self.motion.angleInterpolation("HeadPitch", headPitchAngle,0.3,True)
                for headYawAngle in [-((math.pi*40)/180),-((math.pi*20)/180),0,((math.pi*20)/180),((math.pi*40)/180)]:
                    self.motion.angleInterpolation("HeadYaw",headYawAngle,0.3,True)
                    time.sleep(0.3)
                    if self.hasBall():
                        self.turnToBall()
                        return


    # Nao walks close to ball
    def walkToBall(self):
        ballPosition = self.tracker.getPosition()
        headYawTreshold = ((math.pi*10)/180)
        x = ballPosition[0]/2 + 0.05
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.turnToBall()
        self.motion.post.walkTo(x,0,0)
        while True:
            headYawAngle = self.motion.getAngles("HeadYaw", False)
            if headYawAngle[0] >= headYawTreshold or headYawAngle[0] <= -headYawTreshold:
                self.motion.stopWalk()
                self.turnToBall()
                self.walkToBall()
                break
            #dist = self.getDistance()
            # if dist < 0.1:
            #     self.motion.stopWalk()
            #     self.turnToBall()
            #     self.safePosition()
            #     time.sleep(1)
            #     self.tts.say("You have 5 seconds to move the ball")
            #     time.sleep(5)
            #     # naoWalkToPosition()
            #     self.walkToPosition()
            #     self.setTopCamera()
            #     break
                return


    # nao turns to ball 
    def turnToBall(self):
        if not self.hasBall():
            return False
        self.ballPosition = self.tracker.getPosition()
        b = self.ballPosition[1]/self.ballPosition[0]
        z = math.atan(b)
        self.motion.stiffnessInterpolation("Body", 1.0, 0.1)
        self.motion.walkInit()
        self.motion.walkTo(0,0,z)


#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]

#.........这里部分代码省略.........
		"""
		Robot stands
		"""

		self.pose.goToPosture("Stand", 0.4)

	def turnHead(self, yaw = None, pitch = None, speed = 0.2):
		"""
		Turns robot head to the specified yaw and/or pitch in degrees at the given speed.
		Yaw can range from 119.5 deg (left) to -119.5 deg (right) and pitch can range from 38.5 deg (down) to -29.5 deg (up).
		"""

		if not yaw is None:
			self.motion.setAngles("HeadYaw", math.radians(yaw), speed)
		if not pitch is None:
			self.motion.setAngles("HeadPitch", math.radians(pitch), speed)

	def moveRightArm(self, pitch): # add yaw!
		"""
		Extends robot's right arm to point at the specified yaw and pitch in degrees.
		The angles below are RShoulderPitch, RShoulderRoll, RElbowRoll, RWristYaw, and RHand.
		The angle ranges can be found at http://doc.aldebaran.com/1-14/family/robots/joints_robot.html#right-arm-joints
		For example, the pitch ranges from 119.5 (down) to 0 (horizontal) to -119.5 (up).
		"""

		angles = [pitch, 12, 39, 3, -8] # angles in degrees
		angles = [math.radians(angle) for angle in angles] # convert to radians
		angles.append(1) # also open hand
		times = [1, 1, 1.5, 1.5, 1.5, 1.5]
		self.motion.angleInterpolation("RArm", angles, times, True) # move to those arm angles and open hand

	def moveLeftArm(self, pitch): # add yaw!
		"""
		Extends robot's left arm to point at the specified yaw and pitch in degrees.
		The angles below are LShoulderPitch, LShoulderRoll, LElbowRoll, LWristYaw, and LHand.
		The angle ranges can be found at http://doc.aldebaran.com/1-14/family/robots/joints_robot.html#left-arm-joints
		For example, the pitch ranges from 119.5 (down) to 0 (horizontal) to -119.5 (up).
		"""

		angles = [pitch, -12, -39, -3, 8] # angles in degrees
		angles = [math.radians(angle) for angle in angles] # convert to radians
		angles.append(1) # also open hand
		times = [1, 1, 1.5, 1.5, 1.5, 1.5]
		self.motion.angleInterpolation("LArm", angles, times, True) # move to those arm angles and open hand

	def colorEyes(self, color, fade_duration = 0.2):
		"""
		Fades eye LEDs to specified color over the given duration.
		"Color" argument should be either in hex format (e.g. 0x0063e6c0) or one of the following
		strings: pink, red, orange, yellow, green, blue, purple
		"""

		if color in self.colors:
			color = self.colors[color]

		self.leds.fadeRGB("FaceLeds", color, fade_duration)

	def getHeadAngles(self):
		"""
		Returns current robot head angles in radians as a list of yaw, pitch.
		For yaw, from the robot's POV, left is positive and right is negative. For pitch, up is positive and down is negative.
		See http://doc.aldebaran.com/2-1/family/robots/joints_robot.html for info on the range of its yaw and pitch.
		"""

		robot_head_yaw, robot_head_pitch = self.motion.getAngles("Head", False)

		# return adjusted robot head angles
		return [robot_head_yaw, -robot_head_pitch]

	def getArmAngles(self):
		"""
		Returns all arm angles in radians as a list in the following order.
		[[LShoulderPitch, LShoulderRoll, LElbowRoll, LWristYaw, LHand],
		[RShoulderPitch, RShoulderRoll, RElbowRoll, RWristYaw, RHand]]
		"""

		return [self.motion.getAngles("LArm", True), self.motion.getAngles("RArm", True)]

	def resetEyes(self):
		"""
		Turns eye LEDs white.
		"""

		self.leds.on("FaceLeds")

	def trackFace(self):
		"""
		Sets face tracker to just head and starts.
		"""

		# start face tracker
		self.track.setWholeBodyOn(False)
		self.track.startTracker()

	def stopTrackingFace(self):
		"""
		Stops face tracker.
		"""

		self.track.stopTracker()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import startTracker [as 别名]

#.........这里部分代码省略.........
		# return adjusted robot head angles
		return [robot_head_yaw, -robot_head_pitch]

	def resetEyes(self):
		"""
		Turns eye LEDs white.
		"""

		self.leds.on("FaceLeds")

	def waitForSound(self, time_limit = 7):
		"""
		Waits until either a sound is detected or until the given time limit expires.
		"""

		self.sound.subscribe("sound_detection_client")

		# give waiting a 7-second time limit
		timeout = time.time() + 7

		# check for new sounds every 0.2 seconds
		while (self.mem.getData("SoundDetected")[0] != 1) and (time.time() < timeout):
			time.sleep(0.2)

		self.sound.unsubscribe("sound_detection_client")

	def trackFace(self):
		"""
		Sets face tracker to just head and starts.
		"""

		# start face tracker
		self.track.setWholeBodyOn(False)
		self.track.startTracker()

	def stopTrackingFace(self):
		"""
		Stops face tracker.
		"""

		self.track.stopTracker()

	def recordObjectAngles(self, object_angles):

		# initialize confidences for each object
		self.object_yaws, self.object_pitches = zip(*object_angles)
		self.angle_error = math.radians(15)

	def initGazeCounts(self):
		self.gaze_counts = [[yaw, 0] for yaw in self.object_yaws]

	def updatePersonID(self):
		"""
		Tries to get people IDs from robot memory.
		If none are retrieved, tries again every 0.5 seconds until it gets some.
		Stores the first person ID in the list as self.person_id.
		"""

		# try to get list of IDs of people looking at robot
		people_ids = self.mem.getData("GazeAnalysis/PeopleLookingAtRobot")

		# if robot hasn't gotten any people IDs
		while people_ids is None or len(people_ids) == 0:

			# wait a little bit, then try again
			time.sleep(0.5)