Take goal posts into account

pykick/__main__.py

@@ -46,8 +46,15 @@ if __name__ == '__main__':
                 striker.speak(
                     "I have found the Ball, starting with. Goal search"
                 )
-                goal_center = striker.goal_search()
+                _, _, gcc = striker.goal_search()
-                if goal_center <0:
+
+                if abs(gcc) < 0.4:
+                    striker.speak('Direct approach')
+                    state = 'straight_approach'
+                    approach = 0
+                    continue
+
+                if gcc < 0:
                     striker.speak("I have found the. goal on the right")
                     approach = 1
                 else:
@@ -65,7 +72,7 @@ if __name__ == '__main__':
                 print('Soll angle')
                 striker.ball_tracking(tol=0.15)
                 # break
-                if approach_steps < 2:
+                if approach_steps < 2 and approach != 0:
                     state = 'ball_approach'
                 else:
                     state = 'straight_approach'
@@ -86,6 +93,7 @@ if __name__ == '__main__':
                     state = 'tracking'
 
             elif state == 'ball_approach':
+                sleep(0.8)
                 bil = striker.get_ball_angles_from_camera(
                     striker.lower_camera
                 )  # Ball in lower
@@ -159,9 +167,10 @@ if __name__ == '__main__':
                 striker.mover.stand_up()
                 sleep(0.3)
                 striker.mover.kick(fancy=True, foot='L')
-                ##striker.speak("Nice kick. Let's do a dance")
+                striker.mover.stand_up()
-                #striker.mover.dance()
+                sleep(2)
-                sleep(4)
+                striker.speak("Nice kick. Let's do a dance")
+                striker.mover.dance()
                 break
     finally:
         striker.close()

pykick/striker.py

@@ -148,7 +148,7 @@ class Striker(object):
         x, y = cam.to_angles(x, y)
         return x, y
 
-    def get_goal_center_angle_from_camera(self, cam, mask=True):
+    def get_goal_angles_from_camera(self, cam, mask=True):
         try:
             frame = cam.get_frame()
         except RuntimeError as e:  # Sometimes camera doesn't return an image
@@ -158,14 +158,21 @@ class Striker(object):
         if mask:
             field = self.field_finder.find(frame)
             frame = self.field_finder.mask_it(frame, field, inverse=True)
+
         goal = self.goal_finder.find(frame)
         if goal is None:
             return None
 
-        goal_x = self.goal_finder.goal_center(goal)
+        goal_l, goal_r = self.goal_finder.left_right_post(goal)
-        goal_x, _ = cam.to_relative(goal_x, 0)
+        goal_c = self.goal_finder.goal_center(goal)
-        goal_x, _ = cam.to_angles(goal_x, 0)
+
-        return goal_x
+        goal_l, _ = cam.to_relative(goal_l, 0)
+        goal_l, _ = cam.to_angles(goal_l, 0)
+        goal_r, _ = cam.to_relative(goal_r, 0)
+        goal_r, _ = cam.to_angles(goal_r, 0)
+        goal_c, _ = cam.to_relative(goal_c, 0)
+        goal_c, _ = cam.to_angles(goal_c, 0)
+        return goal_l, goal_r, goal_c
 
     def distance_to_ball(self):
         angles = self.get_ball_angles_from_camera(self.upper_camera)
@@ -192,7 +199,7 @@ class Striker(object):
         while not ball_locked:
             # visibility check
             for i in range(3):
-                cams = [self.upper_camera, self.lower_camera]
+                cams = [self.lower_camera, self.upper_camera]
                 in_sight = False
 
                 for cam in cams:
@@ -295,11 +302,16 @@ class Striker(object):
         self.turn_to_ball(x, y, tol=0.15)
         self.speak('Trying to find the goal')
         sleep(0.2)
-        goal_center_x = self.goal_search()
-        self.speak('Goal found')
 
-        print('Goal center:', goal_center_x)
+        goal = self.goal_search()
-        if goal_center_x is not None and abs(goal_center_x) < 0.1:
+        if goal is None:
+            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
@@ -314,32 +326,32 @@ class Striker(object):
             self.mover.wait()
             # return False
 
-        sign = -1 if goal_center_x > 0 else 1
+        sign = -1 if gcc > 0 else 1
-        num_steps = int(min(abs(goal_center_x), 0.1) // 0.05)
+        num_steps = int(min(abs(gcc), 0.1) // 0.05)
         for _ in range(num_steps):
             self.mover.move_to(0, 0.05 * sign, 0)
             self.mover.wait()
         return False
 
     def goal_search(self):
-        goal_center_x = None
+        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)
             sleep(0.5)
             for i in range(5):
-                print(i, goal_center_x)
+                print(i, goal_angles)
-                if goal_center_x is None:
+                goal_angles = self.get_goal_angles_from_camera(
-                    gcx = self.get_goal_center_angle_from_camera(
+                    self.upper_camera
-                        self.upper_camera
+                )
-                    )
+                if goal_angles is not None:
-                    goal_center_x = gcx + angle if gcx is not None else None
+                    goal_angles = tuple(gc + angle for gc in goal_angles)
-                    print('Goal found: ' + str(goal_center_x)
+                    self.mover.set_head_angles(0, 0)
-                          if goal_center_x is not None
+                    print('Goal found:', str(goal_angles))
-                          else 'Goal not found at ' + str(angle))
+                    return goal_angles
-            if goal_center_x is not None:
+            print('Goal not found at ', str(angle))
-                self.mover.set_head_angles(0, 0)
+
-                return goal_center_x
         self.mover.set_head_angles(0, 0)
         return None