diff --git a/pykick/__main__.py b/pykick/__main__.py index 3cc2b73..6fa1bd3 100644 --- a/pykick/__main__.py +++ b/pykick/__main__.py @@ -11,25 +11,23 @@ if __name__ == '__main__': try: # Hit Ctrl-C to stop, cleanup and exit cfg = read_config() - with InterruptDelayed(): + with InterruptDelayed(): # Ignore Ctrl-C for a while 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'], ) + striker.speak('tiger') + sleep(4.75) + striker.mover.stand_up(1.0) + sleep(9) print('Initialized') - striker.mover.stand_up() + striker.speak('Initialized') state = 'init' - # init_soll = 0.0 - # align_start = 0.15 - # curve_start = -0.1 - # curve_stop = 0.1 - # soll = init_soll - striker.speak("Initialized") - approach_steps = 0 loop_start = time() + while True: loop_end = time() print('Loop time:', loop_end - loop_start) @@ -41,126 +39,96 @@ if __name__ == '__main__': if state == 'init': striker.mover.set_head_angles(0, 0) - striker.speak("Start the Ball tracking") striker.ball_tracking(tol=0.05) - striker.speak( - "I have found the Ball, starting with. Goal search" - ) + striker.mover.stand_up() + sleep(0.5) + bdist = striker.distance_to_ball() + striker.speak('Ball distance is %.2f' % bdist) _, _, gcc = striker.goal_search() + print('Goal center', gcc, 'Ball dist', bdist) - if abs(gcc) < 0.4: - striker.speak('Direct approach') + if abs(gcc) < 0.4 or bdist <= 0.2: + print('Straight approach') state = 'straight_approach' approach = 0 - continue - if gcc < 0: - striker.speak("I have found the. goal on the right") - approach = 1 + elif 0.20 < bdist < 0.50: + print('Rdist is hypo') + state = 'rdist_is_hypo' + approach = 1 if gcc < 0 else - 1 else: - striker.speak("I have found the. goal on the left") - approach = -1 + print('Bdist is hypo') + state = 'bdist_is_hypo' + approach = 1 if gcc < 0 else - 1 + + if approach == 1: + striker.speak('Goal on the right') + elif approach == -1: + striker.speak('Goal on the left') + else: + striker.speak('Direct approach') striker.mover.set_head_angles(0, 0) - #approach = 1 if goal_center < 0 else -1 - #approach = 1 sleep(0.5) - state = 'ball_approach' - - elif state == 'tracking': - # start ball approach when ball is visible - print('Soll angle') - striker.ball_tracking(tol=0.15) - # break - if approach_steps < 2 and approach != 0: - state = 'ball_approach' - else: - state = 'straight_approach' elif state == 'straight_approach': + striker.ball_tracking(tol=0.20) bil = striker.get_ball_angles_from_camera( striker.lower_camera ) # Ball in lower print('Ball in lower!', bil) - if bil is not None and bil[1] > 0.15: - striker.speak('Ball is close enough, stop approach') + if bil is not None and bil[1] > 0.20: striker.mover.stop_moving() - striker.speak('Align to goal') + striker.speak('Aligning to goal') state = 'goal_align' else: - striker.speak('Continue running') striker.run_to_ball(1) - state = 'tracking' - elif state == 'ball_approach': - sleep(0.8) - bil = striker.get_ball_angles_from_camera( - striker.lower_camera - ) # Ball in lower - - try: - d = striker.distance_to_ball() - except ValueError: - if bil is not None: - state = 'straight_approach' - striker.speak('Ball is close. ' + - 'But not close enough maybe') - else: - 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) + elif state == 'bdist_is_hypo': + angle = striker.walking_direction(approach, bdist, 'bdist') + rdist = bdist * cos(angle) print('Approach angle', angle) striker.mover.move_to(0, 0, angle) striker.mover.wait() - striker.run_to_ball(d_run) + striker.run_to_ball(rdist) striker.mover.wait() - striker.speak("I think I have reached the ball. " + - "I will start rotating") - approach_steps += 1 striker.mover.move_to(0, 0, -pi/2 * approach) striker.mover.wait() - state = 'tracking' + state = 'init' + + elif state == 'rdist_is_hypo': + angle = striker.walking_direction(approach, bdist, 'rdist') + rdist = bdist / cos(angle) + print('Approach angle', angle) + + striker.mover.move_to(0, 0, angle) + striker.mover.wait() + striker.run_to_ball(rdist) + striker.mover.wait() + striker.mover.move_to(0, 0, -(pi/2 - angle) * approach) + striker.mover.wait() + state = 'init' elif state == 'goal_align': try: if striker.align_to_goal(): + striker.speak('Ball and goal are aligned') state = "align" except ValueError: - state = 'tracking' + continue 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' + striker.speak('hasta') 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 + striker.ball_tracking() elif state == 'kick': print('KICK!') @@ -168,8 +136,8 @@ if __name__ == '__main__': sleep(0.3) striker.mover.kick(fancy=True, foot='L') striker.mover.stand_up() + striker.speak('Nice kick. Lets do the dance') sleep(2) - striker.speak("Nice kick. Let's do a dance") striker.mover.dance() break finally: @@ -177,6 +145,30 @@ if __name__ == '__main__': striker.mover.rest() + + + +# ____________________ STRIKER NEW ________________________________ +# +# Ball tracking --> Distance to ball --> Goal angle +# ^ | +# | | +# | yes v +# | Ball distance <-- Goal angle > thr +# | / | \ | +# | > 50 cm / |(20,50) \ < 20cm | no +# | / v \ v +# +- Distance is < Walk is hypo \ Straight approach +# | hypo | > until goal align +# | | (bil > 0.2) +# -----------------------+ | +# | +# | / | /^ | / v +# |( | ( |( Ball Goal align +# | \ | \_ | \ <-- align <-- (if lost ball run backwards) +# +#__________________________________________________________________ + # ____________________ STRIKER __________________________ # # +----> Ball tracking (see below) <-------------+ diff --git a/pykick/movements.py b/pykick/movements.py index 2b43440..d964e7d 100644 --- a/pykick/movements.py +++ b/pykick/movements.py @@ -1,10 +1,10 @@ # from time import sleep import argparse -from math import radians +from math import radians, pi from naoqi import ALProxy -from .utils import read_config +from .utils import read_config class NaoMover(object): @@ -169,7 +169,17 @@ class NaoMover(object): if not self.ready_to_move: self.mp.moveInit() self.ready_to_move = True - self.mp.post.moveTo(front, side, rotation) + if rotation != 0: + n_cycles = int(abs(rotation) // (pi/4)) + sign = 1 if rotation > 0 else -1 + rest = abs(rotation) % (pi/4) + print('Rotation', rotation, 'Cycles', n_cycles, 'Rest', rest) + for _ in range(n_cycles): + self.mp.post.moveTo(0, 0, pi/4 * sign) + self.wait() + self.mp.post.moveTo(0, 0, rest * sign) + else: + self.mp.post.moveTo(front, side, rotation) def move_to_fast(self, front, side, rotation, wait=False): if not self.ready_to_move: @@ -210,7 +220,7 @@ class NaoMover(object): clear_existing ) self.wait() - self.stand_up(0.5) + self.stand_up(0.7) def wait(self): self.mp.waitUntilMoveIsFinished() diff --git a/pykick/nao_defaults.json b/pykick/nao_defaults.json index 3743da1..d0a12f6 100644 --- a/pykick/nao_defaults.json +++ b/pykick/nao_defaults.json @@ -11,19 +11,19 @@ 255 ] ], - "cam": 1, "goal": [ [ 0, 0, - 100 + 89 ], [ 180, - 65, + 73, 255 ] ], + "fps": 10, "res": 2, "ball_min_radius": 0.01, "field": [ @@ -38,7 +38,7 @@ 255 ] ], - "fps": 10, + "cam": 1, "ip": "192.168.0.11", "port": 9559 } \ No newline at end of file diff --git a/pykick/striker.py b/pykick/striker.py index c90e2cd..a5c0c72 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -2,7 +2,7 @@ from __future__ import print_function from __future__ import division from threading import Thread -from math import pi, tan, asin, radians +from math import pi, tan, asin, atan, radians from time import sleep, time, strftime from collections import deque @@ -53,7 +53,8 @@ class Striker(object): self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1])) # Talking - self.speaker = ALProxy('ALTextToSpeech', bytes(nao_ip), nao_port) + 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) @@ -80,7 +81,14 @@ class Striker(object): def _speaker(self): while not self.is_over: while self.speach_queue: - self.speaker.say(self.speach_queue.pop()) + 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): @@ -102,25 +110,22 @@ class Striker(object): """Intelligently rotate the robot to search for stuff.""" self.mover.stop_moving() self.rotating = False - yaw = self.mover.get_head_angles()[0] - mag = yaw + 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 mag > 0.8: - self.mover.set_head_angles(-pi / 8, 0, 0.5) + if yaw > 0.8: + self.mover.set_head_angles(-pi / 8, pitch, 0.5) sleep(0.5) - elif mag < -0.8: + elif yaw < -0.8: self.mover.move_to_fast(0, 0, -pi / 4) self.mover.wait() - #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 right, if head yaw angle is smaller than zero else: - #self.speak("I have found the ball") self.mover.change_head_angles(sign * pi / 8, 0, 0.5) sleep(0.3) @@ -175,15 +180,23 @@ class Striker(object): 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: - raise ValueError('No ball in sight') + 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) + 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): - return asin(0.5 / d) * lr + 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. @@ -215,11 +228,9 @@ class Striker(object): # stop visibility check if not in_sight: - self.speak('No ball visible search it') self.scan_rotation() continue - # self.speak('I see the ball') ball_locked = self.turn_to_ball(x, y, soll=soll, tol=tol) print() @@ -257,7 +268,6 @@ class Striker(object): if abs(d_yaw) > tol: self.mover.stop_moving() print('Going to rotate by', d_yaw) - self.speak('Going to rotate') self.mover.set_head_angles(soll, head_pitch, 0.3) self.mover.move_to(0, 0, d_yaw) self.mover.wait() @@ -275,7 +285,7 @@ class Striker(object): goal_x, goal_y = 0.095, 0.4 dx, dy = goal_x - x, goal_y - y - dx = -dx * 0.2 if abs(dx) > 0.05 else 0 + 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 @@ -293,14 +303,11 @@ class Striker(object): ball_angles = self.get_ball_angles_from_camera(self.lower_camera, mask=False) if ball_angles is None: - self.speak("Cannot see the ball") raise ValueError('No ball') x, y = ball_angles print(x, y) - self.speak("Turn to ball") self.turn_to_ball(x, y, tol=0.15) - self.speak('Trying to find the goal') sleep(0.2) goal = self.goal_search() @@ -308,13 +315,9 @@ class Striker(object): return False gcl, gcr, gcc = goal - self.speak('Goal found') print('Goal:', gcl, gcr, gcc) if gcl > 0 > gcr: - self.speak("Goal and ball are aligned") - print('Goal ball aligned!') - #raise SystemExit return True if y > 0.35: @@ -327,19 +330,24 @@ class Striker(object): # return False sign = -1 if gcc > 0 else 1 - num_steps = int(min(abs(gcc), 0.1) // 0.05) - for _ in range(num_steps): + 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) + self.mover.set_head_angles(angle, -0.3) sleep(0.5) for i in range(5): print(i, goal_angles)