Python ALProxy.setMoveArmsEnabled方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
def MoveAction_MoveFor(robotIP, PORT=9559):
    # 前进
    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)


    # Send robot to Stand
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by Motion algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    # motionProxy.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", False]])
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #TARGET VELOCITY
    X = 0.8
    Y = 0.0
    Theta = 0.0
    Frequency =1.0 # max speed
    try:
        motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
    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 setMoveArmsEnabled [as 别名]
def main(robotIP, PORT = 9559, steps=1):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setExternalCollisionProtectionEnabled("All", False)
    motionProxy.moveTo(steps / 4.0, 0, 0) # X, Y, Theta

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

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    #~ motionProxy.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    X = 0.0
    Y = 0.0
    Theta = math.pi/2.0
    motionProxy.post.moveTo(X, Y, Theta)
    motionProxy.setAngles("HeadYaw", 0.6, 0.6)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = m.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
    def __walk_to(self, data):
        #self.__posture("Stand")
        motion = ALProxy("ALMotion", Settings.naoHostName, Settings.naoPort)
        motion.setMoveArmsEnabled( data['arms'], data['arms'] )
        
        self.__motionActive = True
        start_new_thread( self.__motion_background_task, () )

        collisionDetectionEnabled = motion.getExternalCollisionProtectionEnabled("Move")
        motion.setExternalCollisionProtectionEnabled("Move", False)
        motion.moveTo( data['x'], data['y'], radians(data['theta']) )
        motion.setExternalCollisionProtectionEnabled("Move", collisionDetectionEnabled) 
        
        self.__motionActive = False

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

    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    postureProxy.goToPosture("StandInit", 0.5)

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    motionProxy.wakeUp()
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
    motionProxy.post.moveTo(0.2, -0.2, -math.pi / 2.0)
    motionProxy.waitUntilMoveIsFinished()

    print "Robot Move: right"

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

    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setExternalCollisionProtectionEnabled("All", False)
    
    if turn == "left":
        theta = .5
    else:
        theta = -.5

    # X, Y, Theta, Freq
    motionProxy.setWalkTargetVelocity(0, 0, theta, 1)
    time.sleep(3.65)
    motionProxy.setWalkTargetVelocity(0, 0, 0, 0) # stop motion

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

    audiodevice = ALProxy("ALAudioDevice", robotIP, PORT)
    currentVoice = audiodevice.getOutputVolume()
    audiodevice.setOutputVolume(currentVoice + 10)

    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    postureProxy.goToPosture("StandInit", 0.5)

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    # Wake up robot
    motionProxy.wakeUp()
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
    motionProxy.post.moveTo(0.8, 0, 0)
    motionProxy.waitUntilMoveIsFinished()

    print "Robot Move forward"

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
def robotRotate(angle, robotIP, port):
    MOTION = ALProxy("ALMotion", robotIP, port)
    if abs(angle) > math.pi/2:
        if angle >= 0:
            thetaz1 = angle - math.pi/2
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,math.pi/2,moveConfig)
            time.sleep(1)
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,thetaz1,moveConfig)
        else:
            thetaz1 = angle + math.pi/2
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,-math.pi/2,moveConfig)
            time.sleep(1)
            MOTION.setMoveArmsEnabled(False, False)
            MOTION.moveTo(0.0,0.0,thetaz1,moveConfig)
    else:
        MOTION.setMoveArmsEnabled(False, False)
        MOTION.moveTo(0.0,0.0,angle,moveConfig)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
def main(robotIP, PORT=9559):
    # tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    # tts.post.say("我这就退后0.6米.")

    # lower the volume by 10%
    audioDevice = ALProxy("ALAudioDevice", robotIP, PORT)
    currentVoice = audioDevice.getOutputVolume()
    audioDevice.setOutputVolume(currentVoice - 10)

    # stand first
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    postureProxy.goToPosture("StandInit", 0.5)

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    motionProxy.wakeUp()
    motionProxy.setMoveArmsEnabled(True, True)
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
    # action move
    motionProxy.post.moveTo(-0.618, 0, 0)
    motionProxy.waitUntilMoveIsFinished()

    print "Robot Move: back"

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [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 setMoveArmsEnabled [as 别名]
class MotionController:
    # NAO Default head position in radians
    DEFAULT_HEAD_PITCH = -0.05
    DEFAULT_HEAD_YAW = 0.0

    """
    Represents a virtual controller for NAO's motion system
    """
    def __init__(self, robot_ip=None, robot_port=None):
        """
        :param robot_ip: The IP address of the robot
        :type robot_ip: str
        :param robot_port: The port of the robot
        :type robot_port: int
        Creates a new Virtual Controller for NAO
        """
        if robot_ip is None:
            robot_ip = nao.IP
        if robot_port is None:
            robot_port = nao.PORT

        self.ip = robot_ip
        self.port = robot_port

        # Connect and wake up the robot
        self.motion_proxy = ALProxy("ALMotion", robot_ip, robot_port)
        # self.motion_proxy.wakeUp()
        self.motion_proxy.setCollisionProtectionEnabled("Arms", True)
        self.motion_proxy.setMoveArmsEnabled(False, False)
        # self.moveHead(0.114, 0, radians=True)

    def getCameraTopPositionFromTorso(self):
        """
        :return: The 6D coordinates of the top camera of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        return self.motion_proxy.getPosition("CameraTop",
                                             FRAME_TORSO,
                                             True)

    def getCameraBottomPositionFromTorso(self):
        """
        :return: The 6D coordinates of the bottom camera of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        return self.motion_proxy.getPosition("CameraBottom",
                                             FRAME_TORSO,
                                             True)

    def getLeftHandPosition(self):
        """
        :return: The 6D coordinates of the left hand of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        # print self.motion_proxy.getBodyNames("Chains")
        return self.motion_proxy.getPosition("LHand", FRAME_TORSO, True)

    def setLeftHandPosition(self, coord, mask=7, time_limit=3.0):
        """
        :param coord: the coordinates, relative to the robot torso, where the hand will be placed if possible.
                      6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        :type coord: list
        :param mask: 6 bits unsigned integer that represents which axis will be moved / rotated.
                     The sum of the following values will set the mask.
                     1 : x-axis translation
                     2 : y-axis translation
                     4 : z-axis translation
                     8 : x-axis rotation
                     16: y-axis rotation
                     32: z-axis rotation
        :type mask: int
        :param time_limit: The maximum time, in seconds, that the move can take. The shorter the faster.
        :type time_limit: float
        Move the hand to the given coordinates if possible.
        """
        self.stand()
        self.motion_proxy.positionInterpolations("LArm", FRAME_TORSO, [tuple(coord)], mask, [time_limit])

    def moveAt(self, x, y, z_rot):
        """
        :param x: the distance to travel forward (negative value = backward) in meters
        :type x: float
        :param y: the distance to travel to the left (negative value = to the right) in meters
        :type y: float
        :param z_rot: the rotation angle, in radians, around the vertical axis
        :type z_rot: float
        Move the robot to a certain position, defined by the three parameters.
        """
        self.stand()
        self.motion_proxy.wakeUp()
        self.motion_proxy.moveTo(x, y, z_rot)

    def getLeftArmAngles(self):
        """
        :return: the angles in radians of the left arm of NAO in this order :
            [ShoulderPitch, ShoulderRoll, ElbowYaw, ElbowRoll, WristYaw]
        :rtype: list
        """
        joint_names = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw"]
        use_sensors = True
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
def whiteBlockDetection(robotIP="127.0.0.1", port=9559):

    MOTION = ALProxy("ALMotion", robotIP, port)
    CAMERA = ALProxy("ALVideoDevice", robotIP, port)
    TTS = ALProxy("ALTextToSpeech", robotIP, port)

    MOTION.setMoveArmsEnabled(False, False)
    standWithStick(0.3, robotIP, port)
    MOTION.angleInterpolation("HeadPitch", 0, 0.5, True)
    MOTION.angleInterpolation("HeadYaw", 0, 0.5, True)

    CAMERA.setActiveCamera(0)
    # VGA  设置分辨率为2:640*480 0:160*120
    resolution = 2
    # RGB  设置颜色空间为RGB
    colorSpace = 11
    videoClient = CAMERA.subscribe("python_client", resolution, colorSpace, 5)
    #设置曝光度模式
    CAMERA.setCamerasParameter(videoClient,22,2)
    time.sleep(0.5)

    #获取照片
    whiteBlockImage = CAMERA.getImageRemote(videoClient)
    CAMERA.unsubscribe(videoClient)

    imageWidth = whiteBlockImage[0]
    imageHeight = whiteBlockImage[1]
    array = whiteBlockImage[6]

    #装换为PIL图片格式
    img = Image.fromstring("RGB", (imageWidth, imageHeight), array)
    img.save("camImage.png", "PNG")
    imgTest = cv2.imread("camImage.png",1)

    #imgTest = cv2.imread("photo_02.png",1)
    #把黄杆以上的部分减掉
    #RGB转换为HSV颜色空间
    hsv = cv2.cvtColor(imgTest,cv2.COLOR_BGR2HSV)
    #获取掩膜
    mask=cv2.inRange(hsv,yellow_thresholdMin,yellow_thresholdMax)
    #原始图像与掩膜做位运算，提取色块
    res=cv2.bitwise_and(imgTest,imgTest,mask=mask)
    #将hsv转化为rgb
    color = cv2.cvtColor(res,cv2.COLOR_HSV2RGB)
    #转换为灰度图
    gray = cv2.cvtColor(color,cv2.COLOR_RGB2GRAY)
    #二值化
    ret,th1 = cv2.threshold(gray,100,255,cv2.THRESH_BINARY)

    #开运算，去除噪声(开运算=先腐蚀再膨胀)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(2,2))
    erosion = cv2.erode(th1,kernel,iterations = 1)
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(4,8))
    dilation = cv2.dilate(erosion,kernel,iterations = 1)

    #获取轮廓
    contours, hierarchy = cv2.findContours(dilation,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    #如果存在多个轮廓，取最大的那个
    if(len(contours) > 1):
        lengths = map(len, contours)
        contours = [contours[lengths.index(max(lengths))]]
    #根据轮廓值找极点，最下的那个点，像素坐标值
    cnt = contours[0]
    bottommost = tuple(cnt[cnt[:,:,1].argmax()][0])

    cv2.circle(imgTest, bottommost, 5, (0,0,255), -1)

    #图像减运算，把黄杆以上的减掉
    maskArray1 = numpy.zeros((bottommost[1] + 10, imageWidth), numpy.uint8)
    maskArray2 = numpy.ones((imageHeight - bottommost[1] - 10, imageWidth), numpy.uint8)
    maskArray = numpy.concatenate((maskArray1, maskArray2), axis=0)
    imgTest = cv2.bitwise_and(imgTest, imgTest, mask = maskArray)

    imgROI = imgTest[0:imageHeight, imageWidth/2-20:imageWidth/2+20]
    #检测中间有没有障碍物
    grayROI = cv2.cvtColor(imgROI,cv2.COLOR_RGB2GRAY)
    ret,thresholdROI = cv2.threshold(grayROI,200,255,cv2.THRESH_BINARY)
    #获取轮廓
    contoursROI, hierarchy = cv2.findContours(thresholdROI,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    #如果有白色障碍物
    if(contoursROI != []):
        #如果存在多个轮廓，取最大的那个
        if(len(contoursROI) > 1):
            lengthsROI = map(len, contoursROI)
            contoursROI = [contoursROI[lengthsROI.index(max(lengthsROI))]]
        #根据轮廓值找极点，最右的那个点，像素坐标值
        cntROI = contoursROI[0]
        rightmost = tuple(cntROI[cntROI[:,:,0].argmax()][0])
        #cv2.circle(imgROI, rightmost, 5, (0,0,255), -1)
        #计算障碍物极点到机器人的距离
        disY = rightmost[1] - imageHeight/2
        ObjectAngleV = cameraVFOV / imageHeight * disY + 1.2
        distObject = robotHeight/(math.tan(ObjectAngleV * math.pi/180.0))
        ctheta = 0.3 / distObject
        finalAngle = math.atan(ctheta)
        return finalAngle
    else:
        CAMERA.setActiveCamera(1)
        whiteBlockImage = CAMERA.getImageRemote(videoClient)
        CAMERA.unsubscribe(videoClient)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setMoveArmsEnabled [as 别名]
def redBallDetection(red_thresholdMin, red_thresholdMax, onlyTopCamera=False, method=1, addAngle=0, robotIP="127.0.0.1", port=9559):
    MOTION = ALProxy("ALMotion", robotIP, port)
    CAMERA = ALProxy("ALVideoDevice", robotIP, port)
    TTS = ALProxy("ALTextToSpeech", robotIP, port)

    #初始化机器人姿势
    MOTION.setMoveArmsEnabled(False, False)
    standWithStick(0.3, robotIP, port)

    MOTION.angleInterpolation("HeadPitch", 0, 0.5, True)
    MOTION.angleInterpolation("HeadYaw", 0, 0.5, True)

    MOTION.angleInterpolation("HeadPitch", addAngle*math.pi/180.0, 0.5, False)

    angleFlag = 0
    cameraID = 1
    val = []

    TTS.say("开始检测红球")
    if method == 0:
        #首先识别正前方，然后是左边45度，最后是右边45度，三个方向由标志位angleFlag进行标记
        for i in range(0,3):
            #先打开下方摄像头进行检测
            CAMERA.setActiveCamera(1)
            cameraID = 1
            val = searchBall(0, True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = 0
                i -= 1
                break

            val = searchBall(45, True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = 1
                i -= 1
                break

            val = searchBall(-45,True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = -1
                i -= 1
                break

            #先打开上方摄像头进行检测
            CAMERA.setActiveCamera(0)
            cameraID = 0
            val = searchBall(0,True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = -1
                i -= 1
                break

            val = searchBall(45,True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = -1
                i -= 1
                break

            val = searchBall(-45,True, robotIP, port)
            time.sleep(1)
            if(val and isinstance(val,list) and len(val) >= 2):
                angleFlag = -1
                i -= 1
                break
            TTS.say("球在哪里呢")
        #如果3次都没有检测到就返回False
        if(i == 2):
            TTS.say("红球检测失败")
            return False
        #检测完红球后，最后看向正前方
        MOTION.angleInterpolation("HeadYaw",0,0.5,True)
        time.sleep(0.5)
        TTS.say("哈哈!我看到了!")
        #看到球之后,获取球的水平垂直偏角
        ballinfo = val[1]
        thetah = ballinfo[0]
        thetav = ballinfo[1]

        #根据红球方向标志位确定红球具体角度值
        if(cameraID == 0):
            thetav = thetav + 1.2*math.pi/180.0 + addAngle*math.pi/180.0
            cameraHeight = topCameraHeight
        elif(cameraID == 1):
            thetav = thetav + 39.7*math.pi/180.0 + addAngle*math.pi/180.0
            cameraHeight = bottomCameraHeight
        if(angleFlag == 1):
            theta = thetah + 45*math.pi/180.0
        elif(angleFlag == -1):
            theta = thetah - 45*math.pi/180.0
        else:
            theta = thetah

        distBall = cameraHeight/(math.tan(thetav))
        return [distBall,theta]

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

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

    def __init__(self):
        self.animated_speech_proxy = None
        self.motion_proxy = None
        self.posture_proxy = None
        self.awareness_proxy = None
        self.autonomous_move_proxy = None
        self.autonomous_life_proxy = None

    def connect(self, host, port):
        """Takes connection params and builds list of ALProxies"""
        print 'Core - Connecting to robot on {0}:{1}...'.format(host, port)
        try:
            self.animated_speech_proxy = ALProxy("ALAnimatedSpeech", host, port)
            self.motion_proxy = ALProxy("ALMotion", host, port)
            self.posture_proxy = ALProxy("ALRobotPosture", host, port)
            self.awareness_proxy = ALProxy("ALBasicAwareness", host, port)
            self.autonomous_move_proxy = ALProxy("ALAutonomousMoves", host, port)
            self.autonomous_life_proxy = ALProxy("ALAutonomousLife", host, port)
        except Exception as exception: # pylint: disable=broad-except
            raise Exception('Could not create proxy:{0}', format(exception))
        self.set_autonomous_life(False)

    def say(self, animated_speech):
        self.animated_speech_proxy.say(animated_speech) 

    def move(self, rotation, distance):
        motion = self.motion_proxy
        motion.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])
        motion.setMotionConfig([["ENABLE_STIFFNESS_PROTECTION", True]])
        motion.setCollisionProtectionEnabled('Arms', True)
        motion.setExternalCollisionProtectionEnabled('All', True)
        motion.moveTo(0, 0, rotation)
        motion.moveTo(distance, 0, 0)
        
    def open_hand(self, hand):
        self.motion_proxy.openHand(hand)
 
    def close_hand(self, hand):
        self.motion_proxy.closeHand(hand)

    def set_stiffness(self, joint, stiffness):
        print 'Setting {0} to stiffness {1}...'.format(joint, stiffness)
        self.motion_proxy.stiffnessInterpolation(joint, stiffness, 1.0) 
        
    def set_joint_angle(self, joint, angle_degrees, speed=0.1):
        print 'Setting {0} to {1} angle in degrees...'.format(joint, angle_degrees)
        self.motion_proxy.angleInterpolationWithSpeed(joint, math.radians(angle_degrees), speed)
        
    def set_pose(self, pose):
        self.posture_proxy.goToPosture(pose, 0.5) 
      
    def set_autonomous_life(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Life')
        target_state = 'solitary' if set_on else 'disabled'
        self.autonomous_life_proxy.setState(target_state)
        
    def get_autonomous_life_state(self):
        return self.autonomous_life_proxy.getState()
  
    def set_autonomous_moves(self, set_on):
        self.print_sub_system_update(set_on, 'Autonomous Moves')
        target_state = 'backToNeutral' if set_on else 'none'
        self.autonomous_move_proxy.setBackgroundStrategy(target_state)
        
    def set_awareness(self, set_on):
        self.print_sub_system_update(set_on, 'Basic Awareness')
        if set_on:
            self.awareness_proxy.startAwareness() 
        else:
            self.awareness_proxy.stopAwareness()

    def set_breathing(self, set_on):
        self.print_sub_system_update(set_on, 'body breathing')
        self.motion_proxy.setBreathEnabled('Body', set_on)
        self.print_sub_system_update(set_on, 'arm breathing')
        self.motion_proxy.setBreathEnabled('Arms', set_on)

    def set_move_arms_enabled(self, left_arm, right_arm):
        self.motion_proxy.setMoveArmsEnabled(left_arm, right_arm)

    @staticmethod
    def print_sub_system_update(set_on, sub_process):
        on_off = ['off', 'on']
        print 'Turning {0} {1}...'.format(on_off[set_on], sub_process)