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

added distance estimation to ball_approach

Browse Source
This commit is contained in:
Jonas
2018-06-04 12:23:40 +02:00
parent 3e3927a4d9
commit 76b53e83dd
1 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
scripts/ball_approach.py
View File

@@ -6,6 +6,7 @@ from utils import read_config
from imagereaders import NaoImageReader from imagereaders import NaoImageReader
from finders import BallFinder from finders import BallFinder
from movements import NaoMover from movements import NaoMover
from math import tan,pi
class BallFollower(object): class BallFollower(object):
@@ -22,24 +23,30 @@ class BallFollower(object):
self.counter = 0 self.counter = 0
def update(self): def update(self):
print('in update loop') #print('in update loop')
try: try:
(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()
) )
x, y = self.video_top.to_relative(x, y) x, y = self.video_top.to_relative(x, y)
print('Top camera') 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')
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)
print('Low camera') #print('Low camera')
except TypeError: except TypeError:
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