From b8ccb41ba7601afc713f8836cba311e6fa689760 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Wed, 13 Jun 2018 20:25:12 +0200 Subject: [PATCH] started work on goal alignment --- pykick/ball_approach.py | 126 ++++++++++++++++++++++++---------------- pykick/finders.py | 67 +++++++++++++++++++++ 2 files changed, 144 insertions(+), 49 deletions(-) diff --git a/pykick/ball_approach.py b/pykick/ball_approach.py index 06b8dc1..33f5009 100644 --- a/pykick/ball_approach.py +++ b/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 @@ -29,67 +30,89 @@ class BallFollower(object): mag = abs(yaw) sign = 1 if yaw >= 0 else -1 if mag > 2: - self.mover.move_to(0, 0, sign * pi / 12) + self.mover.move_to(0, 0, sign * pi / 12) else: self.mover.change_head_angles(sign * pi / 4, 0, 0.5) - def update(self): - #print('in update loop') + def get_ball_angles_from_camera(self, cam): try: - (x, y), radius = self.finder.find_colored_ball( - self.video_top.get_frame() + (x, y), _ = self.finder.find_colored_ball( + cam.get_frame() ) self.loss_counter = 0 - x, y = self.video_top.to_relative(x, y) - x, y = self.video_top.to_angles(x,y) - # print("y (in radians) angle is:"+str(angles[1])) - # 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') + x, y = cam.to_relative(x, y) + x, y = cam.to_angles(x, y) + return x, y 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( - self.video_bot.get_frame() - ) - x, y = self.video_bot.to_relative(x, y) - self.loss_counter = 0 - #print('Low camera') - except TypeError: - 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) + x, y = self.get_ball_angles_from_camera(self, cam) + in_sight = True + break + except ValueError: + pass - def process_coordinates(self, x, y): - # x_diff = x - 0.5 - # y_diff = y - 0.5 - # print("x_diff: " + str(x_diff)) - # print("y_diff: " + str(y_diff)) + if not in_sight: + print('No ball in sight') + self.loss_counter += 1 + if self.loss_counter > 5: + self.ball_scan() + return + + self.turn_to_ball(x, y) + + def run_after(self): + self.mover.move_to(0.3, 0, 0) + + def turn_to_ball(self, ball_x, ball_y): + d_yaw, d_pitch = ball_x, 0 + print('ball yaw', d_yaw) + + if (abs(d_yaw) > 0.01): + self.mover.change_head_angles(d_yaw, d_pitch, + abs(d_yaw) / 2) + sleep(1) + self.mover.wait() - d_yaw, d_pitch = x, 0 - print(d_yaw) - - # dont move the head, when the angle is below a threshold - # otherwise function would raise an error and stop - if (abs(d_yaw)>=0.00001): - self.mover.change_head_angles(d_yaw * 0.7, d_pitch, - abs(d_yaw) / 2) - - # self.counter = 0 yaw = self.mover.get_head_angles()[0] - if abs(yaw) > 0.4: - # self.counter = 0 + print('head yaw', yaw) + 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() diff --git a/pykick/finders.py b/pykick/finders.py index fe4b0b2..dbaa82f 100644 --- a/pykick/finders.py +++ b/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):