From 5035329de7be9668881356b57f621c182eabddf9 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Mon, 4 Jun 2018 15:23:41 +0200 Subject: [PATCH] Improved ball following 1. Made copyfiles a little saner 2. Implemented ball search when not in field of view --- scripts/ball_approach.py | 83 ++++++++++++++++++++++------------------ scripts/copyfiles.sh | 23 +++-------- scripts/finders.py | 2 - scripts/movements.py | 8 ++-- 4 files changed, 55 insertions(+), 61 deletions(-) diff --git a/scripts/ball_approach.py b/scripts/ball_approach.py index 96acdff..4cc54aa 100644 --- a/scripts/ball_approach.py +++ b/scripts/ball_approach.py @@ -12,7 +12,7 @@ from math import tan,pi class BallFollower(object): def __init__(self, nao_ip, nao_port, res, hsv_lower, hsv_upper, - min_radius): + min_radius, run_after): self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) self.mover.stand_up() self.video_top = NaoImageReader(nao_ip, port=nao_port, res=res, @@ -20,7 +20,18 @@ class BallFollower(object): self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res, fps=30, cam_id=0) self.finder = BallFinder(hsv_lower, hsv_upper, min_radius, None) - self.counter = 0 + self.lock_counter = 0 + self.loss_counter = 0 + self.run_after = run_after + + def ball_scan(self): + yaw = self.mover.get_head_angles()[0] + mag = abs(yaw) + sign = 1 if yaw >= 0 else -1 + if mag > 2: + self.mover.move_to(0, 0, sign * pi / 12) + else: + self.mover.change_head_angles(sign * pi / 4, 0, 0.5) def update(self): #print('in update loop') @@ -28,61 +39,56 @@ class BallFollower(object): (x, y), radius = self.finder.find_colored_ball( self.video_top.get_frame() ) + self.loss_counter = 0 x, y = self.video_top.to_relative(x, y) - angles= self.video_top.to_angles(x,y) - print("y (in radians) angle is:"+str(angles[1])) - y_angle=angles[1] - y_angle=pi/2-y_angle-15*pi/180 - distance = 0.5 * tan(y_angle) - print("Distance="+str(distance)) - print('Top camera\n') + x, y = self.video_top.to_angles(x,y) + # print("y (in radians) angle is:"+str(angles[1])) + # y_angle=angles[1] + # y_angle=pi/2-y_angle-15*pi/180 + # distance = 0.5 * tan(y_angle) + # print("Distance="+str(distance)) + # print('Top camera\n') except TypeError: try: (x, y), radius = self.finder.find_colored_ball( self.video_bot.get_frame() ) x, y = self.video_bot.to_relative(x, y) + self.loss_counter = 0 #print('Low camera') except TypeError: - print("Ball is not in the view\n") - head_angles=self.mover.get_head_angles() - # this conidition needs to be improved, as the robot tend starts to search in the right direction, - # as soon as the angle is not -1 anymore - if head_angles[0]==-1: - self.mover.set_head_angles(1,0,0.05) - print("move head to the left") - else: - self.mover.set_head_angles(-1,0,0.05) - print("move head to the right") + print('No ball in sight') + self.loss_counter += 1 + if self.loss_counter > 5: + self.ball_scan() return #print(x, y) self.process_coordinates(x, y) def process_coordinates(self, x, y): - x_diff = x - 0.5 - y_diff = y - 0.5 - print("x_diff: " + str(x_diff)) - print("y_diff: " + str(y_diff)) + # x_diff = x - 0.5 + # y_diff = y - 0.5 + # print("x_diff: " + str(x_diff)) + # print("y_diff: " + str(y_diff)) - d_yaw, d_pitch = -x_diff / 2, 0 + d_yaw, d_pitch = x, 0 + print(d_yaw) - if abs(x_diff) > 0.1: - self.mover.change_head_angles(d_yaw, d_pitch, 0.3) - self.counter = 0 - else: - self.counter += 1 - print(self.counter) - if self.counter == 4: - self.counter = 0 + self.mover.change_head_angles(d_yaw * 0.7, d_pitch, + abs(d_yaw) / 2) + # self.counter = 0 + yaw = self.mover.get_head_angles()[0] + if abs(yaw) > 0.4: + # self.counter = 0 print('Going to rotate') - head_angle = self.mover.get_head_angles() - if abs(head_angle[0]) > 0.1: - self.mover.set_head_angles(0, 0, 0.05) - self.mover.move_to(0, 0, head_angle[0]) - self.mover.wait() + # self.mover.set_head_angles(0, 0, 0.05) + self.mover.move_to(0, 0, yaw) + self.mover.wait() + if self.run_after: self.mover.move_to(0.3, 0, 0) def close(self): + self.mover.rest() self.video_top.close() self.video_bot.close() @@ -96,7 +102,8 @@ if __name__ == '__main__': res=cfg['res'], hsv_lower=tuple(map(cfg.get, ('low_h', 'low_s', 'low_v'))), hsv_upper=tuple(map(cfg.get, ('high_h', 'high_s', 'high_v'))), - min_radius=cfg['min_radius'] + min_radius=cfg['min_radius'], + run_after=False ) try: while True: diff --git a/scripts/copyfiles.sh b/scripts/copyfiles.sh index 3cdc00b..8909ac1 100755 --- a/scripts/copyfiles.sh +++ b/scripts/copyfiles.sh @@ -3,26 +3,15 @@ # Exit immediately if something fails set -e - -if [ -z "$1" ] +if [ -z "$1" ] then - nao_ip=192.168.0.11 + nao_ip=192.168.0.11 else - nao_ip=$1 + nao_ip=$1 fi -sshpass -p 'nao' ssh nao@$nao_ip 'cd ~ && rm ball_approach.py utils.py finders.py movements.py imagereaders.py || echo "already removed"' - +destination="$nao_ip:/home/nao/kicker_scripts/" # copy the files with scp -sshpass -p 'nao' scp ball_approach.py nao@$nao_ip:~ -sshpass -p 'nao' scp utils.py nao@$nao_ip:~ -sshpass -p 'nao' scp finders.py nao@$nao_ip:~ -sshpass -p 'nao' scp movements.py nao@$nao_ip:~ -sshpass -p 'nao' scp imagereaders.py nao@$nao_ip:~ - - - -# config ersetzen -#sshpass -p 'nao' scp nao_defaults.json nao@192.168.0.11:~ - +sshpass -p 'nao' scp ball_approach.py utils.py finders.py \ + movements.py imagereaders.py $destination diff --git a/scripts/finders.py b/scripts/finders.py index 476c391..fe4b0b2 100644 --- a/scripts/finders.py +++ b/scripts/finders.py @@ -35,7 +35,6 @@ class BallFinder(object): # only proceed if at least one contour was found if len(cnts) == 0: - print('Nothin there') self.history.appendleft(None) return None @@ -45,7 +44,6 @@ class BallFinder(object): ((x, y), radius) = cv2.minEnclosingCircle(c) if radius < self.min_radius: - print('Nothin there') self.history.appendleft(None) return None diff --git a/scripts/movements.py b/scripts/movements.py index c498e2c..2ba8173 100644 --- a/scripts/movements.py +++ b/scripts/movements.py @@ -29,7 +29,7 @@ class NaoMover(object): self.mp.rest() self.ready_to_move = False - def set_head_stiffness(self, stiffness=0.8): + def set_head_stiffness(self, stiffness=0.5): self.mp.setStiffnesses("Head", stiffness) def set_hand_stiffness(self, stiffness=0.0): @@ -48,7 +48,7 @@ class NaoMover(object): self.mp.setStiffnesses("RElbowRoll", stiffness) self.mp.setStiffnesses("RElbowYaw", stiffness) - def set_hip_stiffness(self, stiffness=0.9): + def set_hip_stiffness(self, stiffness=0.6): self.mp.setStiffnesses("LHipYawPitch", stiffness) self.mp.setStiffnesses("LHipPitch", stiffness) self.mp.setStiffnesses("RHipYawPitch", stiffness) @@ -56,13 +56,13 @@ class NaoMover(object): self.mp.setStiffnesses("LHipRoll", stiffness) self.mp.setStiffnesses("RHipRoll", stiffness) - def set_ankle_stiffness(self, stiffness=0.9): + def set_ankle_stiffness(self, stiffness=0.6): self.mp.setStiffnesses("LAnklePitch", stiffness) self.mp.setStiffnesses("LAnkleRoll", stiffness) self.mp.setStiffnesses("RAnklePitch", stiffness) self.mp.setStiffnesses("RAnkleRoll", stiffness) - def set_knee_stiffness(self, stiffness=0.9): + def set_knee_stiffness(self, stiffness=0.6): self.mp.setStiffnesses("LKneePitch", stiffness) self.mp.setStiffnesses("RKneePitch", stiffness)