started work on goal alignment

pykick/ball_approach.py

@@ -1,7 +1,8 @@
 from __future__ import print_function
 from __future__ import division
 
-from math import tan, pi
+from math import pi
+from time import sleep
 
 from .utils import read_config
 from .imagereaders import NaoImageReader
@@ -18,7 +19,7 @@ class BallFollower(object):
         self.video_top = NaoImageReader(nao_ip, port=nao_port, res=res,
                                         fps=30, cam_id=0)
         self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res,
-                                        fps=30, cam_id=0)
+                                        fps=30, cam_id=1)
         self.finder = BallFinder(hsv_lower, hsv_upper, min_radius, None)
         self.lock_counter = 0
         self.loss_counter = 0
@@ -33,63 +34,85 @@ class BallFollower(object):
         else:
             self.mover.change_head_angles(sign * pi / 4, 0, 0.5)
 
-    def update(self):
+    def get_ball_angles_from_camera(self, cam):
-        #print('in update loop')
         try:
-            (x, y), radius = self.finder.find_colored_ball(
+            (x, y), _ = self.finder.find_colored_ball(
-                self.video_top.get_frame()
+                cam.get_frame()
             )
             self.loss_counter = 0
-            x, y = self.video_top.to_relative(x, y)
+            x, y = cam.to_relative(x, y)
-            x, y = self.video_top.to_angles(x,y)
+            x, y = cam.to_angles(x, y)
-            # print("y (in radians) angle is:"+str(angles[1]))
+            return x, y
-            # y_angle=angles[1]
-            # y_angle=pi/2-y_angle-15*pi/180
-            # distance = 0.5 * tan(y_angle)
-            # print("Distance="+str(distance))
-            # print('Top camera\n')
         except TypeError:
+            raise ValueError('Ball not found')
+
+
+    def ball_tracking(self):
+        cams = [self.video_top, self.video_bot]
+        in_sight = False
+        for cam in cams:
             try:
-                (x, y), radius = self.finder.find_colored_ball(
+                x, y = self.get_ball_angles_from_camera(self, cam)
-                    self.video_bot.get_frame()
+                in_sight = True
-                )
+                break
-                x, y = self.video_bot.to_relative(x, y)
+            except ValueError:
-                self.loss_counter = 0
+                pass
-                #print('Low camera')
+
-            except TypeError:
+        if not in_sight:
             print('No ball in sight')
             self.loss_counter += 1
             if self.loss_counter > 5:
                 self.ball_scan()
             return
-        #print(x, y)
-        self.process_coordinates(x, y)
 
-    def process_coordinates(self, x, y):
+        self.turn_to_ball(x, y)
-        # x_diff = x - 0.5
-        # y_diff = y - 0.5
-        # print("x_diff: " + str(x_diff))
-        # print("y_diff: " + str(y_diff))
 
-        d_yaw, d_pitch = x, 0
+    def run_after(self):
-        print(d_yaw)
+        self.mover.move_to(0.3, 0, 0)
 
-	# dont move the head, when the angle is below a threshold
+    def turn_to_ball(self, ball_x, ball_y):
-	# otherwise function would raise an error and stop
+        d_yaw, d_pitch = ball_x, 0
-	if (abs(d_yaw)>=0.00001):
+        print('ball yaw', d_yaw)
-		self.mover.change_head_angles(d_yaw * 0.7, d_pitch,
+
+        if (abs(d_yaw) > 0.01):
+            self.mover.change_head_angles(d_yaw, d_pitch,
                                           abs(d_yaw) / 2)
+            sleep(1)
+            self.mover.wait()
 
-        # self.counter = 0
         yaw = self.mover.get_head_angles()[0]
-        if abs(yaw) > 0.4:
+        print('head yaw', yaw)
-            # self.counter = 0
+        if abs(yaw) > 0.05:
             print('Going to rotate')
             self.mover.set_head_angles(0, 0, 0.5)
             self.mover.move_to(0, 0, yaw)
             self.mover.wait()
-        if self.run_after:
+
-            self.mover.move_to(0.3, 0, 0)
+    def align_to_goal(self):
+        try:
+            x, y = self.get_ball_angles_from_camera(self.video_bot)
+            print(x, y)
+            if abs(x) > 0.05:
+                self.turn_to_ball(x, y)
+                return
+        except Exception as e:
+            print(e)
+            print('No ball')
+            sleep(0.1)
+            return
+
+        print('moving')
+        increment = 0.1
+        # if y < -pi / 8:
+            # self.mover.move_to(-0.1, 0, 0)
+        if y > 0.35:
+            self.mover.move_to(-0.05, 0, 0)
+        elif y < 0.25:
+            self.mover.move_to(0.05, 0, 0)
+        self.mover.wait()
+        self.mover.move_to(0, increment, 0)
+        self.mover.wait()
+
 
     def close(self):
         self.mover.rest()
@@ -111,6 +134,11 @@ if __name__ == '__main__':
     )
     try:
         while True:
-            follower.update()
+            try:
+                print(follower.get_ball_angles_from_camera(
+                    follower.video_bot)[1])
+            except Exception as e:
+                print(e)
+
     finally:
         follower.close()

pykick/finders.py

@@ -3,6 +3,73 @@ from collections import deque
 import cv2
 
 
+class GoalFinder(object):
+
+    def __init__(self, hsv_lower, hsv_upper):
+
+        self.hsv_lower = hsv_lower
+        self.hsv_upper = hsv_upper
+
+    def goal_similarity(self, contour):
+        contour = contour.reshape((-1, 2))
+        hull = cv2.convexHull(contour).reshape((-1, 2))
+        len_h = cv2.arcLength(hull, True)
+
+        # Wild assumption that the goal should lie close to its
+        # enclosing convex hull
+        shape_sim = np.linalg.norm(contour[:,None] - hull,
+                                   axis=2).min(axis=1).sum() / len_h
+
+        # Wild assumption that the area of the goal is rather small
+        # compared to its enclosing convex hull
+        area_c = cv2.contourArea(contour)
+        area_h = cv2.contourArea(hull)
+
+        area_sim = area_c / area_h
+
+        # Final similarity score is just the sum of both
+        final_score = shape_sim + area_sim
+        print(shape_sim, area_sim, final_score)
+        return final_score
+
+    def find_goal_contour(self, frame)
+        thr = 
+
+        # The ususal
+        thr = cv2.erode(thr, None, iterations=2)
+        thr = cv2.dilate(thr, None, iterations=2)
+        cnts, _ = cv2.findContours(thr, cv2.RETR_EXTERNAL,
+                                            cv2.CHAIN_APPROX_SIMPLE)
+        areas = np.array([cv2.contourArea(cnt) for cnt in cnts])
+        # Candidates are at most 6 biggest white areas
+        top_x = 6
+        if len(areas) > top_x:
+            cnt_ind = np.argpartition(areas, -top_x)[-top_x:]
+            cnts = [cnts[i] for i in cnt_ind]
+
+        perimeters = np.array([cv2.arcLength(cnt, True) for cnt in cnts])
+        epsilon = 0.01 * perimeters
+
+        # Approximate resulting contours with simpler lines
+        cnts = [cv2.approxPolyDP(cnt, eps, True)
+                for cnt, eps in zip(cnts, epsilon)]
+
+        # Goal needs normally 8 points for perfect approximation
+        # But with 6 can also be approximated
+        good_cnts = [cnt for cnt in cnts if 6 <= cnt.shape[0] <= 9
+                    and not cv2.isContourConvex(cnt)]
+
+        if not good_cnts:
+            return None
+
+        similarities = [self.goal_similarity(cnt) for cnt in good_cnts]
+        best = min(similarities)
+        if best > 0.4:
+            return None
+        # Find the contour with the shape closest to that of the goal
+        goal = good_cnts[similarities.index(best)]
+
+
 class BallFinder(object):
 
     def __init__(self, hsv_lower, hsv_upper, min_radius, viz=False):