Moved behaviour into top level package
This commit is contained in:
2
.gitignore
vendored
2
.gitignore
vendored
@@ -1,6 +1,8 @@
|
|||||||
*.sw*
|
*.sw*
|
||||||
*.pyc
|
*.pyc
|
||||||
*.jpg
|
*.jpg
|
||||||
|
*.avi
|
||||||
|
*.mpg
|
||||||
*~
|
*~
|
||||||
~*
|
~*
|
||||||
exp_*
|
exp_*
|
||||||
|
|||||||
172
pykick/__main__.py
Normal file
172
pykick/__main__.py
Normal file
@@ -0,0 +1,172 @@
|
|||||||
|
from __future__ import print_function, division
|
||||||
|
|
||||||
|
from time import time, sleep
|
||||||
|
from math import cos, pi
|
||||||
|
|
||||||
|
from .striker import Striker
|
||||||
|
from .utils import read_config
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# ____________________ STRIKER __________________________
|
||||||
|
#
|
||||||
|
# +----> Ball tracking (see below) <-------------+
|
||||||
|
# | |
|
||||||
|
# | | |
|
||||||
|
# | | |
|
||||||
|
# | v |
|
||||||
|
# | Ball in lower cam? |
|
||||||
|
# | / \ |
|
||||||
|
# lost | yes / \ cannot do |
|
||||||
|
# ball | v v |
|
||||||
|
# +-- Goal align Ball is only in top camera --+
|
||||||
|
# | Move closer.
|
||||||
|
# |
|
||||||
|
# successful |
|
||||||
|
# v
|
||||||
|
# Kick it! (Fancy or simple)
|
||||||
|
#
|
||||||
|
# _______________________________________________________
|
||||||
|
|
||||||
|
# ____________________ TRACKING _________________________
|
||||||
|
#
|
||||||
|
# yes
|
||||||
|
# check if ball visible ---> rotate head to the ball
|
||||||
|
# ^ | |
|
||||||
|
# | | no |
|
||||||
|
# | v |
|
||||||
|
# +--- ball scan rotation |
|
||||||
|
# | |
|
||||||
|
# | no V
|
||||||
|
# | +---------- already rotating body?
|
||||||
|
# | | |
|
||||||
|
# | v | yes
|
||||||
|
# | head angle too big? v
|
||||||
|
# | / \ head angle
|
||||||
|
# | yes / \ no is below threshold?
|
||||||
|
# | v v | |
|
||||||
|
# | stop successful | no | yes
|
||||||
|
# | moving exit | v
|
||||||
|
# +----- and start | stop rotating body
|
||||||
|
# | rotating body | |
|
||||||
|
# | | |
|
||||||
|
# +---------------------------------+---------+
|
||||||
|
#
|
||||||
|
# _______________________________________________________
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
|
||||||
|
try: # Hit Ctrl-C to stop, cleanup and exit
|
||||||
|
cfg = read_config()
|
||||||
|
striker = Striker(
|
||||||
|
nao_ip=cfg['ip'], nao_port=cfg['port'],
|
||||||
|
res=cfg['res'], ball_hsv=cfg['ball'],
|
||||||
|
goal_hsv=cfg['goal'], field_hsv=cfg['field'],
|
||||||
|
ball_min_radius=cfg['ball_min_radius'],
|
||||||
|
)
|
||||||
|
|
||||||
|
state = 'init'
|
||||||
|
init_soll = 0.0
|
||||||
|
align_start = 0.15
|
||||||
|
curve_start = -0.1
|
||||||
|
curve_stop = 0.1
|
||||||
|
soll = init_soll
|
||||||
|
striker.speak("Initialized")
|
||||||
|
while True:
|
||||||
|
# meassure time for debbuging
|
||||||
|
loop_start = time()
|
||||||
|
print('State:', state)
|
||||||
|
|
||||||
|
if state == 'init':
|
||||||
|
striker.mover.set_head_angles(0, 0)
|
||||||
|
striker.speak("Start the Ball tracking")
|
||||||
|
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
|
||||||
|
print('Soll angle')
|
||||||
|
striker.ball_tracking(tol=0.05)
|
||||||
|
# break
|
||||||
|
state = 'align'
|
||||||
|
|
||||||
|
elif state == 'ball_approach':
|
||||||
|
# bil = striker.get_ball_angles_from_camera(
|
||||||
|
# striker.lower_camera
|
||||||
|
# ) # Ball in lower
|
||||||
|
|
||||||
|
# print('Ball in lower', bil)
|
||||||
|
striker.speak("I have found the ball. Starting ball approach")
|
||||||
|
try:
|
||||||
|
d = striker.distance_to_ball()
|
||||||
|
except ValueError:
|
||||||
|
state = 'tracking'
|
||||||
|
continue
|
||||||
|
print('Distance to ball', d)
|
||||||
|
striker.speak("The distance to the ball is approximately "+str(round(d,2))+" Meters")
|
||||||
|
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.speak("I think I have reached the ball. I will start rotating")
|
||||||
|
striker.mover.move_to(0, 0, -pi/2 * approach)
|
||||||
|
state = 'tracking'
|
||||||
|
|
||||||
|
elif state == 'goal_align':
|
||||||
|
try:
|
||||||
|
if striker.align_to_goal():
|
||||||
|
striker.speak('I am aligning to ball')
|
||||||
|
state = "align"
|
||||||
|
except ValueError:
|
||||||
|
state = 'tracking'
|
||||||
|
|
||||||
|
elif state == 'align':
|
||||||
|
striker.speak("I will try now to align to the ball")
|
||||||
|
striker.mover.set_head_angles(0, 0.25, 0.3)
|
||||||
|
sleep(0.5)
|
||||||
|
try:
|
||||||
|
success = striker.align_to_ball()
|
||||||
|
sleep(0.3)
|
||||||
|
if success:
|
||||||
|
striker.speak('Hasta la vista, Baby')
|
||||||
|
state = 'kick'
|
||||||
|
except ValueError:
|
||||||
|
pass
|
||||||
|
# striker.mover.set_head_angles(0, 0, 0.3)
|
||||||
|
# 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
|
||||||
|
|
||||||
|
elif state == 'kick':
|
||||||
|
print('KICK!')
|
||||||
|
striker.mover.stand_up()
|
||||||
|
sleep(0.3)
|
||||||
|
striker.mover.kick(fancy=True, foot='L')
|
||||||
|
striker.speak("Nice kick. Let's do a dance")
|
||||||
|
striker.mover.dance()
|
||||||
|
break
|
||||||
|
|
||||||
|
loop_end = time()
|
||||||
|
print('Loop time:', loop_end - loop_start)
|
||||||
|
print('\n\n')
|
||||||
|
finally:
|
||||||
|
striker.close()
|
||||||
|
striker.mover.rest()
|
||||||
@@ -2,11 +2,10 @@ 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, cos, tan, asin, radians
|
from math import pi, tan, asin, radians
|
||||||
from time import sleep, time, strftime
|
from time import sleep, time, strftime
|
||||||
from collections import deque
|
from collections import deque
|
||||||
|
|
||||||
from .utils import read_config
|
|
||||||
from .imagereaders import NaoImageReader
|
from .imagereaders import NaoImageReader
|
||||||
from .finders import BallFinder, GoalFinder, FieldFinder
|
from .finders import BallFinder, GoalFinder, FieldFinder
|
||||||
from .movements import NaoMover
|
from .movements import NaoMover
|
||||||
@@ -334,167 +333,3 @@ class Striker(object):
|
|||||||
return goal_center_x
|
return goal_center_x
|
||||||
self.mover.set_head_angles(0, 0)
|
self.mover.set_head_angles(0, 0)
|
||||||
return None
|
return None
|
||||||
|
|
||||||
|
|
||||||
# ____________________ STRIKER __________________________
|
|
||||||
#
|
|
||||||
# +----> Ball tracking (see below) <-------------+
|
|
||||||
# | |
|
|
||||||
# | | |
|
|
||||||
# | | |
|
|
||||||
# | v |
|
|
||||||
# | Ball in lower cam? |
|
|
||||||
# | / \ |
|
|
||||||
# lost | yes / \ cannot do |
|
|
||||||
# ball | v v |
|
|
||||||
# +-- Goal align Ball is only in top camera --+
|
|
||||||
# | Move closer.
|
|
||||||
# |
|
|
||||||
# successful |
|
|
||||||
# v
|
|
||||||
# Kick it! (Fancy or simple)
|
|
||||||
#
|
|
||||||
# _______________________________________________________
|
|
||||||
|
|
||||||
# ____________________ TRACKING _________________________
|
|
||||||
#
|
|
||||||
# yes
|
|
||||||
# check if ball visible ---> rotate head to the ball
|
|
||||||
# ^ | |
|
|
||||||
# | | no |
|
|
||||||
# | v |
|
|
||||||
# +--- ball scan rotation |
|
|
||||||
# | |
|
|
||||||
# | no V
|
|
||||||
# | +---------- already rotating body?
|
|
||||||
# | | |
|
|
||||||
# | v | yes
|
|
||||||
# | head angle too big? v
|
|
||||||
# | / \ head angle
|
|
||||||
# | yes / \ no is below threshold?
|
|
||||||
# | v v | |
|
|
||||||
# | stop successful | no | yes
|
|
||||||
# | moving exit | v
|
|
||||||
# +----- and start | stop rotating body
|
|
||||||
# | rotating body | |
|
|
||||||
# | | |
|
|
||||||
# +---------------------------------+---------+
|
|
||||||
#
|
|
||||||
# _______________________________________________________
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
|
|
||||||
try: # Hit Ctrl-C to stop, cleanup and exit
|
|
||||||
cfg = read_config()
|
|
||||||
striker = Striker(
|
|
||||||
nao_ip=cfg['ip'], nao_port=cfg['port'],
|
|
||||||
res=cfg['res'], ball_hsv=cfg['ball'],
|
|
||||||
goal_hsv=cfg['goal'], field_hsv=cfg['field'],
|
|
||||||
ball_min_radius=cfg['ball_min_radius'],
|
|
||||||
)
|
|
||||||
|
|
||||||
state = 'init'
|
|
||||||
init_soll = 0.0
|
|
||||||
align_start = 0.15
|
|
||||||
curve_start = -0.1
|
|
||||||
curve_stop = 0.1
|
|
||||||
soll = init_soll
|
|
||||||
striker.speak("Initialized")
|
|
||||||
while True:
|
|
||||||
# meassure time for debbuging
|
|
||||||
loop_start = time()
|
|
||||||
print('State:', state)
|
|
||||||
|
|
||||||
if state == 'init':
|
|
||||||
striker.mover.set_head_angles(0, 0)
|
|
||||||
striker.speak("Start the Ball tracking")
|
|
||||||
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
|
|
||||||
print('Soll angle')
|
|
||||||
striker.ball_tracking(tol=0.05)
|
|
||||||
# break
|
|
||||||
state = 'align'
|
|
||||||
|
|
||||||
elif state == 'ball_approach':
|
|
||||||
# bil = striker.get_ball_angles_from_camera(
|
|
||||||
# striker.lower_camera
|
|
||||||
# ) # Ball in lower
|
|
||||||
|
|
||||||
# print('Ball in lower', bil)
|
|
||||||
striker.speak("I have found the ball. Starting ball approach")
|
|
||||||
try:
|
|
||||||
d = striker.distance_to_ball()
|
|
||||||
except ValueError:
|
|
||||||
state = 'tracking'
|
|
||||||
continue
|
|
||||||
print('Distance to ball', d)
|
|
||||||
striker.speak("The distance to the ball is approximately "+str(round(d,2))+" Meters")
|
|
||||||
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.speak("I think I have reached the ball. I will start rotating")
|
|
||||||
striker.mover.move_to(0, 0, -pi/2 * approach)
|
|
||||||
state = 'tracking'
|
|
||||||
|
|
||||||
elif state == 'goal_align':
|
|
||||||
try:
|
|
||||||
if striker.align_to_goal():
|
|
||||||
striker.speak('I am aligning to ball')
|
|
||||||
state = "align"
|
|
||||||
except ValueError:
|
|
||||||
state = 'tracking'
|
|
||||||
|
|
||||||
elif state == 'align':
|
|
||||||
striker.speak("I will try now to align to the ball")
|
|
||||||
striker.mover.set_head_angles(0, 0.25, 0.3)
|
|
||||||
sleep(0.5)
|
|
||||||
try:
|
|
||||||
success = striker.align_to_ball()
|
|
||||||
sleep(0.3)
|
|
||||||
if success:
|
|
||||||
striker.speak('Hasta la vista, Baby')
|
|
||||||
state = 'kick'
|
|
||||||
except ValueError:
|
|
||||||
pass
|
|
||||||
# striker.mover.set_head_angles(0, 0, 0.3)
|
|
||||||
# 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
|
|
||||||
|
|
||||||
elif state == 'kick':
|
|
||||||
print('KICK!')
|
|
||||||
striker.mover.stand_up()
|
|
||||||
sleep(0.3)
|
|
||||||
striker.mover.kick(fancy=True, foot='L')
|
|
||||||
striker.speak("Nice kick. Let's do a dance")
|
|
||||||
striker.mover.dance()
|
|
||||||
break
|
|
||||||
|
|
||||||
loop_end = time()
|
|
||||||
print('Loop time:', loop_end - loop_start)
|
|
||||||
print('\n\n')
|
|
||||||
finally:
|
|
||||||
striker.close()
|
|
||||||
striker.mover.rest()
|
|
||||||
|
|||||||
Reference in New Issue
Block a user