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

Approach from the side works somehow

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-29 18:02:57 +02:00
parent 93ed282170
commit ae9ec9fcce
2 changed files with 70 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
pykick/nao_defaults.json
View File

@@ -1,7 +1,7 @@
{ {
"ball": [ "ball": [
[ [
0, 174,
150, 150,
100 100
], ],
@@ -28,8 +28,8 @@
"ball_min_radius": 0.01, "ball_min_radius": 0.01,
"field": [ "field": [
[ [
33, 31,
63, 60,
60 60
], ],
[ [

104
pykick/striker.py
View File

@@ -2,8 +2,8 @@ from __future__ import print_function
from __future__ import division from __future__ import division
from threading import Thread from threading import Thread
from math import pi,tan,asin from math import pi, cos, tan, asin, radians
from time import sleep, time from time import sleep, time, strftime
from collections import deque from collections import deque
from .utils import read_config from .utils import read_config
@@ -15,8 +15,11 @@ from naoqi import ALProxy
class Striker(object): class Striker(object):
VIDEO_FOLDER = '/home/nao/recordings/'
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): ball_min_radius):
self.run_id = strftime('%Y%m%d%H%M%S')
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,7 +31,8 @@ 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.speaker = ALProxy("ALTextToSpeech", bytes(nao_ip), nao_port) self.speaker = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port)
self.recorder = ALProxy('ALVideoRecorder', bytes(nao_ip), nao_port)
self.is_over = False self.is_over = False
@@ -48,11 +52,21 @@ class Striker(object):
self.speaker.say(self.speach_queue.pop()) self.speaker.say(self.speach_queue.pop())
sleep(0.1) sleep(0.1)
def speak(self, text): def start_record(self, cam_id):
if not self.speach_queue or self.speach_queue[0] != text: if self.recorder.isRecording():
self.speach_queue.appendleft(text) print('Already recording. Please stop first')
return
self.recorder.setCameraID(cam_id)
self.recorder.startRecording(self.VIDEO_FOLDER,
'cam' + str(cam_id) + self.run_id)
def ball_scan(self): def stop_record(self):
self.recorder.stopRecording()
def speak(self, text):
self.speach_queue.appendleft(text)
def scan_rotation(self):
"""Intelligently rotate the robot to search for stuff.""" """Intelligently rotate the robot to search for stuff."""
self.mover.stop_moving() self.mover.stop_moving()
self.rotating = False self.rotating = False
@@ -122,7 +136,15 @@ class Striker(object):
return goal_x return goal_x
def distance_to_ball(self): def distance_to_ball(self):
return 0.5 angles = self.get_ball_angles_from_camera(self.upper_camera)
if angles == None:
raise ValueError('No ball in sight')
y_angle = angles[1]
y_angle = pi/2 - y_angle - radians(15)
return 0.5 * tan(y_angle)
def walking_direction(self, lr, d):
return asin(0.5 / d) * lr
def ball_tracking(self, soll=0, tol=0.15): 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.
@@ -147,25 +169,7 @@ 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
if cam==self.upper_camera and not self.dy:
#angles= self.video_top.to_angles(x,y)
angles=ball_angles
print("y (in radians) angle is:"+str(angles[1]))
# calculate distance to the ball
y_angle=angles[1]
y_angle=pi/2-y_angle-15*pi/180
distance = 0.5 * tan(y_angle)
# vertical distance to ball at the end
vert_dist=0.3
# calculate angle of the walk
angle_to_walk=asin(vert_dist/distance)
self.dy=1*tan(angle_to_walk)
print("Dy ist ----- "+str(self.dy))
print("Distance --------------- ="+str(distance))
print('Top camera\n')
break break
if in_sight: if in_sight:
break break
@@ -173,15 +177,16 @@ class Striker(object):
if not in_sight: if not in_sight:
self.speak('No ball visible search it') self.speak('No ball visible search it')
self.ball_scan() self.scan_rotation()
continue continue
# self.speak('I see the ball') # self.speak('I see the ball')
ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol) ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol)
print() print()
def run_to_ball(self): def run_to_ball(self, d):
self.mover.move_toward(1, min(2 * self.dy, 1), 0) self.mover.move_to(d, 0, 0)
self.mover.wait()
def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False, def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False,
m_delta=0.2): m_delta=0.2):
@@ -224,7 +229,6 @@ class Striker(object):
return False return False
else: else:
print('Ball locked') print('Ball locked')
# self.speak('Ball locked')
return True return True
else: else:
@@ -313,6 +317,8 @@ class Striker(object):
self.is_over = True self.is_over = True
if self.tts_thread.isAlive(): if self.tts_thread.isAlive():
self.tts_thread.join() self.tts_thread.join()
if self.recorder.isRecording():
self.stop_record()
self.upper_camera.close() self.upper_camera.close()
self.lower_camera.close() self.lower_camera.close()
self.mover.stop_moving() self.mover.stop_moving()
@@ -320,7 +326,8 @@ class Striker(object):
def goal_search(self): def goal_search(self):
goal_center_x = None goal_center_x = None
angles = [0, -pi/6, -pi/4, -pi/3, -pi/2, pi/6, pi/4, pi/3, pi/2] 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: for angle in angles:
self.mover.set_head_angles(angle, 0) self.mover.set_head_angles(angle, 0)
sleep(0.5) sleep(0.5)
@@ -399,7 +406,7 @@ if __name__ == '__main__':
ball_min_radius=cfg['ball_min_radius'], ball_min_radius=cfg['ball_min_radius'],
) )
state = 'tracking' state = 'init'
init_soll = 0.0 init_soll = 0.0
align_start = 0.15 align_start = 0.15
curve_start = -0.1 curve_start = -0.1
@@ -410,11 +417,21 @@ if __name__ == '__main__':
loop_start = time() loop_start = time()
print('State:', state) print('State:', state)
if state == 'tracking': if state == 'init':
striker.start_record(0)
striker.mover.set_head_angles(0, 0)
striker.ball_tracking(tol=0.05)
# goal_center = striker.goal_search()
# approach = 1 if goal_center < 0 else -1
approach = 1
state = 'ball_approach'
elif state == 'tracking':
# start ball approach when ball is visible # start ball approach when ball is visible
print('Soll angle') print('Soll angle')
striker.ball_tracking(tol=0.05) striker.ball_tracking(tol=0.05)
state = 'ball_approach' break
# state = 'ball_approach'
elif state == 'ball_approach': elif state == 'ball_approach':
# bil = striker.get_ball_angles_from_camera( # bil = striker.get_ball_angles_from_camera(
@@ -422,9 +439,20 @@ if __name__ == '__main__':
# ) # Ball in lower # ) # Ball in lower
# print('Ball in lower', bil) # print('Ball in lower', bil)
print('Continue running') try:
striker.run_to_ball() d = striker.distance_to_ball()
# state = 'tracking' except ValueError:
state = 'tracking'
continue
print('Distance to ball', d)
angle = striker.walking_direction(approach, d)
d_run = d * cos(angle)
print('Approach angle', angle)
striker.mover.move_to(0, 0, angle)
striker.mover.wait()
striker.run_to_ball(d_run)
striker.mover.move_to(0, 0, -pi/2 * approach)
state = 'tracking'
elif state == 'goal_align': elif state == 'goal_align':
try: try:
@@ -435,6 +463,8 @@ if __name__ == '__main__':
state = 'tracking' state = 'tracking'
elif state == 'align': elif state == 'align':
striker.stop_record()
striker.start_record(1)
striker.mover.set_head_angles(0, 0.25, 0.3) striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5) sleep(0.5)
try: try: