Python ALProxy.moveInit方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
    def post(self):
		naoip=self.get_argument('ip','')
		naoport= self.get_argument('port','')
		ncmd= self.get_argument('cmd','')	
		if(ncmd=='walk'):
			motion = ALProxy("ALMotion", str(naoip), int(naoport))
			tts    = ALProxy("ALTextToSpeech", str(naoip), int(naoport))
			motion.moveInit()
			motion.post.moveTo(0.5, 0, 0)
			tts.say("I'm walking")
			self.write("DONE!")
		if(ncmd=='talk'):
			naoip=self.get_argument('ip','')
			naoport= self.get_argument('port','')	
			talk=self.get_argument('nspeak','')	
			if (talk==''):
				talk="Hello, I am Nao"
			animatedSpeechProxy = ALProxy("ALAnimatedSpeech", str(naoip), int(naoport))
			# set the local configuration
			configuration = {"bodyLanguageMode":"contextual"}
			# say the text with the local configuration
			animatedSpeechProxy.say(str(talk), configuration)	
			self.write("DONE!")	
		if(ncmd=='dance'):
			naoip=self.get_argument('ip','')
			naoport= self.get_argument('port','')	
			behaviorName=str(self.get_argument('ndance',''))	
			managerProxy = ALProxy("ALBehaviorManager", str(naoip), int(naoport))
			getBehaviors(managerProxy)
			launchAndStopBehavior(managerProxy, behaviorName)
			defaultBehaviors(managerProxy, behaviorName)
			self.write("DONE!")

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

    # Initialize the walk process.
    # Check the robot pose and take a right posture.
    # This is blocking called.
    motionProxy.moveInit()

    # TARGET VELOCITY
    X         = 1.0
    Y         = 0.0
    Theta     = 0.0
    Frequency = 1.0

    # Default walk (MaxStepX = 0.04 m)
    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 moveInit [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.moveInit()
    motionProxy.setExternalCollisionProtectionEnabled("All", False)
    motionProxy.moveTo(0, steps / -4.0, 0) # X, Y, Theta

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
    def walk(self):
        motionProxy = ALProxy("ALMotion")
        #http://doc.aldebaran.com/2-1/dev/python/making_nao_move.html?highlight=post#making-nao-move
        postureProxy = ALProxy("ALRobotPosture")

        # Wake up robot
        motionProxy.wakeUp()

        # Send robot to Stand Init
        postureProxy.goToPosture("StandInit", 1.0)
        time.sleep(1)
        motionProxy.setExternalCollisionProtectionEnabled("All", False)
        motionProxy.moveInit()
        #http://doc.aldebaran.com/2-1/dev/python/examples/motion/walk.html#move-to
        motionProxy.post.moveTo(0.5, 0, 0)
        #moveTo with post isn't a blocking call
        # Go to rest position

        postureProxy.goToPosture("Sit", 1.0)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
    def walk(self):
        motionProxy = ALProxy("ALMotion")
        #http://doc.aldebaran.com/2-1/dev/python/making_nao_move.html?highlight=post#making-nao-move
        postureProxy = ALProxy("ALRobotPosture")
        speechProxy = ALProxy("ALTextToSpeech")

        # Wake up robot
        motionProxy.wakeUp()

        # Send robot to Stand Init
        postureProxy.goToPosture("StandInit", 1.0)
        time.sleep(1)
        motionProxy.setExternalCollisionProtectionEnabled("All", False)
        motionProxy.moveInit()
        #http://doc.aldebaran.com/2-1/dev/python/examples/motion/walk.html#move-to
        motionProxy.post.moveTo(2.0, 0, 0)
        speechProxy.say("I have arrived.")
        #moveTo with post isn't a blocking call, as illustrated by him
        #saying "I have arrived" before actually arriving
        # Go to rest position
        postureProxy.goToPosture("Sit", 1.0)

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

    # Initialize the move process.
    # Check the robot pose and take a right posture.
    # This is blocking called.
    motionProxy.moveInit()

    testTime = 10.0 # seconds
    t  = 0.0
    dt = 0.2

    while t<testTime:
        # WALK
        X         = random.uniform(0.4, 1.0)
        Y         = random.uniform(-0.4, 0.4)
        Theta     = random.uniform(-0.4, 0.4)
        Frequency = random.uniform(0.5, 1.0)
        try:
            motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
        except Exception, errorMsg:
            print str(errorMsg)
            print "This example is not allowed on this robot."
            exit()

        # JERKY HEAD
        motionProxy.setAngles("HeadYaw", random.uniform(-1.0, 1.0), 0.6)
        motionProxy.setAngles("HeadPitch", random.uniform(-0.5, 0.5), 0.6)

        t = t + dt
        time.sleep(dt)

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
tts = ALProxy("ALTextToSpeech", robot_ip, robot_port)
posture = ALProxy("ALRobotPosture", robot_ip, robot_port)

tts.setLanguage("English")
# ----------> Make robot move <----------

# 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()

# 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()	

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
argvs = sys.argv 
ip = argvs[1]
port = int(argvs[2])
speed = float(argvs[3])

try:
  motion_proxy = ALProxy('ALMotion',ip,port)
except:
  quit()

part = 'Body'
body_names = motion_proxy.getBodyNames(part)


motion_proxy.moveInit()
motion_proxy.setAngles('HeadYaw',         0.0, speed)
motion_proxy.setAngles('HeadPitch',       0.0, speed)
motion_proxy.setAngles('LShoulderPitch',  1.4, speed)
motion_proxy.setAngles('LShoulderRoll',   0.0, speed)
motion_proxy.setAngles('LElbowYaw',       0.0, speed)
motion_proxy.setAngles('LElbowRoll',      0.0, speed)
motion_proxy.setAngles('LWristYaw',       -1.5, speed)
motion_proxy.setAngles('LHand',           0.5, speed)
motion_proxy.setAngles('HipRoll',         0.0, speed)
motion_proxy.setAngles('HipPitch',        0.0, speed)
motion_proxy.setAngles('KneePitch',       -0.2, speed)
motion_proxy.setAngles('RShoulderPitch',  1.4, speed)
motion_proxy.setAngles('RShoulderRoll',   0.0, speed)
motion_proxy.setAngles('RElbowYaw',       1.5, speed)
motion_proxy.setAngles('RElbowRoll',      0.0, speed)

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

    # first we defined the goal
    goal = m.Pose2D(0.0, -0.4, 0.0)

    # We get the dubins solution (control points) by
    # calling an almath function

    circleRadius = 0.04
    # Warning : the circle use by dubins curve
    #           have to be 4*CircleRadius < norm(goal)
    dubinsSolutionAbsolute = m.getDubinsSolutions(goal, circleRadius)

    # moveTo With control Points use relative commands but
    # getDubinsSolution return absolute position
    # So, we compute dubinsSolution in relative way
    dubinsSolutionRelative = []
    dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
    for i in range(len(dubinsSolutionAbsolute)-1):
        dubinsSolutionRelative.append(
                dubinsSolutionAbsolute[i].inverse() *
                dubinsSolutionAbsolute[i+1])

    # create a vector of moveTo with dubins Control Points
    moveToTargets = []
    for i in range(len(dubinsSolutionRelative)):
        moveToTargets.append(
            [dubinsSolutionRelative[i].x,
             dubinsSolutionRelative[i].y,
             dubinsSolutionRelative[i].theta] )

    # Initialized the Move process and be sure the robot is ready to move
    # without this call, the first getRobotPosition() will not refer to the position
    # of the robot before the move process
    motionProxy.moveInit()

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

    motionProxy.moveTo( moveToTargets )

    # With MoveTo control Points, it's also possible to customize the gait parameters
    # motionProxy.moveTo(moveToTargets,
    #                    [["StepHeight", 0.001],
    #                     ["MaxStepX", 0.06],
    #                     ["MaxStepFrequency", 1.0]])

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

    # compute and print the robot motion
    robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
    print "The Robot Move Command: ", robotMoveCommand

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
class NCModule(ALModule):
    """docstring for Nao Challenge Module"""
    def __init__(self,name):
        self.name = name
        ALModule.__init__(self, name)
        # Create new object display a colored message on computer's terminal.
        self.logs = logs.logs()
        
        # Create new proxies.
        self.tts = ALProxy("ALTextToSpeech", IP, port)
        self.logs.display("Subscribed to an ALTextToSpeech proxy",
                          "Good")

        
        self.motion = ALProxy("ALMotion", IP, port)
        self.motion.moveInit()
        self.logs.display("Subscribed to an ALMotion proxy",
                          "Good")
        
        self.posture = ALProxy("ALRobotPosture")
        self.logs.display("Subscribed to an ALRobotPosture proxy",
                          "Good")

        self.leds = ALProxy("ALLeds")
        self.logs.display("Subscribed to an ALLeds proxy",
                          "Good")

        self.RedTracker = ALProxy("ALRedBallTracker")
        self.logs.display("Subscribed to an ALRedBallTracker proxy",
                          "Good")
        
        self.RemoteTx = ALProxy("ALInfrared")
        self.RemoteTx.initReception(3)
        self.logs.display("Subscribed to an ALInfrared proxy",
                          "Good")

        self.NCProxy = ALProxy("NaoChallengeGeoloc",IP,port)
        try:
            self.NCProxy.registerToVideoDevice(definition, colors)
            self.logs.display("Subscribed to an NaoChallengeGeoloc proxy",
                              "Good")
        except:
            self.logs.display("NaoChallengeGeoloc proxy already registered",
                              "Warning")

        self.NCMotion = TimelinesModule()
        self.logs.display("Timelines initialisation",
                          "Good")

        self.sonarProxy = ALProxy("ALSonar", ip, 9559)
        self.logs.display("Subscribed to an ALSonar proxy",
                          "Good")

        self.memoryProxy = ALProxy("ALMemory", ip, 9559)
        self.logs.display("Subscribed to an ALMemory proxy",
                          "Good")

        # Prepare Nao.
        self.posture.goToPosture("StandInit", 1.0)
        self.logs.display("Nao is going to posture StandInit","Good")        
        
        self.EventsInit()
        self.logs.display("tactil initialisation","Good")

        # Ready!
        print ("--------------------------------------------------------------------------")
        print ("--------------------------------------------------------------------------")
        self.logs.display("Module ready", "Good")

##############################################################################
##                            start_memento()                               ##
##                    A method to launch memento mod                        ##    
##                                                                          ##

    def start_memento (self,*_args):

        self.TactilStop()
        
        # Walk to the Door
        self.tts.post.say("je m'occupe de la clef")
        self.logs.display("Going to the Door")
        try:
            self.NCProxy.registerToVideoDevice(definition, colors)
        except:
            self.logs.display("NaoChallengeGeoloc proxy already registered","Warning")
        self.NCProxy.walkFromDmtxToDmtx(270,220)
        self.logs.display("Datamatrix 220 reached","Good")
        self.NCProxy.unRegisterFromVideoDevice()
        self.logs.display("NaoChallengeGeoloc proxy already unregistered","Good")

        
        # Aim the key
        try:
            motion.angleInterpolation(["HeadYaw"], [[ 0.0]], [[ 0.5]], True);
        except BaseException, err:
            print err
        self.RedTracker.startTracker()
        while self.RedTracker.getPosition() == [0.0, 0.0, 0.0]:
            self.logs.display("The key isn't visible","Error")
            while self.RedTracker.getPosition() == [0.0, 0.0, 0.0]:
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
def punch(prox, n):
	prox.setAngles(commandes_punch, angles_punch[n], 0.3)

# LEDs parameters
def setLeds(R, G, B):
	ledsProxy = ALProxy("ALLeds", IP, PORT)
	ledsProxy.setIntensity('AllLedsRed', float(R) / 255.0)
	ledsProxy.setIntensity('AllLedsGreen', float(G) / 255.0)
	ledsProxy.setIntensity('AllLedsBlue', float(B) / 255.0)

# Motion
motionProxy = ALProxy("ALMotion", IP, PORT)
postureProxy = ALProxy("ALRobotPosture", IP, PORT)
StiffnessOn(motionProxy, True)
postureProxy.goToPosture("StandInit", 1.0)
motionProxy.moveInit()
motionProxy.setWalkArmsEnabled(False, False)
motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

# Camera
camProxy = ALProxy("ALVideoDevice", IP, PORT)
camProxy.setParam(18, 0)	# "kCameraSelectID", 0 : camera top, 1 : camera bottom
resolution = 0				# 0 : QQVGA, 1 : QVGA, 2 : VGA
colorSpace = 11				# RGB
videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
from_video = camProxy.getImageRemote(videoClient)
imageWidth, imageHeight = from_video[0], from_video[1]
middle_x, middle_y = float(imageWidth / 2), float(imageHeight / 2)

# Speaker
c3po = ALProxy("ALTextToSpeech", IP, PORT)

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

    # constructor
    def __init__(self, ip='127.0.0.1', port=9559):
        self.motionProxy = ALProxy('ALMotion', ip, port)
        self.postureProxy = ALProxy('ALRobotPosture', ip, port)
        self.camProxy = ALProxy('ALVideoDevice', ip, port)
        self.compassProxy = ALProxy('ALVisualCompass', ip, port)
        self.navi = ALProxy('ALNavigation', ip, port)

        resolution = vision_definitions.kVGA
        colorSpace = vision_definitions.kRGBColorSpace
        self.fps = 15
        self.videoClient = self.camProxy.subscribeCamera('python_client', 0, resolution, colorSpace, self.fps)
        # print self.videoClient
        # in case of camera subscribe overflow
        assert self.videoClient is not None
        # wake up nao
        self.motionProxy.wakeUp()
        # stand init
        self.postureProxy.goToPosture("StandInit", 0.5)
        # move init
        self.motionProxy.moveInit()

        # get the origin
        self.__origin = tuple(self.motionProxy.getRobotPosition(False))
        self.__origin_grid = (0, 0, 0)
        self.__position_grid = list(self.__origin_grid)

    # destructor
    def __del__(self):
        self.camProxy.unsubscribe(self.videoClient)
        self.camProxy.setAllParametersToDefault(0)
        # rest, set all stiffness to 0
        self.motionProxy.rest()

    def xiapao(self, x, y):
        print self.navi.navigateTo(x, y)

    # 瞎 jb 看
    def __look(self):
        ret = dict()

        pitch = self.motionProxy.getAngles('HeadPitch', False)[0]
        yaw = self.motionProxy.getAngles('HeadYaw', False)[0]
        print 'taking picture'
        img = self.__takePicture()
        ret[HeadLoc(left=yaw, down=pitch)] = img

        # 随便转转头
        self.motionProxy.setAngles('HeadPitch', random.uniform(-0.1, 0.1), 0.1)
        self.motionProxy.setAngles('HeadYaw', random.uniform(-0.1, 0.1), 0.1)
        time.sleep(2.0)

        pitch = self.motionProxy.getAngles('HeadPitch', False)[0]
        yaw = self.motionProxy.getAngles('HeadYaw', False)[0]
        print 'taking picture'
        img = self.__takePicture()
        ret[HeadLoc(left=yaw, down=pitch)] = img

        self.motionProxy.setAngles('HeadPitch', 0.0, 0.1)
        self.motionProxy.setAngles('HeadYaw', 0.0, 0.1)

        return ret

    def lookAround(self):
        print 'looking around... '

        ret = dict()
        angle = numpy.pi / 4

        ret[0.0] = self.__look()

        self.turn(angle)

        ret[angle] = self.__look()

        self.turn(- 2 * angle)

        ret[2 * numpy.pi - angle] = self.__look()

        self.turn(angle)

        return ret

    # take a picture
    def __takePicture(self):
        # for debug
        print "takePicture"

        naoImage = self.camProxy.getImageRemote(self.videoClient)

        width = naoImage[0]; height = naoImage[1]
        nchanels = naoImage[2]; array = naoImage[6]

        return self.__str2array(array, (height, width, nchanels))

    def __str2array(self, string, shape):
        assert len(string) == shape[0] * shape[1] * shape[2], len(shape) == 3
        image = numpy.zeros(shape, numpy.uint8)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
def main(robotIP, PORT=9559):
    
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    trackerProxy = ALProxy("ALTracker", robotIP, PORT)
    
    camProxy = ALProxy("ALVideoDevice", robotIP, PORT)
    resolution = 2    # VGA
    colorSpace = 11   # RGB

    videoClient = camProxy.subscribe("python_client", resolution, colorSpace, 5)
    
    # Wake up robot
    motionProxy.wakeUp()
    
    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)
    
    motionProxy.moveInit()
    
    # Move Head to the left
    #motionProxy.setAngles("HeadYaw", -math.pi/4.0, 0.6)
    trackerProxy.lookAt([-5, 5, 1], 1, 0.2, False)
    
    time.sleep(1)

    
    # Get position of the head to set it fixed
    names         = "HeadYaw"
    useSensors    = False
    commandAngles = motionProxy.getAngles(names, useSensors)
    
    print "Command angles:"
    print str(commandAngles)
    print ""
    
    testTime = 1.0 # seconds
    t  = 0.0
    dt = 0.2
    
##    while t<testTime:
##        # WALK
##        X         = 0
##        Y         = 0
##        Theta     = math.pi/4.0
##        #Frequency = random.uniform(0.5, 1.0)
##        try:
##            #motionProxy.moveToward(X, Y, Theta, [["Frequency", Frequency]])
##            motionProxy.move(0, 0, Theta)
##            #motionProxy.setAngles("HeadYaw", -0.5, 0.6)
##        except Exception, errorMsg:
##            print str(errorMsg)
##            print "This example is not allowed on this robot."
##            exit()
##
##        # JERKY HEAD
##        #motionProxy.setAngles("HeadYaw", -0.5, 0.6)
##        #motionProxy.setAngles("HeadPitch", random.uniform(-0.5, 0.5), 0.6)
##
##        t = t + dt
##        time.sleep(dt)

    X         = 0
    Y         = 0
    Theta     = -math.pi/4.0

    motionProxy.moveTo(X, Y, Theta)
    
    # Rotate the head the displacement of the body
    #motionProxy.setAngles("HeadYaw", -math.pi/2.0, 0.6)
    trackerProxy.lookAt([-5, 5, 1], 1, 0.2, False)
    
    time.sleep(1)
    
    
    # stop move on the next double support
    motionProxy.stopMove()

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
class planner_move(object):
    global mark
    mark = []
    def __init__(self, ip):
        self.ears = planner_voice()
        self.sonarProxy = ALProxy("ALSonar", ip, 9559)
        self.memoryProxy = ALProxy("ALMemory", ip, 9559)
        self.motionProxy = ALProxy("ALMotion", ip, 9559)
        self.speechProxy = ALProxy("ALTextToSpeech", ip, 9559)
        self.sensorProxy = ALProxy("ALSensors", ip, 9559)
        #self.compassProxy = ALProxy("ALVisualCompass", ip, 9559)
        self.markProxy = ALProxy("ALLandMarkDetection", ip, 9559)
        self.rpos = ALProxy("ALRobotPosture", ip, 9559)
        self.sonarProxy.subscribe("planner_move.py")
        self.sensorProxy.subscribe("planner_move.py")
        #self.compassProxy.subscribe("planner_move.py")
        self.markProxy.subscribe("planner_move.py", 500, 0.0)
        self.motionProxy.stiffnessInterpolation("Body", 0, 1)
        self.rpos.goToPosture("Stand", 1.0, 0)
        self.motionProxy.moveInit()
        self.queue = Queue.Queue()
        thread = threading.Thread(group = None, target = self.ears.run, name = 'thread1', args = (ip, self.queue), kwargs = {})
        thread.daemon = True
        thread.start()
    
    def main(self):
        self.moveForward()
        dataStructure = self.obstacleDetection()
        return dataStructure
        
    def moveForward(self):
        self.motionProxy.moveToward(.5, .02, -.04)
        return
        
    def obstacleDetection(self):
        while(True):
            front = self.memoryProxy.getData("FrontTactilTouched")
            middle = self.memoryProxy.getData("MiddleTactilTouched")
            rear = self.memoryProxy.getData("RearTactilTouched")
            left = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
            right = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
            #direction = self.memoryProxy.getData("VisualCompass/Deviation")
            detection = self.memoryProxy.getData("LandmarkDetected", 0)
            self.touched(front, middle, rear)
            dataStructure = self.landmark(detection)
            if not self.queue.empty():
                dS = self.commanded()
                if dS:
                    if dataStructure == []:
                        dataStructure = dS
                    else:
                        dataStructure[0] = dS[0]
            if not dataStructure == []:
                self.motionProxy.moveToward(0, 0, 0)
                return dataStructure
            self.footBumper()
            self.leftorright(left, right)
            #self.compass(direction)
        return
            
    def leftorright(self, left, right):
        if left == right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.decision(left, right)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        if left > right:
            if right > .25 and right < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, .5)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        if left < right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, -.5)
                self.motionProxy.moveToward(.5, .02, -.03)
            return
        
    def turn(self, radians):
        self.motionProxy.moveToward(0, 0, 0)
        self.motionProxy.moveTo(0, 0, radians)
        return
    
    def touched(self, front, middle, rear):
        if front == 1 or middle == 1 or rear == 1:
            self.motionProxy.moveToward(0, 0, 0)
            time.sleep(1)
            self.motionProxy.rest()
            sys.exit()
    
    def decision(self, left, right):
        self.motionProxy.moveTo(0, 0, .5)
        l1 = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        self.motionProxy.moveTo(0, 0, -1)
        r2 = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
        self.motionProxy.moveTo(0, 0, .5)
        if l1 > r2:
            self.motionProxy.moveTo(0, 0, .5)
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import moveInit [as 别名]
class sec():
    global ip
    ip = "10.211.16.7"
    
    def __init__(self):
        self.sonarProxy = ALProxy("ALSonar", ip, 9559)
        self.memoryProxy = ALProxy("ALMemory", ip, 9559)
        self.motionProxy = ALProxy("ALMotion", ip, 9559)
        self.speechProxy = ALProxy("ALTextToSpeech", ip, 9559)
        self.sensorProxy = ALProxy("ALSensors", ip, 9559)
        self.rpos = ALProxy("ALRobotPosture", ip, 9559)
        self.sonarProxy.subscribe("sec.py")
        self.sensorProxy.subscribe("sec.py")
        self.motionProxy.stiffnessInterpolation("Body", 0, 1)
        self.rpos.goToPosture("Stand", 1.0, 0)
        self.motionProxy.moveInit()
    
    def main(self):
        self.moveForward()
        self.obstacleDetection()
        return
        
    def moveForward(self):
        self.motionProxy.moveToward(.5, 0, -.0085)
        
    def obstacleDetection(self):
        while(True):
            front = self.memoryProxy.getData("FrontTactilTouched")
            middle = self.memoryProxy.getData("MiddleTactilTouched")
            rear = self.memoryProxy.getData("RearTactilTouched")
            left = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
            right = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
            self.touched(front, middle, rear)
            self.footBumper()
            #self.handBumper()
            self.leftorright(left, right)
        return
            
    def leftorright(self, left, right):
        if left == right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.decision(left, right)
                self.motionProxy.moveToward(.5, 0, -.0085)
            return
        if left > right:
            if right > .25 and right < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, .5)
                self.motionProxy.moveToward(.5, 0, -.0085)
            return
        if left < right:
            if left > .25 and left < .3:
                self.motionProxy.moveToward(0, 0, 0)
                self.motionProxy.moveTo(0, 0, -.5)
                self.motionProxy.moveToward(.5, 0, -.0085)
            return
    
    def touched(self, front, middle, rear):
        if front == 1 or middle == 1 or rear == 1:
            self.motionProxy.moveToward(0, 0, 0)
            self.motionProxy.rest()
            sys.exit()
    
    def decision(self, left, right):
        self.motionProxy.moveTo(0, 0, .5)
        l1 = self.memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        self.motionProxy.moveTo(0, 0, -1)
        r2 = self.memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
        self.motionProxy.moveTo(0, 0, .5)
        if l1 > r2:
            self.motionProxy(0, 0, .5)
            return
        if l1 < r2:
            self.motionProxy(0, 0, -.5)
            return
        else:
            randomBinary = random.randint(0, 1)
            if randomBinary == 0:
                self.motionProxy.moveTo(0, 0, .5)
            else:
                self.motionProxy.moveTo(0, 0, -.5)
            return
    
    def footBumper(self):
        rightBumper = self.memoryProxy.getData("RightBumperPressed")
        leftBumper = self.memoryProxy.getData("LeftBumperPressed")
        if rightBumper == 1:
            self.motionProxy.moveToward(0, 0, 0)
            self.motionProxy.moveTo(-.1, 0, .5)
            self.motionProxy.moveTo(0, 0, .5)
            return
        if leftBumper == 1:
            self.motionProxy.moveToward(0, 0, 0)
            self.motionProxy.moveTo(-.1, 0, -.5)
            self.motionProxy.moveTo(0, 0, -.5)
            return
    
    def handBumper(self):
        rightHand = self.memoryProxy.getData("HandRightBackTouched")
#.........这里部分代码省略.........