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):
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:

23
scripts/copyfiles.sh
View File

@@ -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

2
scripts/finders.py
View File

@@ -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

8
scripts/movements.py
View File

@@ -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)