Good stable approach

1. Refactored state machine
2. Goal search robuster by looking up
3. More accurate ball distance approximation
4. Eye of the tiger
5. Hasta la vista baby

pykick/__main__.py

@@ -11,25 +11,23 @@ if __name__ == '__main__':
 
     try:  # Hit Ctrl-C to stop, cleanup and exit
         cfg = read_config()
-        with InterruptDelayed():
+        with InterruptDelayed():  # Ignore Ctrl-C for a while
             striker = Striker(
                 nao_ip=cfg['ip'], nao_port=cfg['port'],
                 res=cfg['res'], ball_hsv=cfg['ball'],
                 goal_hsv=cfg['goal'], field_hsv=cfg['field'],
                 ball_min_radius=cfg['ball_min_radius'],
             )
+            striker.speak('tiger')
+            sleep(4.75)
+            striker.mover.stand_up(1.0)
+            sleep(9)
             print('Initialized')
-            striker.mover.stand_up()
+            striker.speak('Initialized')
 
         state = 'init'
-        # init_soll = 0.0
-        # align_start = 0.15
-        # curve_start = -0.1
-        # curve_stop = 0.1
-        # soll = init_soll
-        striker.speak("Initialized")
-        approach_steps = 0
         loop_start = time()
+
         while True:
             loop_end = time()
             print('Loop time:', loop_end - loop_start)
@@ -41,126 +39,96 @@ if __name__ == '__main__':
 
             if state == 'init':
                 striker.mover.set_head_angles(0, 0)
-                striker.speak("Start the Ball tracking")
                 striker.ball_tracking(tol=0.05)
-                striker.speak(
+                striker.mover.stand_up()
-                    "I have found the Ball, starting with. Goal search"
+                sleep(0.5)
-                )
+                bdist = striker.distance_to_ball()
+                striker.speak('Ball distance is %.2f' % bdist)
                 _, _, gcc = striker.goal_search()
+                print('Goal center', gcc, 'Ball dist', bdist)
 
-                if abs(gcc) < 0.4:
+                if abs(gcc) < 0.4 or bdist <= 0.2:
-                    striker.speak('Direct approach')
+                    print('Straight approach')
                     state = 'straight_approach'
                     approach = 0
-                    continue
 
-                if gcc < 0:
+                elif 0.20 < bdist < 0.50:
-                    striker.speak("I have found the. goal on the right")
+                    print('Rdist is hypo')
-                    approach = 1
+                    state = 'rdist_is_hypo'
+                    approach = 1 if gcc < 0 else - 1
                 else:
-                    striker.speak("I have found the. goal on the left")
+                    print('Bdist is hypo')
-                    approach = -1
+                    state = 'bdist_is_hypo'
+                    approach = 1 if gcc < 0 else - 1
+
+                if approach == 1:
+                    striker.speak('Goal on the right')
+                elif approach == -1:
+                    striker.speak('Goal on the left')
+                else:
+                    striker.speak('Direct approach')
 
                 striker.mover.set_head_angles(0, 0)
-                #approach = 1 if goal_center < 0 else -1
-                #approach = 1
                 sleep(0.5)
-                state = 'ball_approach'
-
-            elif state == 'tracking':
-                # start ball approach when ball is visible
-                print('Soll angle')
-                striker.ball_tracking(tol=0.15)
-                # break
-                if approach_steps < 2 and approach != 0:
-                    state = 'ball_approach'
-                else:
-                    state = 'straight_approach'
 
             elif state == 'straight_approach':
+                striker.ball_tracking(tol=0.20)
                 bil = striker.get_ball_angles_from_camera(
                     striker.lower_camera
                 )  # Ball in lower
                 print('Ball in lower!', bil)
-                if bil is not None and bil[1] > 0.15:
+                if bil is not None and bil[1] > 0.20:
-                    striker.speak('Ball is close enough, stop approach')
                     striker.mover.stop_moving()
-                    striker.speak('Align to goal')
+                    striker.speak('Aligning to goal')
                     state = 'goal_align'
                 else:
-                    striker.speak('Continue running')
                     striker.run_to_ball(1)
-                    state = 'tracking'
 
-            elif state == 'ball_approach':
+            elif state == 'bdist_is_hypo':
-                sleep(0.8)
+                angle = striker.walking_direction(approach, bdist, 'bdist')
-                bil = striker.get_ball_angles_from_camera(
+                rdist = bdist * cos(angle)
-                    striker.lower_camera
-                )  # Ball in lower
-
-                try:
-                    d = striker.distance_to_ball()
-                except ValueError:
-                    if bil is not None:
-                        state = 'straight_approach'
-                        striker.speak('Ball is close. ' +
-                                      'But not close enough maybe')
-                    else:
-                        state = 'tracking'
-                    continue
-
-                print('Distance to ball', d)
-                striker.speak("The distance to the ball is approximately "
-                              + str(round(d,2)) + " Meters")
-                angle = striker.walking_direction(approach, d)
-                d_run = d * cos(angle)
                 print('Approach angle', angle)
 
                 striker.mover.move_to(0, 0, angle)
                 striker.mover.wait()
-                striker.run_to_ball(d_run)
+                striker.run_to_ball(rdist)
                 striker.mover.wait()
-                striker.speak("I think I have reached the ball. " +
-                              "I will start rotating")
-                approach_steps += 1
                 striker.mover.move_to(0, 0, -pi/2 * approach)
                 striker.mover.wait()
-                state = 'tracking'
+                state = 'init'
+
+            elif state == 'rdist_is_hypo':
+                angle = striker.walking_direction(approach, bdist, 'rdist')
+                rdist = bdist / cos(angle)
+                print('Approach angle', angle)
+
+                striker.mover.move_to(0, 0, angle)
+                striker.mover.wait()
+                striker.run_to_ball(rdist)
+                striker.mover.wait()
+                striker.mover.move_to(0, 0, -(pi/2 - angle) * approach)
+                striker.mover.wait()
+                state = 'init'
 
             elif state == 'goal_align':
                 try:
                     if striker.align_to_goal():
+                        striker.speak('Ball and goal are aligned')
                         state = "align"
                 except ValueError:
-                        state = 'tracking'
+                    continue
 
             elif state == 'align':
-                striker.speak("I will try now to align to the ball")
                 striker.mover.set_head_angles(0, 0.25, 0.3)
                 sleep(0.5)
                 try:
                     success = striker.align_to_ball()
                     sleep(0.3)
                     if success:
-                        striker.speak('Hasta la vista, Baby')
                         state = 'kick'
+                        striker.speak('hasta')
                 except ValueError:
-                    pass
+                    striker.ball_tracking()
-                    # striker.mover.set_head_angles(0, 0, 0.3)
-                    # state = 'tracking'
-
-            elif state == 'simple_kick':
-                striker.mover.set_head_angles(0,0.25,0.3)
-                striker.ball_tracking(tol=0.10, soll=0)
-                print('Doing the simple kick')
-
-                # just walk a short distance forward, ball should be near
-                # and it will probably be kicked in the right direction
-                striker.speak("Simple Kick")
-                sleep(1)
-                striker.mover.move_to_fast(0.5, 0, 0)
-                striker.mover.wait()
-                break
 
             elif state == 'kick':
                 print('KICK!')
@@ -168,8 +136,8 @@ if __name__ == '__main__':
                 sleep(0.3)
                 striker.mover.kick(fancy=True, foot='L')
                 striker.mover.stand_up()
+                striker.speak('Nice kick. Lets do the dance')
                 sleep(2)
-                striker.speak("Nice kick. Let's do a dance")
                 striker.mover.dance()
                 break
     finally:
@@ -177,6 +145,30 @@ if __name__ == '__main__':
         striker.mover.rest()
 
 
+
+
+
+# ____________________ STRIKER NEW ________________________________
+#
+#  Ball tracking --> Distance to ball --> Goal angle
+#    ^                                           |
+#    |                                           |
+#    |                                yes        v
+#    |                  Ball distance <--  Goal angle > thr
+#    |                 /   |         \           |
+#    |        > 50 cm /    |(20,50)   \ < 20cm   | no
+#    |               /     v           \         v
+#    +- Distance is <   Walk is hypo    \  Straight approach
+#    |    hypo              |            >  until goal align
+#    |                      |                (bil > 0.2)
+#    -----------------------+                    |
+#                                                |
+#      | / |  /^ | /                             v
+#      |(  | (   |(       Ball           Goal align
+#      | \ |  \_ | \ <-- align <-- (if lost ball run backwards)
+#
+#__________________________________________________________________
+
 # ____________________ STRIKER __________________________
 #
 #        +----> Ball tracking (see below) <-------------+

pykick/movements.py

@@ -1,6 +1,6 @@
 # from time import sleep
 import argparse
-from math import radians
+from math import radians, pi
 
 from naoqi import ALProxy
 
@@ -169,6 +169,16 @@ class NaoMover(object):
         if not self.ready_to_move:
             self.mp.moveInit()
             self.ready_to_move = True
+        if rotation != 0:
+            n_cycles = int(abs(rotation) // (pi/4))
+            sign = 1 if rotation > 0 else -1
+            rest = abs(rotation) % (pi/4)
+            print('Rotation', rotation, 'Cycles', n_cycles, 'Rest', rest)
+            for _ in range(n_cycles):
+                self.mp.post.moveTo(0, 0, pi/4 * sign)
+                self.wait()
+            self.mp.post.moveTo(0, 0, rest * sign)
+        else:
             self.mp.post.moveTo(front, side, rotation)
 
     def move_to_fast(self, front, side, rotation, wait=False):
@@ -210,7 +220,7 @@ class NaoMover(object):
                     clear_existing
                 )
         self.wait()
-        self.stand_up(0.5)
+        self.stand_up(0.7)
 
     def wait(self):
         self.mp.waitUntilMoveIsFinished()

pykick/nao_defaults.json

@@ -11,19 +11,19 @@
             255
         ]
     ], 
-    "cam": 1, 
     "goal": [
         [
             0, 
             0, 
-            100
+            89
         ], 
         [
             180, 
-            65, 
+            73, 
             255
         ]
     ], 
+    "fps": 10, 
     "res": 2, 
     "ball_min_radius": 0.01, 
     "field": [
@@ -38,7 +38,7 @@
             255
         ]
     ], 
-    "fps": 10, 
+    "cam": 1, 
     "ip": "192.168.0.11", 
     "port": 9559
 }

pykick/striker.py

@@ -2,7 +2,7 @@ from __future__ import print_function
 from __future__ import division
 
 from threading import Thread
-from math import pi, tan, asin, radians
+from math import pi, tan, asin, atan, radians
 from time import sleep, time, strftime
 from collections import deque
 
@@ -53,7 +53,8 @@ class Striker(object):
         self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
 
         # Talking
-        self.speaker = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
+        self.player = ALProxy('ALAudioPlayer', bytes(nao_ip), nao_port)
+        self.tts = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
         self.speach_queue = deque(maxlen=4)
         self.speach_history = []
         self.tts_thread = Thread(target=self._speaker)
@@ -80,7 +81,14 @@ class Striker(object):
     def _speaker(self):
         while not self.is_over:
             while self.speach_queue:
-                self.speaker.say(self.speach_queue.pop())
+                text = self.speach_queue.pop()
+                if text in ('hasta', 'tiger'):
+                    file_id = self.player.loadFile(
+                        '/home/nao/audio/' + text + '.mp3'
+                    )
+                    self.player.play(file_id)
+                else:
+                    self.tts.say(text)
             sleep(0.5)
 
     def _pov(self):
@@ -102,25 +110,22 @@ class Striker(object):
         """Intelligently rotate the robot to search for stuff."""
         self.mover.stop_moving()
         self.rotating = False
-        yaw = self.mover.get_head_angles()[0]
+        yaw, pitch = self.mover.get_head_angles()
-        mag = yaw
 
         # determine direction of head rotation
         sign = 1 if yaw >= 0 else -1
 
         # the robot starts to move arround his z-Axis in the direction where his
         # head is aligned when the head yaw angle has reached his maximum
-        if mag > 0.8:
+        if yaw > 0.8:
-            self.mover.set_head_angles(-pi / 8, 0, 0.5)
+            self.mover.set_head_angles(-pi / 8, pitch, 0.5)
             sleep(0.5)
-        elif mag < -0.8:
+        elif yaw < -0.8:
             self.mover.move_to_fast(0, 0, -pi / 4)
             self.mover.wait()
-            #self.speak("Where is the ball? I am searching for it")
         # rotate head to the left, if head yaw angle is equally zero or larger
         # rotate head to the right, if head yaw angle is smaller than zero
         else:
-            #self.speak("I have found the ball")
             self.mover.change_head_angles(sign * pi / 8, 0, 0.5)
             sleep(0.3)
 
@@ -175,15 +180,23 @@ class Striker(object):
         return goal_l, goal_r, goal_c
 
     def distance_to_ball(self):
+        camera = 'upper'
         angles = self.get_ball_angles_from_camera(self.upper_camera)
+        if angles == None:
+            camera = 'lower'
+            angles = self.get_ball_angles_from_camera(self.lower_camera)
             if angles == None:
                 raise ValueError('No ball in sight')
         y_angle = angles[1]
-        y_angle = pi/2 - y_angle - radians(15)
+        y_angle = pi/2 - y_angle - radians(15) - (radians(39)
+                                                  if camera == 'lower'
+                                                  else 0)
+        print('Ball distance through', camera, 'camera')
+        print('Angles', angles)
         return 0.5 * tan(y_angle)
 
-    def walking_direction(self, lr, d):
+    def walking_direction(self, lr, d, hypo):
-        return asin(0.5 / d) * lr
+        return (asin(0.5 / d) if hypo == 'bdist' else atan(0.2 / d)) * lr
 
     def ball_tracking(self, soll=0, tol=0.15):
         """Track the ball using the feed from top and bottom camera.
@@ -215,11 +228,9 @@ class Striker(object):
             # stop visibility check
 
             if not in_sight:
-                self.speak('No ball visible search it')
                 self.scan_rotation()
                 continue
 
-            # self.speak('I see the ball')
             ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
             print()
 
@@ -257,7 +268,6 @@ class Striker(object):
         if abs(d_yaw) > tol:
             self.mover.stop_moving()
             print('Going to rotate by', d_yaw)
-            self.speak('Going to rotate')
             self.mover.set_head_angles(soll, head_pitch, 0.3)
             self.mover.move_to(0, 0, d_yaw)
             self.mover.wait()
@@ -275,7 +285,7 @@ class Striker(object):
         goal_x, goal_y = 0.095, 0.4
         dx, dy = goal_x - x, goal_y - y
 
-        dx = -dx * 0.2 if abs(dx) > 0.05 else 0
+        dx = -dx * 0.2 if abs(dx) > 0.03 else 0
         dy = dy * 0.3 if abs(dy) > 0.05 else 0
         if dx == 0  and dy == 0:
             return True
@@ -293,14 +303,11 @@ class Striker(object):
             ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
                                                            mask=False)
             if ball_angles is None:
-                self.speak("Cannot see the ball")
                 raise ValueError('No ball')
         x, y = ball_angles
 
         print(x, y)
-        self.speak("Turn to ball")
         self.turn_to_ball(x, y, tol=0.15)
-        self.speak('Trying to find the goal')
         sleep(0.2)
 
         goal = self.goal_search()
@@ -308,13 +315,9 @@ class Striker(object):
             return False
 
         gcl, gcr, gcc = goal
-        self.speak('Goal found')
         print('Goal:', gcl, gcr, gcc)
 
         if gcl > 0 > gcr:
-            self.speak("Goal and ball are aligned")
-            print('Goal ball aligned!')
-            #raise SystemExit
             return True
 
         if y > 0.35:
@@ -327,19 +330,24 @@ class Striker(object):
             # return False
 
         sign = -1 if gcc > 0 else 1
-        num_steps = int(min(abs(gcc), 0.1) // 0.05)
+        if sign == 1:
-        for _ in range(num_steps):
+            self.speak('Goal is on the right')
+        elif sign == -1:
+            self.speak('Goal is on the left')
+
+        for _ in range(2):
             self.mover.move_to(0, 0.05 * sign, 0)
             self.mover.wait()
         return False
 
     def goal_search(self):
+        self.speak('Searching for goal')
         goal_angles = None
         positions = [0, pi/6, pi/4, pi/3, pi/2]
         angles = [-p for p in positions] + [p for p in positions][1:]
 
         for angle in angles:
-            self.mover.set_head_angles(angle, 0)
+            self.mover.set_head_angles(angle, -0.3)
             sleep(0.5)
             for i in range(5):
                 print(i, goal_angles)