Python ALProxy.robotIsWakeUp方法代码示例

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

    myBroker = ALBroker("myBroker", "0.0.0.0", 0, robotIP, port)

    #设置机器人各个模块代理
    MEMORY = ALProxy("ALMemory", robotIP, port)
    REDBALL = ALProxy("ALRedBallDetection", robotIP, port)
    LANDMARK = ALProxy("ALLandMarkDetection", robotIP, port)
    TTS = ALProxy("ALTextToSpeech", robotIP, port)
    MOTION = ALProxy("ALMotion", robotIP, port)
    POSTURE = ALProxy("ALRobotPosture", robotIP, port)
    CAMERA = ALProxy("ALVideoDevice", robotIP, port)
    TOUCH = ALProxy("ALTouch", robotIP, port)
    SENSORS = ALProxy("ALSensors", robotIP, port)

    #实例化
    global myGolf
    myGolf = Golf("myGolf", robotIP, port)
    MOTION.setFallManagerEnabled(True)
    redBallDetectMethod = 1

    try:
        #-----------------------------------------------------主死循环，待机状态-----------------------------------------------------#
        while(True):
            if(MOTION.robotIsWakeUp() == False):
                MOTION.wakeUp()
                POSTURE.goToPosture("StandInit", 0.5)
                TTS.say("游戏开始")
                releaseStick(robotIP, port)
                time.sleep(5)
                catchStick(robotIP, port)
                time.sleep(2)
                raiseStick(robotIP, port)
                standWithStick(0.1, robotIP, port)
                hitBall(0.15, robotIP, port)
                time.sleep(1)
                catchStick(robotIP, port)
                time.sleep(10)
                raiseStick(robotIP, port)
                standWithStick(0.1, robotIP, port)
            time.sleep(1)
            print "holeFlag =",holeFlag
            if(holeFlag != 0):
                #---------------------------------------------次死循环，工作状态------------------------------------------------------#
                while(True):
                    MOTION.setMoveArmsEnabled(False, False)

                    if(codeTest == False):
                        #---------------------高尔夫击球第一个阶段，直打----------------------------------------------#
                        if(holeFlag == 1):
                            hitBall(HitBallSpeed_FirstHole_Num_1, robotIP, port)
                        elif(holeFlag == 2):
                            hitBall(HitBallSpeed_SecondHole_Num_1, robotIP, port)
                        elif(holeFlag == 3):
                            hitBall(HitBallSpeed_ThirdHole_Num_1, robotIP, port)

                        time.sleep(0.5)
                        raiseStick(robotIP, port)
                        standWithStick(0.1, robotIP, port)
                        MOTION.moveTo(0, 0, -math.pi/2+10*math.pi/180.0, moveConfig)
                        time.sleep(0.5)

                        if(holeFlag == 1):
                            MOTION.moveTo(1.00, -0.15, -45*math.pi/180.0, moveConfig)
                            redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, True, redBallDetectMethod, 0, robotIP, port)
                        if(holeFlag == 2):
                            MOTION.moveTo(0, -1.0, 0, moveConfig)
                            time.sleep(1)
                            MOTION.moveTo(0.60, MoveDirectYOffSet, MoveDirectAngleOffSet, moveConfig)
                            redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, True, redBallDetectMethod, 0, robotIP, port)
                        if(holeFlag == 3):
                            MOTION.moveTo(0.0, 0.0,-50*math.pi/180.0, moveConfig)
                            time.sleep(0.5)
                            MOTION.moveTo(0, 1.50, 0, moveConfig)
                            redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, False, redBallDetectMethod, 0, robotIP, port)

                        #----------------------高尔夫击球第二个阶段，靠近红球--------------------------------------------------#

                        #检测不到就往前走60cm以防万一
                        if(redBallInfo == False):
                            MOTION.moveTo(0.60, MoveDirectYOffSet, MoveDirectAngleOffSet,moveConfig)
                            time.sleep(0.5)
                            redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, True, redBallDetectMethod, 0, robotIP, port)
                        while(redBallInfo[0] >=0.60):
                            if(redBallInfo[0]*math.cos(redBallInfo[1]) - 0.60 <=0.30):
                                break
                            MOTION.moveTo(0,0,redBallInfo[1],moveConfig)
                            time.sleep(0.5)
                            MOTION.moveTo(0.60, MoveDirectYOffSet, MoveDirectAngleOffSet,moveConfig)
                            time.sleep(0.5)
                            if(redBallInfo[0] - 0.60 < 0.60):
                                redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, False, redBallDetectMethod, 0, robotIP, port)
                                break
                            if(redBallInfo[0] - 0.60 >= 1.2):
                                redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, True, redBallDetectMethod, 0, robotIP, port)
                            else:
                                redBallInfo = redBallDetection(red_thresholdMin, red_thresholdMax, False, redBallDetectMethod, 0, robotIP, port)

                        #移动到机器人30cm前
                        MOTION.moveTo(0,0,redBallInfo[1],moveConfig)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import robotIsWakeUp [as 别名]
#motion.setStiffnesses("Body", 0.0) # 非僵硬状态，此时可任意改变机器人的姿态，程序控制无效。

#motion.stiffnessInterpolation("Body", 0.0, 1.0) #阻塞一段时候(参数3)后调节某部位(参数1)的状态(参数2)
#与setStiffnesses()相比，先延迟一段时间，然后才设置状态;

#setStiffnesses()为非阻塞函数，立刻关闭/打开电机，这样可能有危险
#因此一般建议使用stiffnessInterpolation(), 并加上一段时间供机器人开关电机。

#stiffnesses = motion.getStiffnesses("Body") #获得关节的stiffness值

# ----------> Wake robot <----------

# Wake up robot
# sets Motor on and, if needed, goes to initial position. 唤醒电机，必要时进入初始姿态。
# 对于H25 NAO Robot, 将设置stiffness为开启状态，并恢复起始站立状态;
# 要执行wakeUp(), 则无需执行motion.setStiffnesses("Body", 1.0)
motion.wakeUp()
time.sleep(1)
print motion.getSummary() #打印机器人当前姿态
print "Robot is WakeUp? ", motion.robotIsWakeUp()

print motion.getStiffnesses("HeadYaw");
motion.setStiffnesses("HeadYaw", 0.0)
time.sleep(1)
print motion.getStiffnesses("HeadYaw");

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

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

#.........这里部分代码省略.........
                elif self.breathEnabled == False:
                    self.idlecount += 1



    def rest(self):
        # stop breathing if its enabled
        self.resting  = True
        if self.breathEnabled == True:
            self.breath(False)

        self.motionProxy.rest()
        time.sleep(1)
        self.motionProxy.stiffnessInterpolation("Body", 0, 0.5)
        self.asr.unsubscribe("ASR")

    def breath(self, boolv):
        # pBpm is a float between 5 and 30 setting the breathing frequency in beats per minute.
        # pAmplitude is a float between 0 and 1 setting the amplitude of the breathing animation.
        # At high frequencies, only low amplitudes are allowed. Input amplitude may be clipped.
        if boolv == True:
            self.checkawake()
            self.breathEnabled = True
            # Fast breathing: 20 Bpm and low Amplitude
            self.motionProxy.setBreathConfig([['Bpm', 5.0], ['Amplitude', 0.3]])
        else:
            self.breathEnabled = False

        self.motionProxy.setBreathEnabled('Legs' , boolv)
        self.motionProxy.setBreathEnabled( 'Arms', boolv)
        self.motionProxy.setBreathEnabled('Head',False)

    def wakeup(self):
        self.postureProxy.goToPosture("StandInit", 0.5)
        self.resting = False

    def checkawake(self):
        self.managerProxy.stopAllBehaviors()
        self.breath(False)
        # if robot is not awake, wake it
        if self.motionProxy.robotIsWakeUp() == False:
            self.wakeup()

    def sayAnimated(self, text):
        self.checkawake()
        # set the local configuration
        configuration = {"bodyLanguageMode": "contextual"}
        # say the text with the local configuration
        self.animatedSpeechProxy.say(text, configuration)

    def texttospeach(self, text):
        self.tts.say(text)

    def launchBehavior(self, behaviorName):

        ''' Launch and stop a behavior, if possible. '''
        if (self.managerProxy.isBehaviorInstalled(behaviorName)):
            # Check that it is not already running.
            if (not self.managerProxy.isBehaviorRunning(behaviorName)):
                # check if robot is awake
                self.checkawake()
                # Launch behavior. This is a blocking call, use post if you do not
                # want to wait for the behavior to finish.
                rospy.loginfo ("Running Behavior"+ behaviorName)
                self.headlock = True
                headodom =  self.motionProxy.getAngles(["HeadYaw", "HeadPitch"], True)
                Id = self.managerProxy.post.runBehavior(behaviorName)
                self.headlock = False
                # wait till behavior stops running
                self.managerProxy.wait(Id, 0)

                #return head back to original
                self.headmove(headodom,0.1)
            else:
                rospy.loginfo("Behavior is already running.")
        else:
            rospy.loginfo("Behavior not found.")

    def getBehaviors(self):
        names = self.managerProxy.getInstalledBehaviors()
        for x in names:
            rospy.loginfo(x)

    def on_shutdown(self):
        self.asr.unsubscribe("ASR")
        self.rest()
    def jointstateC(self, data):
        #keeps our odom up to date
        self.headOdom[0] = data.position[0]
        self.headOdom[1] = data.position[1]

    def nod(self):

        prevodom = self.headOdom
        odom = self.headOdom
        odom[1] -= 0.5
        self.headmove(odom,0.08)

        odom[1] += 0.5
        self.headmove(odom,0.08)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import robotIsWakeUp [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 robotIsWakeUp [as 别名]
class NaoMotion():
    def __init__(self, host_name, port):
        self.motionProxy = ALProxy("ALMotion", host_name, port)
        self.postureProxy = ALProxy("ALRobotPosture", host_name, port)

        if self.motionProxy.robotIsWakeUp() is False:
            self.motionProxy.wakeUp()
            self.postureProxy.goToPosture("Stand", 0.5)
            time.sleep(3)
            self.set_head()
        else:
            self.set_head()

        # Parameters for handGesture
        self.joints = ["LShoulderRoll", "LShoulderPitch", "LElbowRoll", "LElbowYaw", "RShoulderRoll", "RShoulderPitch",
                       "RElbowRoll",
                       "RElbowYaw"]
        self.angles = []
        self.fractionMaxSpeed = 0.2

        # Parameters for walk
        self.forward_direction = 0
        self.sideward_diretion = 0
        self.rotation = 0
        self.step_frequency = 0.5

    def set_head(self):
        self.head_joints = ["HeadPitch", "HeadYaw"]
        self.head_angles = [-0.3145120143890381, -0.013848066329956055]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)

    def reset_head(self):
        self.head_joints = ["HeadPitch", "HeadYaw"]
        self.head_angles = [0.06438612937927246, -4.1961669921875e-05]
        self.motionProxy.setAngles(self.head_joints, self.head_angles, 0.1)

    def walk(self, sign):
        if sign == "Walk":
            self.reset_head()
            time.sleep(3)
            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(5)
            self.motionProxy.stopMove()
            time.sleep(2)
            self.set_head()

        elif sign == "Turn Right":
            self.reset_head()
            time.sleep(3)
            self.forward_direction = 0.0
            self.sideward_diretion = 0.0
            self.rotation = 1.0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion, self.rotation,
                                        [["Frequency", self.step_frequency]])
            time.sleep(3)
            self.motionProxy.stopMove()
            time.sleep(2)
            self.set_head()

        elif sign == "Turn Left":
            self.reset_head()
            time.sleep(3)
            self.forward_direction = 0.0
            self.sideward_diretion = 0.0
            self.rotation = -1.0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion, self.rotation,
                                        [["Frequency", self.step_frequency]])
            time.sleep(3)
            self.motionProxy.stopMove()
            time.sleep(2)
            self.set_head()

        elif sign == "Move Right":
            self.reset_head()
            time.sleep(3)
            self.forward_direction = 0.0
            self.sideward_diretion = 0.0
            self.rotation = 1.0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion, self.rotation,
                                        [["Frequency", self.step_frequency]])
            time.sleep(3)
            self.motionProxy.stopMove()
            time.sleep(3)
            self.forward_direction = 1.0
            self.sideward_diretion = 0.0
            self.rotation = 0
            self.motionProxy.moveToward(self.forward_direction, self.sideward_diretion, self.rotation,
                                        [["Frequency", self.step_frequency]])
            time.sleep(5)
            self.motionProxy.stopMove()
            time.sleep(2)
            self.set_head()

        elif sign == "Move Left":
            self.reset_head()
            time.sleep(3)
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
        angles = [yaw, pitch]
        self.motion_proxy.angleInterpolation(joint_names, angles, 1., True)

    def compareToLeftHandPosition(self, coord, must_print=False):
        """
        :param coord: the 6D coordinates to compare with the left hand position
        :type coord: list
        :return: the difference of position from the hand to the given coordinates [x-axis, y-axis]
        :rtype: np.array
        """
        hand_coord = self.getLeftHandPosition()
        if must_print:
            print coord
            print hand_coord
        return geom.vectorize(hand_coord[0:2], coord[0:2])

    def playDisc(self, hole_coordinates):
        """
        :param hole_coordinates: The 6D translation + rotation vector of the hole
        """
        self.stand()
        self.setLeftHandPosition(hole_coordinates, mask=63)
        self.motion_proxy.openHand("LHand")
        self.motion_proxy.closeHand("LHand")
        self.setLeftArmAlongsideBody()

    def setLeftArmRaised(self, secure=False):
        """
        Raise NAO's left arm to the sky
        """
        self.stand()
        if secure:
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
            self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, RAISED, 2., True)

    def setLeftArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, ARM_ALONGSIDE_BODY, 2., True)

    def setRightArmAlongsideBody(self):
        """
        Move the left arm of NAO alongside his body.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(RARM_CHAIN, ARM_ALONGSIDE_BODY_R, 2., True)

    def setLeftArmToAskingPosition(self):
        """
        Move the left arm of NAO in a "asking disc" position, and open his left hand.
        """
        self.stand()
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_BOTTOM, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, INTERMEDIATE_TOP, 2., True)
        self.motion_proxy.angleInterpolation(LARM_CHAIN, ASKING_HAND, 2., True)
        self.motion_proxy.openHand("LHand")

    def lookAtGameBoard(self, dist):
        """
        :param dist: the supposed distance between NAO and the board
        Make NAO look to the hypothetical game board position, located at "dist" meters from the robot
        """
        self.crouch()
        height = 0.165
        b = height
        c = geom.pythagore(height, dist)  # The length of the side betwaeen NAO's head and the board to look at
        a = dist  # The difference between the theoretical position (1m) and the actual position (dist)
        pitch_angle = geom.al_kashi(b, a, c, None)
        self.moveHead(pitch_angle, 0, radians=True)

    def stand(self):
        """
        Make the robot stand up
        """
        if not self.motion_proxy.robotIsWakeUp():
            self.motion_proxy.wakeUp()

    def crouch(self):
        """
        Crouch the robot, but stiff the head
        """
        self.motion_proxy.rest()
        self.motion_proxy.setStiffnesses("HeadPitch", 1.0)

    def stiffHead(self):
        """
        Set the stiffness of the head to 1 (Max stiff)
        """
        self.motion_proxy.setStiffnesses(["HeadPitch", "HeadYaw"], [1, 1])

    def releaseHead(self):
        """
        Release the stiffness of the head
        """
        self.motion_proxy.setStiffnesses(["HeadPitch", "HeadYaw"], [0, 0])

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import robotIsWakeUp [as 别名]
            else:
                if e.type == pygame.locals.JOYBUTTONUP and key_map[e.dict['button']] == 'start':
                    started = False
                    motion.stopMove()
                    print('Control of the motion of the robot disabled')

                if e.type == pygame.locals.JOYBUTTONDOWN:
                    button = e.dict['button']
                    if key_map[button] == 'start':
                        started = True
                        print('Control of the motion of the robot enabled')
                        motion.setStiffnesses("Head", 1.0)
                    elif not started:
                        if key_map[button] == 'exit':
                            print('Shutting down remote control')
                            if motion.robotIsWakeUp():
                                motion.rest()
                            exit()
                    elif key_map[button] == 'wake_up':
                        motion.wakeUp()
                    elif key_map[button] == 'rest':
                        motion.rest()
                    elif key_map[button] == 'enable':
                        print('Remote control disabled')
                        enabled = False
                        started = False
                    elif key_map[button] == 'say':
                        tts.say('Hello! I am NAO robot. I am here to inform you that I suck.')
                    elif key_map[button] == 'none':
                        pass
                    else:

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import robotIsWakeUp [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:
#.........这里部分代码省略.........