Ball search basics

scripts/reacting_to_events.py

@@ -0,0 +1,129 @@
+from naoqi import ALProxy
+from naoqi import ALBroker
+from naoqi import ALModule
+import time
+
+NAO_IP = "192.168.0.11"
+
+# Global variable to store the BallSearcher module instance
+BallSearcher = None
+memory = None
+ball_proxy = None
+ball_found = False
+
+
+class BallSearcherModule(ALModule):
+    """ A simple module able to react to facedetection 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 ALTextToSpeech for later use
+        self.tts = ALProxy("ALTextToSpeech")
+        self.tts.setParameter('speed', 100)
+        self.mp = ALProxy('ALMotion')
+        self.mp.setStiffnesses("Head", 1.0)
+
+        # Subscribe to the BallDetected event:
+        global memory
+        global ball_proxy
+        global ball_found
+        ball_proxy = ALProxy('ALRedBallDetection')
+        ball_proxy.subscribe('detector')
+        memory = ALProxy("ALMemory")
+        memory.subscribeToEvent("redBallDetected",
+            "BallSearcher",
+            "onBallDetected")
+
+    def searchForBall(self):
+        names = ["HeadYaw", "HeadPitch"]
+        sleep_period = 0.8
+        fractionMaxSpeed = 0.5
+        i=0
+        while i<2:
+            time.sleep(sleep_period)
+            if ball_found:
+                return
+            print i
+            angles = [i,0]
+            self.mp.setAngles(names, angles, fractionMaxSpeed)
+            i=float(i)+3.14/4
+
+        # go back to middle position
+        time.sleep(sleep_period)
+        if ball_found:
+            return
+        print 'go back'
+        angles = [0,0]
+        self.mp.setAngles(names, angles, fractionMaxSpeed)
+
+        # go into the right direction
+        i=0
+        while i > -2:
+            time.sleep(sleep_period)
+            if ball_found:
+                return
+            print i
+            angles = [i,0]
+            self.mp.setAngles(names, angles, fractionMaxSpeed)
+            i=i-3.14/4
+
+        # go back to middle position
+        time.sleep(sleep_period)
+        if ball_found:
+            return
+        print "get back"
+        angles = [0,0]
+        self.mp.setAngles(names, angles, fractionMaxSpeed)
+
+    def onBallDetected(self, *_args):
+        """ This will be called each time a ball is detected."""
+
+        # Unsubscribe to the event when talking, to avoid repetitions
+        memory.unsubscribeToEvent("redBallDetected", "BallSearcher")
+        # time.sleep(0.1)
+        global ball_found
+        ball_found = True
+        print 'gotcha'
+        self.tts.say("Hello, ball")
+
+        # Subscribe again to the event
+        # memory.subscribeToEvent("redBallDetected",
+            # "BallSearcher",
+            # "onBallDetected")
+
+
+def main():
+    """ Main entry point."""
+
+    # We need this broker to be able to construct
+    # NAOqi modules and subscribe to other modules
+    # The broker must stay alive until the program exists
+    myBroker = ALBroker(
+        "myBroker",
+        "0.0.0.0",
+        0,
+        '192.168.0.11',
+        9559
+    )
+
+
+    # Warning: BallSearcher must be a global variable
+    # The name given to the constructor must be the name of the
+    # variable
+    global BallSearcher
+    BallSearcher = BallSearcherModule("BallSearcher")
+    BallSearcher.searchForBall()
+
+    try:
+        while True:
+            time.sleep(1)
+    except KeyboardInterrupt:
+        print "Interrupted by user, shutting down"
+        myBroker.shutdown()
+
+
+if __name__ == "__main__":
+    main()