88 lines
2.7 KiB
Python
88 lines
2.7 KiB
Python
from __future__ import print_function
|
|
from __future__ import division
|
|
|
|
from utils import read_config
|
|
|
|
from imagereaders import NaoImageReader
|
|
from finders import BallFinder
|
|
from movements import NaoMover
|
|
|
|
|
|
class BallFollower(object):
|
|
|
|
def __init__(self, nao_ip, nao_port, res, hsv_lower, hsv_upper,
|
|
min_radius):
|
|
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,
|
|
fps=30, cam_id=0)
|
|
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
|
|
|
|
def update(self):
|
|
try:
|
|
(x, y), radius = self.finder.find_colored_ball(
|
|
self.video_top.get_frame()
|
|
)
|
|
x, y = self.video_top.to_relative(x, y)
|
|
print('Top camera')
|
|
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)
|
|
print('Low camera')
|
|
except TypeError:
|
|
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))
|
|
|
|
d_yaw, d_pitch = -x_diff / 2, 0
|
|
|
|
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
|
|
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.move_to(0.3, 0, 0)
|
|
|
|
def close(self):
|
|
self.video_top.close()
|
|
self.video_bot.close()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
cfg = read_config()
|
|
follower = BallFollower(
|
|
nao_ip=cfg['ip'],
|
|
nao_port=cfg['port'],
|
|
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']
|
|
)
|
|
try:
|
|
while True:
|
|
follower.update()
|
|
finally:
|
|
follower.close()
|