Pavel Lutskov
e54858fbfd
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
366 lines
12 KiB
Python
366 lines
12 KiB
Python
from __future__ import print_function
|
|
from __future__ import division
|
|
|
|
from threading import Thread
|
|
from math import pi, tan, asin, atan, radians
|
|
from time import sleep, time, strftime
|
|
from collections import deque
|
|
|
|
from .imagereaders import NaoImageReader
|
|
from .finders import BallFinder, GoalFinder, FieldFinder
|
|
from .movements import NaoMover
|
|
from naoqi import ALProxy
|
|
|
|
|
|
class Striker(object):
|
|
|
|
def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
|
|
ball_min_radius):
|
|
|
|
# Timestamp
|
|
self.run_id = strftime('%Y%m%d%H%M%S')
|
|
|
|
# Motion
|
|
self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
|
|
self.is_over = False
|
|
|
|
# Sight
|
|
self.upper_camera = NaoImageReader(
|
|
nao_ip, port=nao_port, res=res, fps=10, cam_id=0,
|
|
)
|
|
self.lower_camera = NaoImageReader(
|
|
nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
|
|
)
|
|
|
|
# POV
|
|
self.upper_pov = NaoImageReader(
|
|
nao_ip, port=nao_port, res=1, fps=10, cam_id=0,
|
|
video_file='./cam0_' + self.run_id + '.avi'
|
|
)
|
|
|
|
self.lower_pov = NaoImageReader(
|
|
nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
|
|
video_file='./cam1_' + self.run_id + '.avi'
|
|
)
|
|
self.pov_thread = Thread(target=self._pov)
|
|
self.pov_thread.start()
|
|
|
|
# Recognition
|
|
self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
|
|
ball_min_radius)
|
|
self.field_finder = FieldFinder(tuple(field_hsv[0]),
|
|
tuple(field_hsv[1]))
|
|
self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
|
|
|
|
# Talking
|
|
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)
|
|
self.tts_thread.start()
|
|
|
|
# Debugging
|
|
self._timer_start = 0
|
|
self._timer_stop = 0
|
|
|
|
def close(self):
|
|
self.is_over = True
|
|
|
|
if self.tts_thread.isAlive():
|
|
self.tts_thread.join()
|
|
if self.pov_thread.isAlive():
|
|
self.pov_thread.join()
|
|
|
|
self.upper_camera.close()
|
|
self.lower_camera.close()
|
|
self.upper_pov.close()
|
|
self.lower_pov.close()
|
|
self.mover.stop_moving()
|
|
|
|
def _speaker(self):
|
|
while not self.is_over:
|
|
while self.speach_queue:
|
|
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):
|
|
while not self.is_over:
|
|
try:
|
|
self.upper_pov.get_frame()
|
|
self.lower_pov.get_frame()
|
|
sleep(0.1)
|
|
except RuntimeError as e:
|
|
print(e)
|
|
continue
|
|
|
|
def speak(self, text):
|
|
if not self.speach_history or self.speach_history[-1] != text:
|
|
self.speach_queue.appendleft(text)
|
|
self.speach_history.append(text)
|
|
|
|
def scan_rotation(self):
|
|
"""Intelligently rotate the robot to search for stuff."""
|
|
self.mover.stop_moving()
|
|
self.rotating = False
|
|
yaw, pitch = self.mover.get_head_angles()
|
|
|
|
# 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 yaw > 0.8:
|
|
self.mover.set_head_angles(-pi / 8, pitch, 0.5)
|
|
sleep(0.5)
|
|
elif yaw < -0.8:
|
|
self.mover.move_to_fast(0, 0, -pi / 4)
|
|
self.mover.wait()
|
|
# 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.mover.change_head_angles(sign * pi / 8, 0, 0.5)
|
|
sleep(0.3)
|
|
|
|
def get_ball_angles_from_camera(self, cam, mask=True):
|
|
"""Detect the ball and return its angles in camera coordinates."""
|
|
try:
|
|
frame = cam.get_frame()
|
|
except RuntimeError as e: # Sometimes camera doesn't return an image
|
|
print(e)
|
|
return None
|
|
|
|
if cam == self.lower_camera:
|
|
mask = False
|
|
|
|
if mask:
|
|
field = self.field_finder.find(frame)
|
|
frame = self.field_finder.mask_it(frame, field)
|
|
ball = self.ball_finder.find(frame)
|
|
|
|
if ball is None:
|
|
return None
|
|
|
|
(x, y), _ = ball
|
|
x, y = cam.to_relative(x, y)
|
|
x, y = cam.to_angles(x, y)
|
|
return x, y
|
|
|
|
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
|
|
print(e)
|
|
return None
|
|
|
|
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_l, goal_r = self.goal_finder.left_right_post(goal)
|
|
goal_c = self.goal_finder.goal_center(goal)
|
|
|
|
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):
|
|
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) - (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, hypo):
|
|
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.
|
|
|
|
Returns
|
|
-------
|
|
bool
|
|
True if robot is nicely aligned to ball; else False.
|
|
|
|
"""
|
|
|
|
ball_locked = False
|
|
while not ball_locked:
|
|
# visibility check
|
|
for i in range(3):
|
|
cams = [self.lower_camera, self.upper_camera]
|
|
in_sight = False
|
|
|
|
for cam in cams:
|
|
ball_angles = self.get_ball_angles_from_camera(cam)
|
|
if ball_angles is not None:
|
|
x, y = ball_angles
|
|
self._timer_start = time()
|
|
in_sight = True
|
|
|
|
break
|
|
if in_sight:
|
|
break
|
|
# stop visibility check
|
|
|
|
if not in_sight:
|
|
self.scan_rotation()
|
|
continue
|
|
|
|
ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
|
|
print()
|
|
|
|
def run_to_ball(self, d):
|
|
self.mover.move_to(d, 0, 0)
|
|
# self.mover.wait()
|
|
|
|
def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0):
|
|
"""Align robot to the ball.
|
|
|
|
If head is not centered at the ball (within tolerance), then
|
|
turn head to ball. If after that the angle of head to body
|
|
becomes too big, rotate the body by the head angle and
|
|
simultaneously rotate head into 0 position to achieve alignment.
|
|
"""
|
|
|
|
# only the x ball angle is relevant for alignment
|
|
d_yaw, d_pitch = ball_x, 0
|
|
print('ball yaw in camera:', d_yaw)
|
|
|
|
# center head at the ball
|
|
if (abs(d_yaw) > 0.01):
|
|
self.mover.change_head_angles(d_yaw, d_pitch,
|
|
min(1, abs(d_yaw) * 1.25))
|
|
sleep(0.1)
|
|
|
|
head_yaw, head_pitch = self.mover.get_head_angles()
|
|
self._timer_stop = time()
|
|
print('Ball to head', self._timer_stop - self._timer_start)
|
|
print('Head yaw', head_yaw, end=' ')
|
|
d_yaw = head_yaw - soll
|
|
print('Head d_yaw', d_yaw)
|
|
print('Allowed tolerance', tol)
|
|
|
|
if abs(d_yaw) > tol:
|
|
self.mover.stop_moving()
|
|
print('Going to rotate by', d_yaw)
|
|
self.mover.set_head_angles(soll, head_pitch, 0.3)
|
|
self.mover.move_to(0, 0, d_yaw)
|
|
self.mover.wait()
|
|
return False
|
|
else:
|
|
print('Ball locked')
|
|
return True
|
|
|
|
def align_to_ball(self):
|
|
ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
|
|
mask=False)
|
|
if ball_angles is None:
|
|
raise ValueError('No ball')
|
|
x, y = ball_angles
|
|
goal_x, goal_y = 0.095, 0.4
|
|
dx, dy = goal_x - x, goal_y - y
|
|
|
|
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
|
|
print('Moving to dxdy', dx, dy)
|
|
self.mover.move_to(dy, dx, 0)
|
|
self.mover.wait()
|
|
return False
|
|
|
|
def align_to_goal(self):
|
|
ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
|
|
mask=False)
|
|
if ball_angles is None:
|
|
self.mover.move_to(-0.1, 0, 0)
|
|
self.mover.wait()
|
|
ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
|
|
mask=False)
|
|
if ball_angles is None:
|
|
raise ValueError('No ball')
|
|
x, y = ball_angles
|
|
|
|
print(x, y)
|
|
self.turn_to_ball(x, y, tol=0.15)
|
|
sleep(0.2)
|
|
|
|
goal = self.goal_search()
|
|
if goal is None:
|
|
return False
|
|
|
|
gcl, gcr, gcc = goal
|
|
print('Goal:', gcl, gcr, gcc)
|
|
|
|
if gcl > 0 > gcr:
|
|
return True
|
|
|
|
if y > 0.35:
|
|
self.mover.move_to(-0.05, 0, 0)
|
|
self.mover.wait()
|
|
# return False
|
|
elif y < 0.25:
|
|
self.mover.move_to(0.05, 0, 0)
|
|
self.mover.wait()
|
|
# return False
|
|
|
|
sign = -1 if gcc > 0 else 1
|
|
if sign == 1:
|
|
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.3)
|
|
sleep(0.5)
|
|
for i in range(5):
|
|
print(i, goal_angles)
|
|
goal_angles = self.get_goal_angles_from_camera(
|
|
self.upper_camera
|
|
)
|
|
if goal_angles is not None:
|
|
goal_angles = tuple(gc + angle for gc in goal_angles)
|
|
self.mover.set_head_angles(0, 0)
|
|
print('Goal found:', str(goal_angles))
|
|
return goal_angles
|
|
print('Goal not found at ', str(angle))
|
|
|
|
self.mover.set_head_angles(0, 0)
|
|
return None
|