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

More work on goal-ball alignment

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-23 20:50:28 +02:00
parent 73ca100c83
commit 67a8cccd51
5 changed files with 33 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
pykick/finders.py
View File

@@ -39,7 +39,6 @@ class FieldFinder(object):
def mask_it(self, frame, field, inverse=False):
if field is not None:
print(frame.shape)
mask = np.zeros(frame.shape[:2], dtype=np.uint8)
cv2.drawContours(mask, (field,), -1, 255, -1)
if inverse:
@@ -123,6 +122,8 @@ class GoalFinder(object):
def goal_center(self, contour):
l, r = self.left_right_post(contour)
print('Left goal post:', l,
'Right goal post:', r)
return (l + r) / 2
def draw(self, frame, goal):

1
pykick/imagereaders.py
View File

@@ -32,6 +32,7 @@ class NaoImageReader(object):
)
def to_relative(self, x, y):
print('Camera resolution:', self.res)
return x / self.res[1], y / self.res[0]
def get_frame(self):

23
pykick/move_robot.py
View File

@@ -7,13 +7,18 @@ if __name__ == "__main__":
mover = NaoMover(cfg['ip'], cfg['port'])
mover.stand_up()
while True:
axis = int(raw_input('Axis: '))
amount = float(raw_input('How much: '))
if axis == 0:
mover.move_to(amount, 0, 0)
elif axis == 1:
mover.move_to(0, amount, 0)
elif axis == 2:
mover.move_to(0, 0, amount)
else:
print('Axis out of range (0, 1, 2)')
mover.move_to(0, 0, 0.5 * amount)
mover.wait()
mover.move_to(0, -0.3 * amount, 0)
mover.wait()
mover.move_to(-0.1 * abs(amount), 0, 0)
# amount = float(raw_input('How much: '))
# if axis == 0:
# mover.move_to(amount, 0, 0)
# elif axis == 1:
# mover.move_to(0, amount, 0)
# elif axis == 2:
# mover.move_to(0, 0, amount)
# else:
# print('Axis out of range (0, 1, 2)')

4
pykick/nao_defaults.json
View File

@@ -11,7 +11,6 @@
255
]
],
"cam": 1,
"goal": [
[
0,
@@ -24,6 +23,7 @@
255
]
],
"fps": 30,
"res": 2,
"ball_min_radius": 0.01,
"field": [
@@ -38,7 +38,7 @@
255
]
],
"fps": 30,
"cam": 1,
"ip": "192.168.0.11",
"port": 9559
}

21
pykick/striker.py
View File

@@ -212,12 +212,14 @@ class Striker(object):
print(x, y)
self.turn_to_ball(x, y, tol=0.15)
goal_center_x = self.get_goal_center_angle_from_camera(
self.upper_camera
)
goal_center_x = None
for i in range(3):
if goal_center_x is None:
goal_center_x = self.get_goal_center_angle_from_camera(
self.upper_camera
)
print('Goal center:', goal_center_x)
if goal_center_x is not None and abs(goal_center_x) < 0.01:
if goal_center_x is not None and abs(goal_center_x) < 0.1:
print('Goal ball aligned!')
raise SystemExit
@@ -232,7 +234,9 @@ class Striker(object):
# return False
self.mover.move_to(0, 0.2, 0)
print('Moving sideways')
self.mover.wait()
print('Finished moving')
return False
def close(self):
@@ -362,7 +366,7 @@ if __name__ == '__main__':
# (see diagram above)
else:
try: # Hit Ctrl-C to stop, cleanup and exit
state = 'goal_align'
state = 'tracking'
# t = None
while True:
# meassure time for debbuging
@@ -381,10 +385,10 @@ if __name__ == '__main__':
striker.lower_camera
)
print(ball_in_lower)
if (ball_in_lower is not None and ball_in_lower[1] > 0.10):
if (ball_in_lower is not None and ball_in_lower[1] > 0):
print('Ball is close enough, stop approach')
striker.mover.stop_moving()
striker.say('Align to goal')
striker.speak('Align to goal')
state = 'goal_align'
else:
print('Continue running')
@@ -425,5 +429,6 @@ if __name__ == '__main__':
loop_end = time()
print('Loop time:', loop_end - loop_start)
print('\n\n')
finally:
striker.close()