started work on goal alignment
This commit is contained in:
@@ -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()
|
||||
|
||||
@@ -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):
|
||||
|
||||
Reference in New Issue
Block a user