pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Disabled fancy tracking, improved talking

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-27 17:36:24 +02:00
parent e965be5a3f
commit d0a127aec0
2 changed files with 97 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
pykick/nao_defaults.json
View File

@@ -2,15 +2,16 @@
"ball": [ "ball": [
[ [
0, 0,
175, 150,
100 100
], ],
[ [
6, 8,
255, 255,
255 255
] ]
], ],
"cam": 1,
"goal": [ "goal": [
[ [
0, 0,
@@ -23,7 +24,6 @@
255 255
] ]
], ],
"fps": 10,
"res": 2, "res": 2,
"ball_min_radius": 0.01, "ball_min_radius": 0.01,
"field": [ "field": [
@@ -38,7 +38,7 @@
255 255
] ]
], ],
"cam": 1, "fps": 10,
"ip": "192.168.0.11", "ip": "192.168.0.11",
"port": 9559 "port": 9559
} }

181
pykick/striker.py
View File

@@ -5,6 +5,7 @@ import argparse
from threading import Thread from threading import Thread
from math import pi from math import pi
from time import sleep, time from time import sleep, time
from collections import deque
from .utils import read_config from .utils import read_config
from .imagereaders import NaoImageReader from .imagereaders import NaoImageReader
@@ -16,7 +17,7 @@ from naoqi import ALProxy
class Striker(object): class Striker(object):
def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv, def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
ball_min_radius, run_after): ball_min_radius):
self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
self.mover.stand_up() self.mover.stand_up()
self.upper_camera = NaoImageReader(nao_ip, port=nao_port, res=res, self.upper_camera = NaoImageReader(nao_ip, port=nao_port, res=res,
@@ -28,35 +29,28 @@ class Striker(object):
self.field_finder = FieldFinder(tuple(field_hsv[0]), self.field_finder = FieldFinder(tuple(field_hsv[0]),
tuple(field_hsv[1])) tuple(field_hsv[1]))
self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1])) self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
self.lock_counter = 0
self.loss_counter = 0
self.run_after = run_after
self.in_move = False
self.speaker = ALProxy("ALTextToSpeech", bytes(nao_ip), nao_port) self.speaker = ALProxy("ALTextToSpeech", bytes(nao_ip), nao_port)
self.tts_thread = None self.is_over = False
self.last_speak = None
self.speach_queue = deque(maxlen=4)
self.tts_thread = Thread(target=self._speaker)
self.tts_thread.start()
self.rotating = False self.rotating = False
self.rot_dir = 0 self.rot_dir = 0
self.timer_start = 0 self.timer_start = 0
self.timer_stop = 0 self.timer_stop = 0
def _speaker(self):
while not self.is_over:
while self.speach_queue:
self.speaker.say(self.speach_queue.pop())
sleep(0.1)
def speak(self, text): def speak(self, text):
if ( if not self.speach_queue or self.speach_queue[0] != text:
(self.tts_thread is None or not self.tts_thread.isAlive()) self.speach_queue.appendleft(text)
and text != self.last_speak
):
if (
self.last_speak == "Where is the ball? I am searching for it"
and text == "Going to rotate"
):
text = "I have found the ball"
self.tts_thread = Thread(
target=lambda text: self.speaker.say(str(text)),
args=(text,)
)
self.tts_thread.start()
self.last_speak = text
def ball_scan(self): def ball_scan(self):
"""Intelligently rotate the robot to search for stuff.""" """Intelligently rotate the robot to search for stuff."""
@@ -74,7 +68,7 @@ class Striker(object):
self.mover.set_head_angles(-pi / 8, 0, 0.5) self.mover.set_head_angles(-pi / 8, 0, 0.5)
sleep(0.5) sleep(0.5)
elif mag < -0.8: elif mag < -0.8:
self.mover.move_to(0, 0, -pi / 6) self.mover.move_to_fast(0, 0, -pi / 4)
self.mover.wait() self.mover.wait()
#self.speak("Where is the ball? I am searching for it") #self.speak("Where is the ball? I am searching for it")
# rotate head to the left, if head yaw angle is equally zero or larger # rotate head to the left, if head yaw angle is equally zero or larger
@@ -130,7 +124,7 @@ class Striker(object):
def distance_to_ball(self): def distance_to_ball(self):
return 0.5 return 0.5
def ball_tracking(self, soll=0): def ball_tracking(self, soll=0, tol=0.15):
"""Track the ball using the feed from top and bottom camera. """Track the ball using the feed from top and bottom camera.
Returns Returns
@@ -153,24 +147,25 @@ class Striker(object):
x, y = ball_angles x, y = ball_angles
self.timer_start = time() self.timer_start = time()
in_sight = True in_sight = True
self.loss_counter = 0
break break
if in_sight: if in_sight:
break break
# stop visibility check # stop visibility check
if not in_sight: if not in_sight:
self.speak('No ball visible search it')
self.ball_scan() self.ball_scan()
continue continue
ball_locked = self.turn_to_ball(x, y, soll=soll) # self.speak('I see the ball')
ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
print() print()
return True
def run_to_ball(self): def run_to_ball(self):
self.mover.move_to(1, 0, 0) self.mover.move_to_fast(1, 0, 0)
def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, m_delta=0.2): def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False,
m_delta=0.2):
"""Align robot to the ball. """Align robot to the ball.
If head is not centered at the ball (within tolerance), then If head is not centered at the ball (within tolerance), then
@@ -187,38 +182,52 @@ class Striker(object):
if (abs(d_yaw) > 0.01): if (abs(d_yaw) > 0.01):
self.mover.change_head_angles(d_yaw, d_pitch, self.mover.change_head_angles(d_yaw, d_pitch,
min(1, abs(d_yaw) * 1.25)) min(1, abs(d_yaw) * 1.25))
sleep(0.05) sleep(0.1)
yaw = self.mover.get_head_angles()[0] head_yaw, head_pitch = self.mover.get_head_angles()
self.timer_stop = time() self.timer_stop = time()
print('Ball to head', self.timer_stop - self.timer_start) print('Ball to head', self.timer_stop - self.timer_start)
print('head yaw', yaw, end=' ') print('Head yaw', head_yaw, end=' ')
d_yaw = yaw - soll d_yaw = head_yaw - soll
print('head d_yaw', d_yaw) print('Head d_yaw', d_yaw)
print('Rotating', self.rotating, end=' ') print('Rotating', self.rotating, end=' ')
print('Rotation direction', self.rot_dir, end=' ') print('Rotation direction', self.rot_dir, end=' ')
print('Allowed tolerance', tol) print('Allowed tolerance', tol)
# align body with the head if not fancy:
if not self.rotating:
if abs(d_yaw) > tol: if abs(d_yaw) > tol:
self.mover.stop_moving() self.mover.stop_moving()
self.rotating = True print('Going to rotate by', d_yaw)
self.rot_dir = -1 if d_yaw > 0 else 1 self.speak('Going to rotate')
print('Going to rotate') self.mover.set_head_angles(soll, head_pitch, 0.3)
self.speak("Going to rotate") self.mover.move_to(0, 0, d_yaw)
self.mover.move_toward(0, 0, -self.rot_dir) self.mover.wait()
sleep(0.8)
return False return False
else: else:
print('Success') print('Ball locked')
self.speak('Ball locked') # self.speak('Ball locked')
return True return True
else: else:
if d_yaw * self.rot_dir > -tol - m_delta: if not self.rotating:
self.rotating = False if abs(d_yaw) > tol:
self.mover.stop_moving() self.mover.stop_moving()
return False self.rotating = True
self.rot_dir = -1 if d_yaw > 0 else 1
print('Going to rotate')
self.speak("Going to rotate")
self.mover.move_toward(0, 0, -self.rot_dir)
# sleep(1.5)
return False
else:
print('Success')
# self.speak('Ball locked')
return True
else:
if d_yaw * self.rot_dir > -tol - m_delta:
self.rotating = False
self.mover.stop_moving()
return False
def align_to_ball(self): def align_to_ball(self):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera, ball_angles = self.get_ball_angles_from_camera(self.lower_camera,
@@ -232,10 +241,10 @@ class Striker(object):
print(x, y) print(x, y)
return True return True
if abs(dx) > 0.05: if abs(dx) > 0.05:
self.mover.move_to(0, -dx * 0.2, 0) self.mover.move_to_fast(0, -dx * 0.2, 0)
self.mover.wait() self.mover.wait()
if abs(dy) > 0.05: if abs(dy) > 0.05:
self.mover.move_to(dy * 0.3, 0, 0) self.mover.move_to_fast(dy * 0.3, 0, 0)
self.mover.wait() self.mover.wait()
return False return False
@@ -255,8 +264,9 @@ class Striker(object):
print(x, y) print(x, y)
self.speak("Turn to ball") self.speak("Turn to ball")
self.turn_to_ball(x, y, tol=0.15) self.turn_to_ball(x, y, tol=0.15)
self.speak('I am trying to find the goal') self.speak('Trying to find the goal')
goal_center_x = self.goal_search() goal_center_x = self.goal_search()
self.speak('Goal found')
print('Goal center:', goal_center_x) print('Goal center:', goal_center_x)
if goal_center_x is not None and abs(goal_center_x) < 0.1: if goal_center_x is not None and abs(goal_center_x) < 0.1:
@@ -266,27 +276,24 @@ class Striker(object):
return True return True
if y > 0.35: if y > 0.35:
self.speak("moving backward")
self.mover.move_to(-0.05, 0, 0) self.mover.move_to(-0.05, 0, 0)
self.mover.wait() self.mover.wait()
# return False # return False
elif y < 0.25: elif y < 0.25:
self.speak("moving forward")
self.mover.move_to(0.05, 0, 0) self.mover.move_to(0.05, 0, 0)
self.mover.wait() self.mover.wait()
# return False # return False
sign = -1 if goal_center_x > 0 else 1 sign = -1 if goal_center_x > 0 else 1
num_steps = int(min(abs(goal_center_x), 0.1) // 0.05) num_steps = int(min(abs(goal_center_x), 0.1) // 0.05)
self.speak("Moving sideways")
print('Moving sideways')
for _ in range(num_steps): for _ in range(num_steps):
self.mover.move_to(0, 0.05 * sign, 0) self.mover.move_to(0, 0.05 * sign, 0)
self.mover.wait() self.mover.wait()
print('Finished moving')
return False return False
def close(self): def close(self):
self.is_over = True
self.tts_thread.join()
self.mover.rest() self.mover.rest()
self.upper_camera.close() self.upper_camera.close()
self.lower_camera.close() self.lower_camera.close()
@@ -372,7 +379,6 @@ if __name__ == '__main__':
goal_hsv=cfg['goal'], goal_hsv=cfg['goal'],
field_hsv=cfg['field'], field_hsv=cfg['field'],
ball_min_radius=cfg['ball_min_radius'], ball_min_radius=cfg['ball_min_radius'],
run_after=False
) )
# allow additional arguments when running the function like # allow additional arguments when running the function like
@@ -407,26 +413,22 @@ if __name__ == '__main__':
else: else:
try: # Hit Ctrl-C to stop, cleanup and exit try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking' state = 'tracking'
start_soll = 0.8 init_soll = 0.0
align_start = 0.29 align_start = 0.15
curve_start = -0.1 curve_start = -0.1
curve_stop = 0.1 curve_stop = 0.1
soll = start_soll soll = init_soll
# t = None
while True: while True:
# meassure time for debbuging # meassure time for debbuging
loop_start = time() loop_start = time()
print('State:', state) print('State:', state)
# striker.speak(str(state))
if state == 'tracking': if state == 'tracking':
# start ball approach when ball is visible # start ball approach when ball is visible
print('Soll angle', soll) print('Soll angle', soll)
if striker.ball_tracking(soll=soll): striker.ball_tracking(soll=soll, tol=0.20)
striker.speak("Ball approach") state = 'ball_approach'
state = 'ball_approach'
# striker.speak('I got the ball')
# sleep(10)
elif state == 'ball_approach': elif state == 'ball_approach':
bil = striker.get_ball_angles_from_camera( bil = striker.get_ball_angles_from_camera(
@@ -434,35 +436,25 @@ if __name__ == '__main__':
) # Ball in lower ) # Ball in lower
print('Ball in lower', bil) print('Ball in lower', bil)
if bil is not None: if bil is not None:
if bil[1] > curve_start and bil[1] < curve_stop: if bil[1] > curve_start:
soll = 0
# if bil[1] > curve_start and bil[1] < curve_stop:
striker.speak('Going linear') striker.speak('Going linear')
soll = (-start_soll / (curve_stop - curve_start) # soll = (-init_soll / (curve_stop - curve_start)
* bil[1] # * bil[1] +
+ start_soll # init_soll
/ (curve_stop - curve_start) * curve_stop) # / (curve_stop - curve_start) * curve_stop)
elif bil[1] > align_start: if bil[1] > align_start:
print('Ball is close enough, stop approach') print('Ball is close enough, stop approach')
striker.mover.stop_moving() striker.mover.stop_moving()
striker.speak('Align to goal') striker.speak('Aligning to ball now')
state = 'align' state = 'simple_kick'
continue continue
print('Continue running') print('Continue running')
striker.run_to_ball() striker.run_to_ball()
state = 'tracking' state = 'tracking'
# elif state == 'simple_kick':
# striker.mover.set_head_angles(0,0.25,0.3)
# print('Doing the simple kick')
# just walk a short distance forward, ball should be near
# and it will probably be kicked in the right direction
# striker.speak("Simple Kick")
# striker.mover.move_to(0.3, 0, 0)
# striker.mover.wait()
# state = 'tracking'
elif state == 'goal_align': elif state == 'goal_align':
# print(striker.ball_and_goal_search())
try: try:
if striker.align_to_goal(): if striker.align_to_goal():
striker.speak('I am aligning to ball') striker.speak('I am aligning to ball')
@@ -483,9 +475,22 @@ if __name__ == '__main__':
striker.mover.set_head_angles(0, 0, 0.3) striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking' state = 'tracking'
elif state == 'simple_kick':
striker.mover.set_head_angles(0,0.25,0.3)
striker.ball_tracking(tol=0.10, soll=0)
print('Doing the simple kick')
# just walk a short distance forward, ball should be near
# and it will probably be kicked in the right direction
striker.speak("Simple Kick")
sleep(1)
striker.mover.move_to_fast(0.5, 0, 0)
striker.mover.wait()
break
# state = 'tracking'
elif state == 'kick': elif state == 'kick':
print('KICK!') print('KICK!')
striker.speak('I am going. To kick ass')
striker.mover.stand_up() striker.mover.stand_up()
sleep(0.3) sleep(0.3)
striker.mover.kick(fancy=True, foot='L') striker.mover.kick(fancy=True, foot='L')