pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Improved ball following

Browse Source 
1. Made copyfiles a little saner
2. Implemented ball search when not in field of view
This commit is contained in:
Pavel Lutskov
2018-06-04 15:23:41 +02:00
parent 6aefab5694
commit 5035329de7
4 changed files with 55 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
scripts/ball_approach.py
View File

@@ -12,7 +12,7 @@ from math import tan,pi
class BallFollower(object): class BallFollower(object):
def __init__(self, nao_ip, nao_port, res, hsv_lower, hsv_upper, 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 = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
self.mover.stand_up() self.mover.stand_up()
self.video_top = NaoImageReader(nao_ip, port=nao_port, res=res, 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, self.video_bot = NaoImageReader(nao_ip, port=nao_port, res=res,
fps=30, cam_id=0) fps=30, cam_id=0)
self.finder = BallFinder(hsv_lower, hsv_upper, min_radius, None) 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): def update(self):
#print('in update loop') #print('in update loop')
@@ -28,61 +39,56 @@ class BallFollower(object):
(x, y), radius = self.finder.find_colored_ball( (x, y), radius = self.finder.find_colored_ball(
self.video_top.get_frame() self.video_top.get_frame()
) )
self.loss_counter = 0
x, y = self.video_top.to_relative(x, y) x, y = self.video_top.to_relative(x, y)
angles= self.video_top.to_angles(x,y) x, y = self.video_top.to_angles(x,y)
print("y (in radians) angle is:"+str(angles[1])) # print("y (in radians) angle is:"+str(angles[1]))
y_angle=angles[1] # y_angle=angles[1]
y_angle=pi/2-y_angle-15*pi/180 # y_angle=pi/2-y_angle-15*pi/180
distance = 0.5 * tan(y_angle) # distance = 0.5 * tan(y_angle)
print("Distance="+str(distance)) # print("Distance="+str(distance))
print('Top camera\n') # print('Top camera\n')
except TypeError: except TypeError:
try: try:
(x, y), radius = self.finder.find_colored_ball( (x, y), radius = self.finder.find_colored_ball(
self.video_bot.get_frame() self.video_bot.get_frame()
) )
x, y = self.video_bot.to_relative(x, y) x, y = self.video_bot.to_relative(x, y)
self.loss_counter = 0
#print('Low camera') #print('Low camera')
except TypeError: except TypeError:
print("Ball is not in the view\n") print('No ball in sight')
head_angles=self.mover.get_head_angles() self.loss_counter += 1
# this conidition needs to be improved, as the robot tend starts to search in the right direction, if self.loss_counter > 5:
# as soon as the angle is not -1 anymore self.ball_scan()
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")
return return
#print(x, y) #print(x, y)
self.process_coordinates(x, y) self.process_coordinates(x, y)
def process_coordinates(self, x, y): def process_coordinates(self, x, y):
x_diff = x - 0.5 # x_diff = x - 0.5
y_diff = y - 0.5 # y_diff = y - 0.5
print("x_diff: " + str(x_diff)) # print("x_diff: " + str(x_diff))
print("y_diff: " + str(y_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 * 0.7, d_pitch,
self.mover.change_head_angles(d_yaw, d_pitch, 0.3) abs(d_yaw) / 2)
self.counter = 0 # self.counter = 0
else: yaw = self.mover.get_head_angles()[0]
self.counter += 1 if abs(yaw) > 0.4:
print(self.counter) # self.counter = 0
if self.counter == 4:
self.counter = 0
print('Going to rotate') print('Going to rotate')
head_angle = self.mover.get_head_angles() # self.mover.set_head_angles(0, 0, 0.05)
if abs(head_angle[0]) > 0.1: self.mover.move_to(0, 0, yaw)
self.mover.set_head_angles(0, 0, 0.05) self.mover.wait()
self.mover.move_to(0, 0, head_angle[0]) if self.run_after:
self.mover.wait()
self.mover.move_to(0.3, 0, 0) self.mover.move_to(0.3, 0, 0)
def close(self): def close(self):
self.mover.rest()
self.video_top.close() self.video_top.close()
self.video_bot.close() self.video_bot.close()
@@ -96,7 +102,8 @@ if __name__ == '__main__':
res=cfg['res'], res=cfg['res'],
hsv_lower=tuple(map(cfg.get, ('low_h', 'low_s', 'low_v'))), hsv_lower=tuple(map(cfg.get, ('low_h', 'low_s', 'low_v'))),
hsv_upper=tuple(map(cfg.get, ('high_h', 'high_s', 'high_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: try:
while True: while True:

23
scripts/copyfiles.sh
View File

@@ -3,26 +3,15 @@
# Exit immediately if something fails # Exit immediately if something fails
set -e set -e
if [ -z "$1" ]
if [ -z "$1" ]
then then
nao_ip=192.168.0.11 nao_ip=192.168.0.11
else else
nao_ip=$1 nao_ip=$1
fi 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 # copy the files with scp
sshpass -p 'nao' scp ball_approach.py nao@$nao_ip:~ sshpass -p 'nao' scp ball_approach.py utils.py finders.py \
sshpass -p 'nao' scp utils.py nao@$nao_ip:~ movements.py imagereaders.py $destination
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:~

2
scripts/finders.py
View File

@@ -35,7 +35,6 @@ class BallFinder(object):
# only proceed if at least one contour was found # only proceed if at least one contour was found
if len(cnts) == 0: if len(cnts) == 0:
print('Nothin there')
self.history.appendleft(None) self.history.appendleft(None)
return None return None
@@ -45,7 +44,6 @@ class BallFinder(object):
((x, y), radius) = cv2.minEnclosingCircle(c) ((x, y), radius) = cv2.minEnclosingCircle(c)
if radius < self.min_radius: if radius < self.min_radius:
print('Nothin there')
self.history.appendleft(None) self.history.appendleft(None)
return None return None

8
scripts/movements.py
View File

@@ -29,7 +29,7 @@ class NaoMover(object):
self.mp.rest() self.mp.rest()
self.ready_to_move = False 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) self.mp.setStiffnesses("Head", stiffness)
def set_hand_stiffness(self, stiffness=0.0): def set_hand_stiffness(self, stiffness=0.0):
@@ -48,7 +48,7 @@ class NaoMover(object):
self.mp.setStiffnesses("RElbowRoll", stiffness) self.mp.setStiffnesses("RElbowRoll", stiffness)
self.mp.setStiffnesses("RElbowYaw", 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("LHipYawPitch", stiffness)
self.mp.setStiffnesses("LHipPitch", stiffness) self.mp.setStiffnesses("LHipPitch", stiffness)
self.mp.setStiffnesses("RHipYawPitch", stiffness) self.mp.setStiffnesses("RHipYawPitch", stiffness)
@@ -56,13 +56,13 @@ class NaoMover(object):
self.mp.setStiffnesses("LHipRoll", stiffness) self.mp.setStiffnesses("LHipRoll", stiffness)
self.mp.setStiffnesses("RHipRoll", 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("LAnklePitch", stiffness)
self.mp.setStiffnesses("LAnkleRoll", stiffness) self.mp.setStiffnesses("LAnkleRoll", stiffness)
self.mp.setStiffnesses("RAnklePitch", stiffness) self.mp.setStiffnesses("RAnklePitch", stiffness)
self.mp.setStiffnesses("RAnkleRoll", 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("LKneePitch", stiffness)
self.mp.setStiffnesses("RKneePitch", stiffness) self.mp.setStiffnesses("RKneePitch", stiffness)