From 21cdfafc8036fdba9814e90af0355de2d4b1eb00 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sat, 30 Jun 2018 10:18:05 +0200 Subject: [PATCH] Moved behaviour into top level package --- .gitignore | 2 + pykick/__init__.py | 0 pykick/__main__.py | 172 +++++++++++++++++++++++++++++++++++++++++++++ pykick/striker.py | 167 +------------------------------------------ 4 files changed, 175 insertions(+), 166 deletions(-) delete mode 100644 pykick/__init__.py create mode 100644 pykick/__main__.py diff --git a/.gitignore b/.gitignore index 5b268bd..a528a28 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ *.sw* *.pyc *.jpg +*.avi +*.mpg *~ ~* exp_* diff --git a/pykick/__init__.py b/pykick/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/pykick/__main__.py b/pykick/__main__.py new file mode 100644 index 0000000..3c9214c --- /dev/null +++ b/pykick/__main__.py @@ -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() diff --git a/pykick/striker.py b/pykick/striker.py index 410ccf8..8002d78 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -2,11 +2,10 @@ from __future__ import print_function from __future__ import division 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 collections import deque -from .utils import read_config from .imagereaders import NaoImageReader from .finders import BallFinder, GoalFinder, FieldFinder from .movements import NaoMover @@ -334,167 +333,3 @@ class Striker(object): return goal_center_x self.mover.set_head_angles(0, 0) 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()