From ed98bca9f6c9da5096462284fd4e80675d4cd3bc Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Sun, 17 Jun 2018 17:54:51 +0200 Subject: [PATCH] Approach, align and kick working --- pykick/copyfiles.sh | 4 +-- pykick/finders.py | 2 -- pykick/movements.py | 12 +++++--- pykick/striker.py | 72 ++++++++++++++++++++++++++++++++++----------- 4 files changed, 65 insertions(+), 25 deletions(-) diff --git a/pykick/copyfiles.sh b/pykick/copyfiles.sh index 8909ac1..4a35ad3 100755 --- a/pykick/copyfiles.sh +++ b/pykick/copyfiles.sh @@ -10,8 +10,8 @@ else nao_ip=$1 fi -destination="$nao_ip:/home/nao/kicker_scripts/" +destination="$nao_ip:/home/nao/pykick/" # copy the files with scp -sshpass -p 'nao' scp ball_approach.py utils.py finders.py \ +scp -v striker.py utils.py finders.py \ movements.py imagereaders.py $destination diff --git a/pykick/finders.py b/pykick/finders.py index f290307..df287ce 100644 --- a/pykick/finders.py +++ b/pykick/finders.py @@ -104,8 +104,6 @@ class BallFinder(object): mask = cv2.inRange(hsv, self.hsv_lower, self.hsv_upper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) - if self.viz: - cv2.imshow('ball_mask', mask) # find contours in the mask and initialize the current # (x, y) center of the ball diff --git a/pykick/movements.py b/pykick/movements.py index 810be43..40b070f 100644 --- a/pykick/movements.py +++ b/pykick/movements.py @@ -21,7 +21,7 @@ class NaoMover(object): [[(1, 0, 'HipPitch', -45, 0.08), (1, 0, 'KneePitch', 10, 0.20), - (1, 0, 'AnklePitch', 20, 0.16)], + (1, 0, 'AnklePitch', 30, 0.16)], 4] ] @@ -61,12 +61,12 @@ class NaoMover(object): ) sleep(wait) - self.stand_up() + self.stand_up(0.5) - def stand_up(self): + def stand_up(self, speed=0.8): self.set_arm_stiffness(0.9) self.set_hand_stiffness(0.9) - self.pp.goToPosture('StandInit', 1.0) + self.pp.goToPosture('StandInit', speed) self.set_hand_stiffness() self.set_arm_stiffness() @@ -130,3 +130,7 @@ class NaoMover(object): def wait(self): self.mp.waitUntilMoveIsFinished() + + def stop_moving(self): + print('STOOOP') + self.mp.stopMove() diff --git a/pykick/striker.py b/pykick/striker.py index 0f8fdde..2ced96d 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -2,7 +2,7 @@ from __future__ import print_function from __future__ import division from math import pi -from time import sleep +from time import sleep, time from .utils import read_config from .imagereaders import NaoImageReader @@ -20,11 +20,13 @@ class Striker(object): fps=30, cam_id=0) self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res, fps=30, cam_id=1) - self.ball_finder = BallFinder(red_hsv[0], red_hsv[1], min_radius, None) + self.ball_finder = BallFinder(tuple(red_hsv[0]), tuple(red_hsv[1]), + min_radius) self.goal_finder = GoalFinder(white_hsv[0], white_hsv[1]) self.lock_counter = 0 self.loss_counter = 0 self.run_after = run_after + self.in_move = False def ball_scan(self): yaw = self.mover.get_head_angles()[0] @@ -54,7 +56,7 @@ class Striker(object): cams = [self.video_top, self.video_bot] in_sight = False for cam in cams: - ball_angles = self.get_ball_angles_from_camera(self, cam) + ball_angles = self.get_ball_angles_from_camera(cam) if ball_angles is not None: x, y = ball_angles in_sight = True @@ -68,18 +70,19 @@ class Striker(object): self.ball_scan() return False - if abs(x) > 0.05: + if abs(x) > 0.15: + self.mover.stop_moving() self.turn_to_ball(x, y) return False else: return True - def run_after(self): - self.mover.move_to(0.3, 0, 0) + def run_to_ball(self): + self.mover.move_to(1, 0, 0) def turn_to_ball(self, ball_x, ball_y): d_yaw, d_pitch = ball_x, 0 - print('ball yaw', d_yaw) + print('ball yaw:', d_yaw) if (abs(d_yaw) > 0.01): self.mover.change_head_angles(d_yaw, d_pitch, @@ -95,6 +98,22 @@ class Striker(object): self.mover.move_to(0, 0, yaw) self.mover.wait() + def align_to_ball(self): + ball_angles = self.get_ball_angles_from_camera(self.video_bot) + if ball_angles is None: + raise ValueError('No ball') + x, y = ball_angles + goal_x, goal_y = 0.115, 0.25 + dx, dy = goal_x - x, goal_y - y + if abs(dx) < 0.05 and abs(dy) < 0.05: + print(x, y) + return True + self.mover.move_to(dy * 0.5, 0, 0) + self.mover.wait() + self.mover.move_to(0, -dx * 0.5, 0) + self.mover.wait() + return False + def align_to_goal(self): ball_angles = self.get_ball_angles_from_camera(self.video_bot) if ball_angles is None: @@ -102,7 +121,7 @@ class Striker(object): x, y = ball_angles print(x, y) - if abs(x) > 0.05: + if abs(x) > 0.1: self.turn_to_ball(x, y) return False @@ -165,29 +184,48 @@ if __name__ == '__main__': red_hsv=cfg['red'], white_hsv=cfg['white'], min_radius=cfg['min_radius'], + run_after=False ) try: state = 'tracking' while True: + loop_start = time() + print('State:', state) if state == 'tracking': if striker.ball_tracking(): - state = 'try_align' - if state == 'try_align': - if (striker.get_ball_angles_from_camera(striker.video_bot) - is not None): + state = 'ball_approach' + + elif state == 'ball_approach': + ball_in_lower = striker.get_ball_angles_from_camera( + striker.video_bot + ) + + print(ball_in_lower) + if (ball_in_lower is not None + and ball_in_lower[1] > 0.25): + + print('Ball is in lower camera, go to align') + striker.mover.stop_moving() state = 'align' else: - striker.run_after() + print('Continue running') + striker.run_to_ball() state = 'tracking' - if state == 'align': + elif state == 'align': + striker.mover.set_head_angles(0, 0.25, 0.3) + sleep(0.5) try: - success = striker.align_to_goal() + success = striker.align_to_ball() if success: state = 'kick' except ValueError: + striker.mover.set_head_angles(0, 0, 0.3) state = 'tracking' - if state == 'kick': - print('YEEEEEEEE') + elif state == 'kick': + print('KICK!') + striker.mover.kick() break + loop_end = time() + print('Loop time:', loop_end - loop_start) finally: striker.close()