Python ALProxy.stopTracker方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [as 别名]
def main(IP, PORT, faceSize):

    print "Connecting to", IP, "with port", PORT
    motion = ALProxy("ALMotion", IP, PORT)
    tracker = ALProxy("ALTracker", IP, PORT)

    # First, wake up.
    motion.wakeUp()

    # Add target to track.
    targetName = "Face"
    faceWidth = faceSize
    tracker.registerTarget(targetName, faceWidth)

    # Then, start tracker.
    tracker.track(targetName)

    print "ALTracker successfully started, now show your face to robot!"
    print "Use Ctrl+c to stop this script."

    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print
        print "Interrupted by user"
        print "Stopping..."

    # Stop tracker.
    tracker.stopTracker()
    tracker.unregisterAllTargets()
    motion.rest()

    print "ALTracker stopped."

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [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 stopTracker [as 别名]
def main(IP, PORT, ballSize, effector):

    print "Connecting to", IP, "with port", PORT

    motion = ALProxy("ALMotion", IP, PORT)
    posture = ALProxy("ALRobotPosture", IP, PORT)
    tracker = ALProxy("ALTracker", IP, PORT)

    # First, wake up.
    motion.wakeUp()

    fractionMaxSpeed = 0.8
    # Go to posture stand
    posture.goToPosture("StandInit", fractionMaxSpeed)

    # Add target to track.
    targetName = "RedBall"
    diameterOfBall = ballSize
    tracker.registerTarget(targetName, diameterOfBall)

    # set mode
    mode = "Head"
    tracker.setMode(mode)

    # set effector
    tracker.setEffector(effector)

    # Then, start tracker.
    tracker.track(targetName)

    print "ALTracker successfully started, now show a red ball to robot!"
    print "Use Ctrl+c to stop this script."

    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print
        print "Interrupted by user"
        print "Stopping..."

    # Stop tracker, go to posture Sit.
    tracker.stopTracker()
    tracker.unregisterAllTargets()
    tracker.setEffector("None")
    posture.goToPosture("Sit", fractionMaxSpeed)
    motion.rest()

    print "ALTracker stopped."

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [as 别名]
def main(IP, PORT):

    print "Connecting to", IP, "with port", PORT
    motion = ALProxy("ALMotion", IP, PORT)
    posture = ALProxy("ALRobotPosture", IP, PORT)
    tracker = ALProxy("ALTracker", IP, PORT)

    # First, wake up.
    motion.wakeUp()

    fractionMaxSpeed = 0.8

    # Go to posture stand
    posture.goToPosture("StandInit", fractionMaxSpeed)

    # Set target to track.
    eventName = "ALTracker/BlobDetected"

    # set mode
    mode = "Move"
    tracker.setMode(mode)

    # Set the robot relative position to target
    # The robot stays a 50 centimeters of target with 10 cm precision
    tracker.setRelativePosition([-0.5, 0.0, 0.0, 0.1, 0.1, 0.3])

    # Then, start tracker.
    tracker.trackEvent(eventName)

    print "ALTracker successfully started."
    print "Use Ctrl+c to stop this script."

    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print
        print "Interrupted by user"
        print "Stopping..."

    # Stop tracker, go to posture Sit.
    tracker.stopTracker()
    tracker.unregisterAllTargets()
    posture.goToPosture("Sit", fractionMaxSpeed)
    motion.rest()

    print "ALTracker stopped."

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [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 stopTracker [as 别名]
            #maxStepYLeft,
            #["MaxStepTheta", 0.0],
            maxStepFrequencyLeft,
            stepHeightLeft,
            torsoWxLeft,
            torsoWyLeft])
    
    time.sleep(.5)
    crdArry = redBallTracker.getPosition()
    if (math.sqrt(math.pow(crdArry[0], 2) +math.pow(crdArry[1],2))<=threshold and crdArry[0]!=0):
        print "distance from Ball: ",math.sqrt(math.pow(crdArry[0], 2) +math.pow(crdArry[1],2))
        break

motionProxy.setWalkTargetVelocity(0.0,0.0,0.0,0.0)
# Stop tracker and remove head stiffness.
redBallTracker.stopTracker()
motionProxy.setStiffnesses("Head", 0.0)

maxStepXRight = ["MaxStepX", 0.07]
maxStepFrequencyRight = ["MaxStepFrequency", 1]
stepHeightRight = ["StepHeight", 0.015]
torsoWxRight = ["TorsoWx", 0.0] 
torsoWyRight = ["TorsoWy", 0.0]

#LEFT FOOT
maxStepXLeft = ["MaxStepX", 0.7]
maxStepFrequencyLeft = ["MaxStepFrequency", 1]
stepHeightLeft = ["StepHeight", 0.015]
torsoWxLeft = ["TorsoWx", 0.0] 
torsoWyLeft = ["TorsoWy", 0.0]    

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [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 stopTracker [as 别名]
#
# Summer at the Edge 2012 Project: Humanoid Robotics with NAO
# John Dulin, Case Western Reserve University
# August 3, 2012
#
# This simple script resets the robot after an error.  It removes motor stiffness and stops the tracker.
# This should only be ran when someone is ready to catch the robot when it falls.
#

from naoqi import ALProxy

motion = ALProxy("ALMotion", "127.0.0.1", 9559)
tracker = ALProxy("ALRedBallTracker", "127.0.0.1", 9559)

tracker.stopTracker()
motion.setStiffnesses("Body", 0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [as 别名]
class NaoFaceTrack:
    def __init__(self, ip, port):
        #self.__proxyTTS = ALProxy("ALTextToSpeech", ip, port) #test only, will need to use publisher later
        #self.__proxyVideo = ALProxy("ALVideoDevice", ip, port)
        #self.__proxyFace = ALProxy("ALFaceDetection", ip, port)
        #self.__proxyMemory = ALProxy("ALMemory", ip, port)		              
    	
    	
    	self.__proxyTracker = ALProxy("ALTracker", ip, port)
        self.__proxyMotion = ALProxy("ALMotion", ip, port)
        
    	#self.__face_sub = rospy.Subscriber( 
    	#	_Constants.face_det_topic, 
    	#	Face_detection, 
    	#	self.face_tracker)		
        
        self.__facepub = rospy.Publisher(
            _Constants.face_res_topic, 
            Face_detection , 
            latch=True, 
            queue_size =1000)
        
        #name_id = self.__proxyVideo.subscribe(
        #        _Constants.unique_name,
        #        _Constants.resolution,
        #        _Constants.colour_space,
        #        _Constants.fps )
        
        #Period = 500
        #self.__proxyFace.subscribe("TEST", Period, 0.0)	
           

    def face_tracker(self):
        # Stop sound localization
        #self.__facepub.publish(True, 0.0)
        
        # First, wake up
        self.__proxyMotion.setStiffnesses("Head", 1.0)
        
		# Add target to track.
        targetName = "Face"
        faceWidth = 0.1
        self.__proxyTracker.registerTarget(targetName, faceWidth)
        
		# Then, start tracker.
        self.__proxyTracker.track(targetName)
        
        rospy.logwarn("ALTracker successfully started, now show your face to robot!")
        rospy.logwarn("Use Ctrl+c to stop this script.")
        
        rate = rospy.Rate(10) # 10hz
        
        while not rospy.is_shutdown():
            rate.sleep()

        # Stop tracker.
        rospy.logwarn("Stopping...")
        self.__proxyTracker.stopTracker()
        self.__proxyTracker.unregisterAllTargets()
        self.__proxyMotion.setStiffnesses("Head", 0.0)
        rospy.logwarn("Stopped")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [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 stopTracker [as 别名]
class SoccerModule(ALModule):
    """A simple module allowing Nao to play soccer"""

    def __init__(self, var_name):
        """Define and initialize the class' attributes"""

        # Request the parent module to initialize and register our module
        ALModule.__init__(self, var_name)

        # Initialize the memory
        global memory
        memory = ALProxy("ALMemory")
        # Initialize the motion module
        self.motion = ALProxy("ALMotion")
        # Initialize the posture module
        self.posture = ALProxy("ALRobotPosture")
        # Initialize the tracker module
        self.tracker = ALProxy("ALTracker")

        # Initialize the goal position
        self.goal = [0, 0]
        # Initialize the ball position
        self.ball = [0, 0]

    def initialize(self):
        """Subscribe to the required events and enable the different sub-modules"""

        # Enable the fall manager
        self.motion.setFallManagerEnabled(True)
        # Enable the body balancer
        self.motion.wbEnable(True)

        # Wake the robot up
        self.motion.wakeUp()
        # Request the robot to stand
        self.posture.goToPosture("StandInit", 0.5)

        # Wait for some time
        sleep(1)

        # Subscribe to the redBallDetected event
        memory.subscribeToEvent("redBallDetected", self.getName(), "on_red_ball")
        # Subscribe to the robot has fallen event
        memory.subscribeToEvent("robotHasFallen", self.getName(), "on_fall")
        # Subscribe to the landmarkDetected event
        memory.subscribeToEvent("LandmarkDetected", self.getName(), "on_landmark_detected")

        # Register the red ball and landmark as targets for the tracker module
        self.tracker.registerTarget("RedBall", 0.5)  # The second parameter is the distance to keep with the ball
        self.tracker.registerTarget("LandMark", [0.2, [85]])  # The second parameter is the target's size, followed by the mark id

        # Fix the tracker's limits
        self.tracker.setMaximumDistanceDetection(0.5)  # Objects more than 50 centimeters away are not considered
        self.tracker.setTimeOut(2000)  # Objects not detected for more than 2 sec are lost

        # Have the robot look for the targets if they are lost
        self.tracker.toggleSearch(True)

        # Have the robot track the red ball
        self.tracker.track("RedBall")

    def shutdown(self):
        """Define the procedure to follow upon shutting down"""

        # If the tracker is still running
        if self.tracker.isActive():
            # Shut the tracker down
            self.tracker.stopTracker()

        # Unregister all tracker's targets
        self.tracker.unregisterAllTargets()
        # Request the robot to go in resting position
        self.motion.rest()
        # Set the body stiffness to 0
        self.motion.setStiffnesses("Body", 0)

    def on_red_ball(self):
        """Compute the position of the ball relative to the robot and move towards it"""

        # Check if the tracker is stopped or not tracking the red ball
        if not self.tracker.isActive() or self.tracker.getActiveTarget() != "RedBall":
            # Stop the tracker
            self.tracker.stopTracker()

            # Ask the robot to track the target by moving towards it
            self.tracker.setMode("Move")
            # Start to track the red ball
            self.tracker.track("RedBall")
        else:
            # Get the data from the memory
            # Apparently the robot blocks all calls here because the data query is really slow.
            data = memory.getData("redBallDetected")
            ball_info = data[1]
            camera_pos = data[3]

            # Compute the position of the ball relative to the robot
            self.ball[0] = camera_pos[2] * tan(camera_pos[4] + ball_info[1] + ball_info[2]) + camera_pos[0]
            self.ball[1] = camera_pos[2] * tan(camera_pos[3]) + camera_pos[1]

            # Compute the distance between the robot and the ball
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopTracker [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 stopTracker [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 stopTracker [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 stopTracker [as 别名]

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

		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)
			people_ids = self.mem.getData("GazeAnalysis/PeopleLookingAtRobot")

		self.person_id = people_ids[0]

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