Good stable approach
1. Refactored state machine 2. Goal search robuster by looking up 3. More accurate ball distance approximation 4. Eye of the tiger 5. Hasta la vista baby
This commit is contained in:
@@ -11,25 +11,23 @@ if __name__ == '__main__':
|
||||
|
||||
try: # Hit Ctrl-C to stop, cleanup and exit
|
||||
cfg = read_config()
|
||||
with InterruptDelayed():
|
||||
with InterruptDelayed(): # Ignore Ctrl-C for a while
|
||||
striker = Striker(
|
||||
nao_ip=cfg['ip'], nao_port=cfg['port'],
|
||||
res=cfg['res'], ball_hsv=cfg['ball'],
|
||||
goal_hsv=cfg['goal'], field_hsv=cfg['field'],
|
||||
ball_min_radius=cfg['ball_min_radius'],
|
||||
)
|
||||
striker.speak('tiger')
|
||||
sleep(4.75)
|
||||
striker.mover.stand_up(1.0)
|
||||
sleep(9)
|
||||
print('Initialized')
|
||||
striker.mover.stand_up()
|
||||
striker.speak('Initialized')
|
||||
|
||||
state = 'init'
|
||||
# init_soll = 0.0
|
||||
# align_start = 0.15
|
||||
# curve_start = -0.1
|
||||
# curve_stop = 0.1
|
||||
# soll = init_soll
|
||||
striker.speak("Initialized")
|
||||
approach_steps = 0
|
||||
loop_start = time()
|
||||
|
||||
while True:
|
||||
loop_end = time()
|
||||
print('Loop time:', loop_end - loop_start)
|
||||
@@ -41,126 +39,96 @@ if __name__ == '__main__':
|
||||
|
||||
if state == 'init':
|
||||
striker.mover.set_head_angles(0, 0)
|
||||
striker.speak("Start the Ball tracking")
|
||||
striker.ball_tracking(tol=0.05)
|
||||
striker.speak(
|
||||
"I have found the Ball, starting with. Goal search"
|
||||
)
|
||||
striker.mover.stand_up()
|
||||
sleep(0.5)
|
||||
bdist = striker.distance_to_ball()
|
||||
striker.speak('Ball distance is %.2f' % bdist)
|
||||
_, _, gcc = striker.goal_search()
|
||||
print('Goal center', gcc, 'Ball dist', bdist)
|
||||
|
||||
if abs(gcc) < 0.4:
|
||||
striker.speak('Direct approach')
|
||||
if abs(gcc) < 0.4 or bdist <= 0.2:
|
||||
print('Straight approach')
|
||||
state = 'straight_approach'
|
||||
approach = 0
|
||||
continue
|
||||
|
||||
if gcc < 0:
|
||||
striker.speak("I have found the. goal on the right")
|
||||
approach = 1
|
||||
elif 0.20 < bdist < 0.50:
|
||||
print('Rdist is hypo')
|
||||
state = 'rdist_is_hypo'
|
||||
approach = 1 if gcc < 0 else - 1
|
||||
else:
|
||||
striker.speak("I have found the. goal on the left")
|
||||
approach = -1
|
||||
print('Bdist is hypo')
|
||||
state = 'bdist_is_hypo'
|
||||
approach = 1 if gcc < 0 else - 1
|
||||
|
||||
if approach == 1:
|
||||
striker.speak('Goal on the right')
|
||||
elif approach == -1:
|
||||
striker.speak('Goal on the left')
|
||||
else:
|
||||
striker.speak('Direct approach')
|
||||
|
||||
striker.mover.set_head_angles(0, 0)
|
||||
#approach = 1 if goal_center < 0 else -1
|
||||
#approach = 1
|
||||
sleep(0.5)
|
||||
state = 'ball_approach'
|
||||
|
||||
elif state == 'tracking':
|
||||
# start ball approach when ball is visible
|
||||
print('Soll angle')
|
||||
striker.ball_tracking(tol=0.15)
|
||||
# break
|
||||
if approach_steps < 2 and approach != 0:
|
||||
state = 'ball_approach'
|
||||
else:
|
||||
state = 'straight_approach'
|
||||
|
||||
elif state == 'straight_approach':
|
||||
striker.ball_tracking(tol=0.20)
|
||||
bil = striker.get_ball_angles_from_camera(
|
||||
striker.lower_camera
|
||||
) # Ball in lower
|
||||
print('Ball in lower!', bil)
|
||||
if bil is not None and bil[1] > 0.15:
|
||||
striker.speak('Ball is close enough, stop approach')
|
||||
if bil is not None and bil[1] > 0.20:
|
||||
striker.mover.stop_moving()
|
||||
striker.speak('Align to goal')
|
||||
striker.speak('Aligning to goal')
|
||||
state = 'goal_align'
|
||||
else:
|
||||
striker.speak('Continue running')
|
||||
striker.run_to_ball(1)
|
||||
state = 'tracking'
|
||||
|
||||
elif state == 'ball_approach':
|
||||
sleep(0.8)
|
||||
bil = striker.get_ball_angles_from_camera(
|
||||
striker.lower_camera
|
||||
) # Ball in lower
|
||||
|
||||
try:
|
||||
d = striker.distance_to_ball()
|
||||
except ValueError:
|
||||
if bil is not None:
|
||||
state = 'straight_approach'
|
||||
striker.speak('Ball is close. ' +
|
||||
'But not close enough maybe')
|
||||
else:
|
||||
state = 'tracking'
|
||||
continue
|
||||
|
||||
print('Distance to ball', d)
|
||||
striker.speak("The distance to the ball is approximately "
|
||||
+ str(round(d,2)) + " Meters")
|
||||
angle = striker.walking_direction(approach, d)
|
||||
d_run = d * cos(angle)
|
||||
elif state == 'bdist_is_hypo':
|
||||
angle = striker.walking_direction(approach, bdist, 'bdist')
|
||||
rdist = bdist * cos(angle)
|
||||
print('Approach angle', angle)
|
||||
|
||||
striker.mover.move_to(0, 0, angle)
|
||||
striker.mover.wait()
|
||||
striker.run_to_ball(d_run)
|
||||
striker.run_to_ball(rdist)
|
||||
striker.mover.wait()
|
||||
striker.speak("I think I have reached the ball. " +
|
||||
"I will start rotating")
|
||||
approach_steps += 1
|
||||
striker.mover.move_to(0, 0, -pi/2 * approach)
|
||||
striker.mover.wait()
|
||||
state = 'tracking'
|
||||
state = 'init'
|
||||
|
||||
elif state == 'rdist_is_hypo':
|
||||
angle = striker.walking_direction(approach, bdist, 'rdist')
|
||||
rdist = bdist / cos(angle)
|
||||
print('Approach angle', angle)
|
||||
|
||||
striker.mover.move_to(0, 0, angle)
|
||||
striker.mover.wait()
|
||||
striker.run_to_ball(rdist)
|
||||
striker.mover.wait()
|
||||
striker.mover.move_to(0, 0, -(pi/2 - angle) * approach)
|
||||
striker.mover.wait()
|
||||
state = 'init'
|
||||
|
||||
elif state == 'goal_align':
|
||||
try:
|
||||
if striker.align_to_goal():
|
||||
striker.speak('Ball and goal are aligned')
|
||||
state = "align"
|
||||
except ValueError:
|
||||
state = 'tracking'
|
||||
continue
|
||||
|
||||
elif state == 'align':
|
||||
striker.speak("I will try now to align to the ball")
|
||||
striker.mover.set_head_angles(0, 0.25, 0.3)
|
||||
sleep(0.5)
|
||||
try:
|
||||
success = striker.align_to_ball()
|
||||
sleep(0.3)
|
||||
if success:
|
||||
striker.speak('Hasta la vista, Baby')
|
||||
state = 'kick'
|
||||
striker.speak('hasta')
|
||||
except ValueError:
|
||||
pass
|
||||
# striker.mover.set_head_angles(0, 0, 0.3)
|
||||
# state = 'tracking'
|
||||
|
||||
elif state == 'simple_kick':
|
||||
striker.mover.set_head_angles(0,0.25,0.3)
|
||||
striker.ball_tracking(tol=0.10, soll=0)
|
||||
print('Doing the simple kick')
|
||||
|
||||
# just walk a short distance forward, ball should be near
|
||||
# and it will probably be kicked in the right direction
|
||||
striker.speak("Simple Kick")
|
||||
sleep(1)
|
||||
striker.mover.move_to_fast(0.5, 0, 0)
|
||||
striker.mover.wait()
|
||||
break
|
||||
striker.ball_tracking()
|
||||
|
||||
elif state == 'kick':
|
||||
print('KICK!')
|
||||
@@ -168,8 +136,8 @@ if __name__ == '__main__':
|
||||
sleep(0.3)
|
||||
striker.mover.kick(fancy=True, foot='L')
|
||||
striker.mover.stand_up()
|
||||
striker.speak('Nice kick. Lets do the dance')
|
||||
sleep(2)
|
||||
striker.speak("Nice kick. Let's do a dance")
|
||||
striker.mover.dance()
|
||||
break
|
||||
finally:
|
||||
@@ -177,6 +145,30 @@ if __name__ == '__main__':
|
||||
striker.mover.rest()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# ____________________ STRIKER NEW ________________________________
|
||||
#
|
||||
# Ball tracking --> Distance to ball --> Goal angle
|
||||
# ^ |
|
||||
# | |
|
||||
# | yes v
|
||||
# | Ball distance <-- Goal angle > thr
|
||||
# | / | \ |
|
||||
# | > 50 cm / |(20,50) \ < 20cm | no
|
||||
# | / v \ v
|
||||
# +- Distance is < Walk is hypo \ Straight approach
|
||||
# | hypo | > until goal align
|
||||
# | | (bil > 0.2)
|
||||
# -----------------------+ |
|
||||
# |
|
||||
# | / | /^ | / v
|
||||
# |( | ( |( Ball Goal align
|
||||
# | \ | \_ | \ <-- align <-- (if lost ball run backwards)
|
||||
#
|
||||
#__________________________________________________________________
|
||||
|
||||
# ____________________ STRIKER __________________________
|
||||
#
|
||||
# +----> Ball tracking (see below) <-------------+
|
||||
|
||||
Reference in New Issue
Block a user