Python ALProxy.stopMove方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
def main(robotIP, PORT=9559):
    motionProxy  = ALProxy("ALMotion", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

# 	motionProxy.moveInit()

    # Example showing the use of moveToward
    # The parameters are fractions of the maximums
    # Here we are asking for full speed forwards, 即x = 1.0
    x     = 1.0
    y     = 0.0
    theta = 0.0
    frequency = 1.0
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # If we don't send another command, he will move forever
    # Lets make him slow down(step length) and turn after 3 seconds
    time.sleep(3) # 延时3秒运动

    x     = 0.5
    theta = 0.6
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # Lets make him slow down(frequency) after 3 seconds
    time.sleep(3)
    frequency = 0.5 # 频率1为最快, 0.5表示50%的频率运动
    motionProxy.moveToward(x, y, theta, [["Frequency", frequency]])

    # Lets make him stop after 3 seconds
    time.sleep(3)
    motionProxy.stopMove()

	# 详细的Move Config配置
	##################################
	#
    # TARGET VELOCITY
    X         = 1.0
    Y         = 0.0
    Theta     = 0.0
    Frequency = 1.0

    # Defined a limp walk
    try:
        motionProxy.moveToward(X, Y, Theta,[["Frequency", Frequency],
                                            # LEFT FOOT
                                            ["LeftStepHeight", 0.02],
                                            ["LeftTorsoWy", 5.0*almath.TO_RAD],
                                            # RIGHT FOOT
                                            ["RightStepHeight", 0.005],
                                            ["RightMaxStepX", 0.001],
                                            ["RightMaxStepFrequency", 0.0],
                                            ["RightTorsoWx", -7.0*almath.TO_RAD],
                                            ["RightTorsoWy", 5.0*almath.TO_RAD]] )
    except Exception, errorMsg:
        print str(errorMsg)
        print "This example is not allowed on this robot."
        exit()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
class BotFreezerModule(ALModule):
    """ A simple module able to react
    to head tactile sensor events

    """
    def __init__(self, name):
        ALModule.__init__(self, name)
        # No need for IP and port here because
        # we have our Python broker connected to NAOqi broker

        # Create a proxy to ALMotion for later use
        self.tts = ALProxy("ALMotion")

        # Subscribe to the MiddleTactilTouched event:
        global memory
        memory = ALProxy("ALMemory")
        memory.subscribeToEvent("FrontTactilTouched",
            "BotFreezer",
            "onTactilTouched")
        memory.subscribeToEvent("MiddleTactilTouched",
            "BotFreezer",
            "onTactilTouched")
        memory.subscribeToEvent("RearTactilTouched",
            "BotFreezer",
            "onTactilTouched")

    def onTactilTouched(self, *_args):
        """ This will be called each time any head tactil is
        detected.

        """
        # Unsubscribe to the event
        # to avoid repetitions
        memory.unsubscribeToEvent("FrontTactilTouched",
            "BotFreezer")
        memory.unsubscribeToEvent("MiddleTactilTouched",
            "BotFreezer")
        memory.unsubscribeToEvent("RearTactilTouched",
            "BotFreezer")
        print "Stopping walk"
        self.tts.stopMove();
        self.tts.rest();

        try:
            sys.exit(0)
        except SystemExit:
            os._exit(0)

        # Subscribe again to the event
        memory.subscribeToEvent("FrontTactilTouched",
            "BotFreezer",
            "onTactilTouched")
        memory.subscribeToEvent("MiddleTactilTouched",
            "BotFreezer",
            "onTactilTouched")
        memory.subscribeToEvent("RearTactilTouched",
            "BotFreezer",
            "onTactilTouched")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
def main(robotIP, port=9559):
    sonar = Sonar(robotIP, port)
    controler = Controller()
    motionProxy = ALProxy("ALMotion", robotIP, port)
    postureProxy = ALProxy("ALRobotPosture", robotIP, port)
    frame = motion.FRAME_ROBOT # maybe test TORSO?
    last_theta = 0
    memory_factor = 0.3

    # Get ready, NAO!
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)

    start_t = time.time()

    # Start walking (sonar appears to start reading correct values only after this)
    motionProxy.moveToward(0.01, 0, 0)
    time.sleep(0.5)

    # Main loop
    while (time.time() - start_t <= 180):
        # Sensor reading step
        x_velocity = 1
        y_velocity = 0
        theta_correction = 0

        lread, rread = sonar.getSampleReading(n_readings=10)

        # Course correction decision step (theta is positive counterclockwise)
        print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
        theta_correction, v_correction = controler.compute(lread=lread, rread=rread, last_theta=last_theta)
        print("Theta %.2f; Velocity %.2f" % (theta_correction, v_correction))

        # Course correction execution step
        #motionProxy.moveToward(v_correction, y_velocity, theta_correction)
        motionProxy.moveToward(v_correction, y_velocity, (memory_factor * last_theta + (1 - memory_factor) * theta_correction))

        last_theta = theta_correction

        time.sleep(0.5)

    # Stop motion
    motionProxy.moveToward(0, 0, 0) # I found this to be less motion breaking
    motionProxy.stopMove()
    postureProxy.goToPosture("StandInit", 0.2) # make him stand again

    # Just to indicate we're finished
    motionProxy.rest()

    print("NAOvigation script finished.")

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
def main(robotIP, PORT=9559):
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    tts.post.say("stop")

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    motionProxy.killAll()

    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    print ('\n=======posture=====before=====')
    print postureProxy.getPosture()
    postureProxy.stopMove()
    print ('=======after=====')
    print postureProxy.getPosture()

    audioDeviceProxy = ALProxy("ALAudioDevice", robotIP, PORT)
    print 'muted:', audioDeviceProxy.isAudioOutMuted()
    audioDeviceProxy.flushAudioOutputs()
    print 'muted:', audioDeviceProxy.isAudioOutMuted()

    audioPlayerProxy = ALProxy("ALAudioPlayer", robotIP, PORT)
    print ('\n=======audio=====before=====')
    allItems = audioPlayerProxy.getLoadedSoundSetsList()
    for i in allItems:
        print i
    # audioPlayerProxy.unloadAllFiles()
    # print ('=======afterUnload=====')
    # allItems = audioPlayerProxy.getLoadedSoundSetsList()
    # for i in allItems:
    #     print i
    audioPlayerProxy.stopAll()
    print ('=======afterStop=====')
    allItems = audioPlayerProxy.getLoadedSoundSetsList()
    for i in allItems:
        print i

    alBehaviorManagerProxy = ALProxy("ALBehaviorManager", robotIP, PORT)
    print ('\n=======behavior=====before=====')
    allItems = alBehaviorManagerProxy.getRunningBehaviors()
    for i in allItems:
        print i
    alBehaviorManagerProxy.stopBehavior('gangnam-style')
    alBehaviorManagerProxy.stopBehavior('taichi-dance-free')
    alBehaviorManagerProxy.stopBehavior('caravan-palace')
    print ('=======after=====')
    allItems = alBehaviorManagerProxy.getRunningBehaviors()
    for i in allItems:
        print i

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
def main(robotIP, port=9559):
    sonar = Sonar(robotIP, port)
    controler = Controller()
    motionProxy = ALProxy("ALMotion", robotIP, port)
    postureProxy = ALProxy("ALRobotPosture", robotIP, port)
    frame = motion.FRAME_ROBOT # maybe test TORSO?

    # Get ready, NAO!
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)

    start_t = time.time()

    # Start walking (sonar appears to start reading correct values only after this)
    motionProxy.moveToward(0.01, 0, 0)
    time.sleep(0.5)

    # Main loop
    while (time.time() - start_t <= 180):
        # make course corrections
        # async walk
        # little sleep

        # Sensor reading step
        x_velocity = 1
        y_velocity = 0
        theta_correction = 0

        lread, rread = sonar.getSampleReading(n_readings=10)

        # Course correction decision step (theta is positive counterclockwise)
        print("Sonar readings (lread, rread) = (%.2f, %.2f)" % (lread, rread))
        theta_correction = controler.compute(lread=lread, rread=rread)
        print("Theta %.2f" % theta_correction)

        # Course correction execution step
        motionProxy.moveToward(x_velocity, y_velocity, theta_correction)

        time.sleep(0.5)

    # Stop motion
    motionProxy.stopMove()

    # Just to indicate we're finished
    motionProxy.rest()

    print("NAOvigation script finished.")

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

    # Initialization of the needed NAO proxies
    def __init__(self, parameters):

        self.nao_ip = parameters["nao_ip"]
        self.nao_port = int(parameters["nao_port"])

        self.motion = ALProxy("ALMotion", self.nao_ip, self.nao_port)

    # Assistive function to return errors
    def ret_exc(self, text):
        print text
        return {'error': text}

    # Enables the NAO motors. The robot cannot move if its motors are not
    # enabled (a.k.a. stifness on).
    def enableMotors(self):
        try:
          self.motion.stiffnessInterpolation('Body', 1.0, 0.5)
        except Exception as e:
            return self.ret_exc("motion.enableMotors: Unrecognized exception: " + \
                e.message)

        return {'error': None}
  
    # Disables motors (a.k.a. stifness off)
    def disableMotors(self):
        try:
          self.motion.stiffnessInterpolation('Body', 0.0, 0.5)
        except Exception as e:
            return self.ret_exc("motion.disableMotors: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Moves the NAO robot by velocities (all inputs are bounded to [-1,+1], where
    # +1 is the maximum positive speed and -1 is the maximum negative speed
    def moveByVelocity(self, x_vel, y_vel, theta_vel):

        if x_vel < -1 or x_vel > 1:
            return self.ret_exc('motion.moveByVelocity: x_vel out of bounds')
        if y_vel < -1 or y_vel > 1:
            return self.ret_exc('motion.moveByVelocity: y_vel out of bounds')
        if theta_vel < -1 or theta_vel > 1:
            return self.ret_exc('motion.moveByVelocity: theta_vel out of bounds')

        try:
            self.motion.moveToward(x_vel, y_vel, theta_vel)
        except Exception as e:
            return self.ret_exc("motion.moveByVelocity: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Moves the NAO robot to the specified pose. x & y are meters, theta is
    # in radians
    def moveTo(self, x, y, theta):
    
        if type(x) not in [float, int]:
            return self.ret_exc('motion.moveTo: x not a number')
        if type(y) not in [float, int]:
            return self.ret_exc('motion.moveTo: y not a number')
        if type(theta) not in [float, int]:
            return self.ret_exc('motion.moveTo: theta not a number')

        try:
          self.motion.moveTo(x, y, theta)
        except Exception as e:
            return self.ret_exc("motion.moveTo: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Stops the NAO motion. May be a bit harsh. It is suggested to use the
    # moveByVelocity(0,0,0) instead.
    def stop(self):
        try:
            self.motion.stopMove()
        except Exception as e:
            return self.ret_exc("motion.stop: Unrecognized exception: " + \
                e.message)

        return {'error': None}

    # Returns the NAO velocities in the three axes (x,y,theta)
    def getVelocities(self):
        try:
            data = self.motion.getRobotVelocity()
            return {'velocities': data, 'error': None}
        except Exception as e:
            return self.ret_exc("motion.getVelocities: Unrecognized exception: " + \
                e.message)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
class NaoMotion():
    def __init__(self, host_name, port):
        self.motionProxy = ALProxy("ALMotion", host_name, port)
        self.postureProxy = ALProxy("ALRobotPosture", host_name, port)
        self.ttsProxy = ALProxy("ALTextToSpeech", host_name, port)

        # self.disable_smart_stiffness()

        # Head Motion
        self.head_joints = ["HeadPitch", "HeadYaw"]
        self.head_angles = []

        # Hand Motion
        self.hand_joints = ["LShoulderRoll", "LShoulderPitch", "LElbowRoll", "LElbowYaw",
                            "RShoulderRoll",
                            "RShoulderPitch",
                            "RElbowRoll",
                            "RElbowYaw"]
        self.hand_angles = []
        self.hand_speed = 0.2

        # Body Motion
        self.head_angles = []
        self.forward_direction = 0
        self.sideward_diretion = 0
        self.rotation = 0
        self.step_frequency = 1.0
        self.walk_duration = 5
        self.turn_duration = 4

        # Is the robot sleeping?
        self.wake_up()

    # If robot is sleeping, wake it up
    def wake_up(self):
        if self.motionProxy.robotIsWakeUp() is False:
            self.motionProxy.wakeUp()
            # time.sleep(3)
            # self.postureProxy.goToPosture("Stand", 0.5)
            time.sleep(3)
            self.set_head()
        else:
            self.set_head()

    # Set the head to look at the user
    def set_head(self):
        self.head_angles = [-0.3145120143890381, -0.013848066329956055]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)
        time.sleep(2)

    # Reset the head
    def reset_head(self):
        self.head_angles = [0.06438612937927246, -4.1961669921875e-05]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)
        time.sleep(2)

    # Reset the hand
    def reset_hand(self):
        self.hand_angles = [0.2638061046600342, 1.4097040891647339, -1.0123980045318604,
                            -1.3806419372558594, -0.2623560428619385, 1.418992042541504,
                            1.0170841217041016, 1.3774900436401367]
        self.motionProxy.setAngles(self.hand_joints, self.hand_angles, self.hand_speed)

    # Wake up using click gesture
    def click_wake_up(self):
        if self.motionProxy.robotIsWakeUp() is False:
            self.motionProxy.wakeUp()
            self.set_head()
        else:
            self.set_head()

    # Set the robot to sleep
    def sleep_robot(self):
        self.motionProxy.rest()

    def get_smart_stiffness(self):
        print self.motionProxy.getSmartStiffnessEnabled()

    def disable_smart_stiffness(self):
        self.motionProxy.setSmartStiffnessEnabled(False)

    # Motion based on given Gesture
    def gesture_to_motion(self, sign):
        if sign == "Walk":
            self.reset_head()
            self.forward_direction = 1.0
            self.sideward_diretion = 0.0
            self.rotation = 0.0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion,
                                        self.rotation,
                                        [["Frequency", self.step_frequency]])

            time.sleep(self.walk_duration)
            self.motionProxy.stopMove()
            time.sleep(2)

            self.set_head()

        elif sign == "Turn Right":
            self.reset_head()
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
				#moveheadChange()
			elif (keyboard == "41"):#r,l,u,d
				#newpos="41"
				moveheadGoLeft()
			elif (keyboard == "42"):#r,l,u,d
				#newpos="42"
				moveheadGoRight()
			elif (keyboard == "43"):#r,l,u,d
				#newpos="43"
				moveheadGoUp()
			elif (keyboard == "44"):#r,l,u,d
				#newpos="44"
				moveheadGoDown()
			elif (keyboard == "50"):
				print"stop"
				postureProxy.stopMove()
			elif (keyboard == "51"):
				print "rest"
				motionProxy.rest()
			elif (keyboard == "52"):
				print "::SUMMARY::"
				print motionProxy.getSummary()
			elif (keyboard == "60"):
				getNaoNetworkInfo()
			elif (keyboard == '70'):
				naoTalks()
			elif(keyboard == '80'):
				menu_RArmControl()
			elif (remoteInput[2] == 'KEY_POWER' or keyboard=="9"):
				break
			#								#

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
# sets Motor on and, if needed, goes to initial position. 唤醒电机，必要时进入初始姿态。
# 对于H25 NAO Robot, 将设置stiffness为开启状态，并恢复起始站立状态;
# 执行了wakeUp(), 则无需执行motion.setStiffnesses("Body", 1.0)
motion.wakeUp()

# void ALMotionProxy::moveInit()
# Initializes the move process. Checks the robot pose and takes a right posture. This is blocking called.
# 初始化移动进程，检查机器人的姿势并切换为正确的姿势。调用此程序前需要打开电机(即wakeUp)，否则调用无效。
motion.moveInit()

# void ALMotionProxy::moveTo(const float& x, const float& y, const float& theta)
# Makes the robot move to the given pose in the ground plane, relative to FRAME_ROBOT. This is a blocking call.
# Parameters:	
#	x - Distance along the X axis in meters.
#	y - Distance along the Y axis in meters.
#	theta - Rotation around the Z axis in radians [-3.1415 to 3.1415].
# If moveTo() method does nothing on the robot, read the section about walk protection.
	
#移动位置是相对与FRAME_ROBOT的，因此如果机器人走到对应位置，下次调用相同moveTo()将无效果
#motion.moveTo(0.5, 0, 0) #阻塞调用，不并行
motion.post.moveTo(0.2, 0, 0) # 每个ALProxy代理都有post属性，通过它可以将程序变为后台运行，从而实现并行.	
tts.say("I am walking now")	  # 这里移动的同时，还要说话;
time.sleep(5)				  # 延时，使得机器人继续运动一段时间
motion.stopMove()	

# ----------> robot rest <----------
# 关闭电机，进入非僵硬模式，此后程序控制无效(因电机已关闭), 但可手工改变电机位置
# 进入复位状态(蹲伏动作)
tts.say("I am going to sleep")
motion.rest()

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

    def __init__(self, name):
        ALModule.__init__(self, name)
        self.name = name
        self.memory = ALProxy("ALMemory")
        self.tts = ALProxy("ALTextToSpeech")
        self.faceDetection = ALProxy("ALFaceDetection")
        self.motion = ALProxy("ALMotion")
        self.previousFaceEvent = None
        self.faceNumber = 0
        self.framing = "medium" # value is in ["medium", "closeup", "headshot"]
       # self.framingSizes = {"medium": 0.15, "closeup": 0.3, "headshot": 0.6}
        self.framingSizes = {"medium": 0.15, "headshot": 0.6}
        self.framingSizeRange = 0.05
        self.onPictureFramed = None

    def start(self):
        #self.frame(1, "closeup")
        self.frame(2, "medium")

    def stop(self):
        self.tts.say("You terminated me!")
        self.motion.stopMove()
        self.faceDetection.unsubscribe(self.name)
        self.memory.unsubscribeToEvent("FaceDetected", self.name)

    def frame(self, faceNumber=1, framing="medium", onPictureFramed=None):
         #self.faceDetection.enableRecognition(False) # makes naoqi crash
#        reactions = ["Let's take a great shot!"]
#        self.tts.say(random.choice(reactions))
        if framing == "headshot":
            prompts = ["Please take me in your arms"]
            self.tts.say(random.choice(prompts))

        self.faceNumber = faceNumber
        self.framing = framing
        self.onPictureFramed = onPictureFramed
        refreshPeriod = 200 # ms
        self.faceDetection.subscribe(self.name, refreshPeriod, 0.0)
        self.memory.subscribeToEvent("FaceDetected", self.name, "onFaceDetected")

    @staticmethod
    def median(values):
        median = sorted(values)[len(values)/2]
        return median

    @staticmethod
    def average(values):
        if len(values) != 0:
            return sum(values) / len(values)
        else:
            return 0

    @staticmethod
    def distanceFromVisualAngle(visualAngle, size):
        return size / (2.0 * math.tan(visualAngle / 2.0))

    @staticmethod
    def sizeFromVisualAngle(visualAngle, distance):
        return 2.0 * distance * math.tan(visualAngle / 2.0)

    @staticmethod
    def sideSize(side1Size, side2Size, oppositeAngle):
        return math.sqrt(side1Size*side1Size + side2Size*side2Size - 2*side1Size*side2Size*math.cos(oppositeAngle))

    @staticmethod
    def oppositeAngle(oppositeSideSize, side1Size, side2Size):
        return math.acos((side1Size*side1Size + side2Size*side2Size - oppositeSideSize*oppositeSideSize) / (2.0 * side1Size * side2Size))

    def onFaceDetected(self, eventName, value, subscriberIdentifier):
        if len(value) == 0:
            # there were faces and now there are none
            return

        event = FaceDetectedEvent(value[0], value[1][:-1], value[4])
        print "onFaceDetected:", "%d faces on camera %d (%d)" % (len(event.faces), event.cameraId, event.timestamp[0])
        shapeInfos = [ FaceShapeInfo(shapeInfoRaw) for shapeInfoRaw in event.faces ]
        sizes = [ shapeInfo.sizeX for shapeInfo in shapeInfos ]
        averageSize = PictureFramerModule.average(sizes)

        #standardFaceHeight = 0.15
        #standardEyeDistance = 0.045
        #standardMouthWidth = 0.045
        #angle = 0.51 # to compute from face size proportions
        #distance = PictureFramerModule.distanceFromVisualAngle(shapeInfos[0].sizeX, standardFaceHeight)
        #eyeAngle = shapeInfos[0].leftEyeX - shapeInfos[0].rightEyeX
        #eyeDistance = PictureFramerModule.sizeFromVisualAngle(eyeAngle, distance)
        #mouthAngle = shapeInfos[0].leftMouthX - shapeInfos[0].rightMouthX
        #mouthWidth = PictureFramerModule.sizeFromVisualAngle(mouthAngle, distance)
        #angle = math.acos(eyeDistance / standardEyeDistance)
        #targetAngle = self.framingSizes[self.framing]
        #targetDistance = PictureFramerModule.distanceFromVisualAngle(targetAngle, standardFaceHeight)
        #walkingDistance = PictureFramerModule.sideSize(distance, targetDistance, angle)
        #walkingAngle = PictureFramerModule.oppositeAngle(targetDistance, walkingDistance, distance)

        if self.framing == "headshot":
            if self.isTooClose(averageSize):
                self.tts.say("I'm a bit too close")
            elif self.isTooFar(averageSize):
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
def main(robot_IP, robot_PORT=9559):
	
	global ROBOT_IP
	global ROBOT_PORT
	ROBOT_IP = robot_IP
	ROBOT_PORT = robot_PORT

	# ----------> Connect to robot <----------
	tts = ALProxy("ALTextToSpeech", robot_IP, robot_PORT)
	motion = ALProxy("ALMotion", robot_IP, robot_PORT)
	posture = ALProxy("ALRobotPosture", robot_IP, robot_PORT)
	global memory
	memory = ALProxy("ALMemory", robot_IP, robot_PORT)
	leds = ALProxy("ALLeds", robot_IP, robot_PORT)
	global battery
	battery = ALProxy("ALBattery", ROBOT_IP, ROBOT_PORT)
	autonomous = ALProxy("ALAutonomousLife", robot_IP, robot_PORT)
	autonomous.setState("disabled") # turn ALAutonomousLife off

	# ----------> 开启socket服务器监听端口 <----------
	sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	sock.bind((robot_IP, LISTEN_PORT))
	sock.listen(10)

	try:
		while True:	# 等待客户端连接，单线程监听单一客户端
			global connection
			connection,address = sock.accept()
			CONNECT = True
			while CONNECT == True:
				try:
					#connection.settimeout(10)
					# 服务器接受指令
					buf = connection.recv(1024)
					print "get:[", buf, "]"
					# 根据接受的命令执行不同操作
					if buf == COMMAND_WAKEUP:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
						connection.send("Robot Motion: [ Wakeup ]\r")
					elif buf == COMMAND_REST:
						if motion.robotIsWakeUp() == True: 
							motion.post.rest()
						connection.send("Robot Motion: [ Rest ]\r")
					elif buf == COMMAND_FORWARD:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						#motion.post.moveTo(0.3, 0, 0)
						motion.move(0.1,0,0) # 固定速率持续行走
						connection.send("Robot Motion: [ Forward ]\r")
					elif buf == COMMAND_BACK:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						#motion.post.moveTo(-0.1, 0, 0)
						motion.move(-0.1,0,0)
						connection.send("Robot Motion: [ Back ]\r")
					elif buf == COMMAND_LEFT:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						#motion.post.moveTo(0, 0.1, 0)
						motion.move(0,0.1,0)
						connection.send("Robot Motion: [ Left ]\r")
					elif buf == COMMAND_RIGHT:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						#motion.post.moveTo(0, -0.1, 0)
						motion.move(0,-0.1,0)
						connection.send("Robot Motion: [ Right ]\r")
					elif buf == COMMAND_STOP:
						motion.stopMove()
						connection.send("Robot Motion: [ stop move ]\r")
					elif buf == COMMAND_TURNRIGHT:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						motion.move(0, 0, -0.3)
						connection.send("Robot Motion: [ turn right ]\r")
					elif buf == COMMAND_TURNLEFT:
						if motion.robotIsWakeUp() == False: 
							motion.post.wakeUp()
							connection.send("Robot Motion: [ Wakeup ]\r")
							motion.post.moveInit()
							connection.send("Robot Motion: [ MoveInit ]\r")
						motion.move(0, 0, 0.3)
						connection.send("Robot Motion: [ turn left ]\r")
					elif buf == COMMAND_DISCONNECT:
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
    def run(self):
        nNaoMove=ALProxy("ALMotion", self.IP, self.PORT)
        postureProxy = ALProxy("ALRobotPosture", self.IP, self.PORT)
        nNaoMove.wakeUp()
        postureProxy.goToPosture("Stand", 0.5)
        pygame.init()
        screen = pygame.display.set_mode((454, 603), 0, 32)
        background = pygame.image.load(r"F:\NaoTest\NaoSource\mythreads\naonew.png").convert_alpha()
        pygame.display.set_caption("Dragon's dance")
        screen.blit(background, (0, 0))
        pygame.display.update()
        while True:
            for event in pygame.event.get():
                if event.type == QUIT:
                    exit()
                elif event.type == KEYDOWN:
                    if event.key == K_UP:
                        x = 0.7
                        y = 0.0
                        theta = 0.0
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])

                    elif event.key == K_DOWN:
                        x = -0.4
                        y = 0.0
                        theta = 0.0
                        frequency = 0.2
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_LEFT:
                        x = 0.0
                        y = 0.2
                        theta = 0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_RIGHT:
                        x = 0.0
                        y = -0.2
                        theta = -0.6
                        frequency = 0.4
                        nNaoMove.moveToward(x, y, theta, [["Frequency", frequency]])
                    elif event.key == K_q:
                        nNaoMove.stopMove()
                        time.sleep(1)
                        nNaoMove.rest()
                    elif event.key == K_r:
                        nNaoMove.wakeUp()
                        postureProxy.goToPosture("StandInit", 0.5)
                        time.sleep(1.5)
                    elif event.key == K_s:
                        nNaoMove.stopMove()

                    elif event.key == K_z:
                        # 头部左转
                        nNaoMove.setStiffnesses("Head", 1.0)
                        names = "HeadYaw"
                        changes = 0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_c:
                        # 头部右转
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        changes = -0.5
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_a:
                        # 向上看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = 0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_d:
                        # 向下看
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadPitch"
                        changes = -0.2
                        fractionMaxSpeed = 0.1
                        nNaoMove.changeAngles(names, changes, fractionMaxSpeed)
                    elif event.key == K_x:
                        # 头部恢复
                        nNaoMove.setStiffnesses("Head", 1)
                        names = "HeadYaw"
                        angles = 0.0
                        fractionMaxSpeed = 0.2
                        nNaoMove.setAngles(names, angles, fractionMaxSpeed)
                        # 左手运动
                    elif event.key == K_1:
                        changes = -0.25
                        fractionMaxSpeed = 0.1
                        nNaoMove.setStiffnesses("LShoulderPitch", 1)
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)  # (关节，改变的角度，快慢）
                    elif event.key == K_2:
                        changes = 0.25
                        nNaoMove.changeAngles("LShoulderPitch", changes, 0.1)
                    elif event.key == K_3:
                        changes = 0.2
                        nNaoMove.setStiffnesses("LShoulderRoll", 1)
                        nNaoMove.changeAngles("LShoulderRoll", changes, 0.1)
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
            raise ValueError('Invalid interface: %s' % str(interface))

        super(RoboticAgent, self).__init__()

        self.identity = identity
        self.interface = interface
        self.random_state = random_state or np.random.RandomState()

        self.motion = ALProxy("ALMotion", *interface)
        self.posture = ALProxy("ALRobotPosture", *interface)

        self.adapter = NAOAdapter(link, 'NAO') if link else None
        self.sensors = dict()

        self.joint_manager_ = None
        self.perception_ = None
        self.behavior_ = self.BEHAVIORS.disabled

    def start(self, link=None):
        if link is not None:
            self.adapter = NAOAdapter(link, 'NAO')

        link = self.adapter.link

        self.motion.wakeUp()
        self.posture.goToPosture('Stand', self.SPEED)

        self.joint_manager_ = utils.JointManager(link=link,
                                                 motion=self.motion,
                                                 identity=self.identity,
                                                 robot_adapter=self.adapter)
        self.joint_manager_.start()

        self.sensors = {
            'vision': [
                components.Camera(link, component='NAO_vision1'),
                components.Camera(link, component='NAO_vision2'),
            ],
            'proximity': [
                # front
                components.ProximitySensor(link, component='Proximity_sensor1'),
                # back
                components.ProximitySensor(link, component='Proximity_sensor4'),
                # right
                components.ProximitySensor(link, component='Proximity_sensor3'),
                # left
                components.ProximitySensor(link, component='Proximity_sensor2')
            ],
            'position': [
                components.Tag(link, component='tag1'),
                components.Tag(link, component='tag2'),
                components.Tag(link, component='tag3')
            ],
            'orientation': [
                components.Compass(link, component=self.adapter)
            ]
        }

        # Sleep for one second to guarantee sensors are ready.
        time.sleep(1)

        self.behavior_ = self.BEHAVIORS.idle

    def perceive(self):
        for tag, sensors in self.sensors.items():
            for s in sensors:
                s.read()
        return self

    def act(self):
        """Find a method with the same name of its current state and execute
        it.
        """
        return getattr(self, str(self.behavior_))()

    def stuck(self):
        """Reset agent to the starting point."""
        logger.info('agent is stuck. Restarting...')
        self.dispose().start(self.adapter.link)

    def dead(self):
        """Doesn't do anything."""
        logger.warning('attempt to update a dead agent')

    def dispose(self):
        self.motion.stopMove()
        self.posture.goToPosture('Stand', self.SPEED)

        if self.joint_manager_:
            # Stop sync routine.
            self.joint_manager_.dispose().join()

        if self.adapter:
            # Clear adapter.
            self.adapter.dispose()

        self.behavior_ = self.BEHAVIORS.dead
        self.cycle_ = 0

        return self

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

#.........这里部分代码省略.........
            "Face/Led/Red/Left/0Deg/Actuator/Value",
            "Face/Led/Red/Left/90Deg/Actuator/Value",
            "Face/Led/Red/Left/180Deg/Actuator/Value",
            "Face/Led/Red/Left/270Deg/Actuator/Value",
            "Face/Led/Red/Right/0Deg/Actuator/Value",
            "Face/Led/Red/Right/90Deg/Actuator/Value",
            "Face/Led/Red/Right/180Deg/Actuator/Value",
            "Face/Led/Red/Right/270Deg/Actuator/Value"]
            self.__Leds.createGroup("MyGroup",names)
            
    def set_crouch_on_stop(self, crouch=True):
        self.__stop_crouch = crouch

    def move(self, Joint, Angle, Speed):
        self.__Motion.setAngles(Joint, Angle, Speed)

    def walk(self, X=0, Y=0, Theta=0):
        self.__Motion.walkTo(X, Y, Theta)
        
    def moveToward(self, X=0, Y=0, Theta=0):
        self.__Motion.moveToward(X, Y, Theta)

    def walkNav(self, X=0, Y=0, Theta=0, distance = 0.4):
        self.__Navigation.setSecurityDistance(distance)
        self.__Navigation.moveTo(X, Y, Theta)

    def isWalking(self):
        return self.__Motion.walkIsActive()    
        
    def isMoving(self):
        return self.__Motion.moveIsActive()
        
    def stopwalk(self):
        self.__Motion.stopMove()
        
    def setWalkTargetVelocity(self, x , y, theta, frequency):
        self.__Motion.setWalkTargetVelocity(x , y, theta, frequency)

    def is_speaking(self):
        if self.simulation:
            return False
        else:
            is_done_speaking = self.__Memory.getData("ALTextToSpeech/TextDone")
            if not is_done_speaking == None:
                is_done_speaking = int(is_done_speaking)
                if is_done_speaking == 0:
                    return True
            return False

    def say(self, Text, filename=None):
        if self.__TTS:
            #self.__TTS.say(str(Text))
            if filename:
                self.__TTS.sayToFile(Text, filename)
            else:
                self.__TTS.post.say(Text)
        else:
            print "[Nao says] " + Text


    def get_sonar_distance(self):
#        sonarValue= "SonarLeftDetected"
        if self.__Sonar:
#            data = self.__Sonar.getOutputNames()
            data = {"left": self.__Memory.getData("SonarLeftDetected",0), "right" : self.__Memory.getData("SonarRightDetected",0)}
            return data

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import stopMove [as 别名]
 def __motion_background_task(self):
     global stopProgramFlag
     motion = ALProxy("ALMotion", Settings.naoHostName, Settings.naoPort)
     while self.__motionActive:
         if stopProgramFlag:
             motion.stopMove()