def printBall(self):
(ball_x, ball_y) = polar2cart(self._world.ball.distSmoothed, self._world.ball.bearing)
# minimal printing for spreadshit
# print "%s, %3.3f, %3.3f, %3.3f, %3.3f, %3.3f" % (self._world.ball.seen, self._world.ball.dist, self._world.ball.distSmoothed, self._world.ball.bearing, ball_x, ball_y)
# legible printing
(ball_x, ball_y) = polar2cart(self._world.ball.distSmoothed, self._world.ball.bearing)
print "dist: %3.3f, distSmoothed: %3.3f, ball bearing: %3.3f, Ball x: %3.3f, Ball y: %3.3f" % (self._world.ball.dist, self._world.ball.distSmoothed, self._world.ball.bearing, ball_x, ball_y)
def doNextAction(self):
if self.verbose:
print "\nDeciding on next move: (ball seen %s, dist: %3.3f, distSmoothed: %3.3f, ball bearing: %3.3f)" % (self._world.ball.seen, self._world.ball.dist, self._world.ball.distSmoothed, self._world.ball.bearing)
print "------------------"
# if kicking-point is not known, search for it
if self.kp is None:
print "kicking-point unknown, searching for ball & opponent goal"
# do a quick search for kicking point
self.searchBallAndGoal()
return
# for now, just directly approach ball and kick it wherever
ballBearing = self._world.ball.bearing
ballDist = self._world.ball.distSmoothed
(ball_x, ball_y) = polar2cart(ballDist, ballBearing)
if self.verbose:
print "ball_x: %3.3fcm, ball_y: %3.3fcm" % (ball_x, ball_y)
# determine kicking leg
side = ballBearing < 0 # 0 = LEFT, 1 = RIGHT
if self.verbose:
if (side == LEFT): print "LEFT"
else: print "RIGHT"
def onBallInFrame(self):
ball = self._world.ball
if ball.seen:
print "ball: centerX %s, centerY %s" % (ball.centerX, ball.centerY)
print "Ball seen!: (ball seen %s, dist: %3.3f, distSmoothed: %3.3f, ball bearing: %3.3f)" % (self._world.ball.seen, self._world.ball.dist, self._world.ball.distSmoothed, self._world.ball.bearing)
(ball_x, ball_y) = polar2cart(self._world.ball.distSmoothed, self._world.ball.bearing)
print "Ball x: %3.3f, Ball y: %3.3f" % (ball_x, ball_y)
def trackBall(self):
new_ball_loc = self._world.ball.centerX, self._world.ball.centerY
if new_ball_loc != self._last_ball_loc:
print "ball at %s" % str(new_ball_loc)
self._last_ball_loc = new_ball_loc
self._actions.executeTracking(self._world.ball)
print "Ball seen!: (ball seen %s, dist: %3.3f, distSmoothed: %3.3f, ball bearing: %3.3f)" % (self._world.ball.seen, self._world.ball.dist, self._world.ball.distSmoothed, self._world.ball.bearing)
(ball_x, ball_y) = polar2cart(self._world.ball.distSmoothed, self._world.ball.bearing)
print "Ball x: %3.3f, Ball y: %3.3f" % (ball_x, ball_y)
def ball_found(self):
print "BALL FOUND"
opposing_lp, opposing_rp = self._world.opposing_lp, self._world.opposing_rp
(ball_x, ball_y) = polar2cart(self._world.ball.distSmoothed, self._world.ball.bearing)
goal = (opposing_lp.bearing + opposing_rp.bearing)/2
(side, kick_parameter) = getKickingType(goal, ball_y, 0)
if side == None or kick_parameter == None or not isInEllipse(ball_x, ball_y, side, 0):
print "ERROR: could not find the right kicking parameter, side: ", side, ", parameter: ", kick_parameter, " x: ", ball_x
(side, kick_parameter) = getKickingType(goal, ball_y, 1)
if side == None or kick_parameter == None or not isInEllipse(ball_x, ball_y, side, 1):
print "ERROR: STILL could not find the right kicking parameter, side: ", side, ", parameter: ", kick_parameter, " x: ", ball_x
else:
self._ballFinder.stop().onDone(lambda: self._actions.adjusted_straight_kick(side , kick_parameter))
else:
print "KICKING"
self._ballFinder.stop().onDone(lambda: self._actions.adjusted_straight_kick(side , kick_parameter))
def onSearchOver(self):
print "Search over"
# if we got here, all searched objects (currently: ball) are assumed to be found.
self._actions.track(self._world.ball, self.onLostBall)
self.kp = True
# for now, just directly approach ball and kick it wherever
ballBearing = self._world.ball.bearing
ballDist = self._world.ball.distSmoothed
(ball_x, ball_y) = polar2cart(ballDist, ballBearing)
print "ball_x: %3.3fcm, ball_y: %3.3fcm" % (ball_x, ball_y)
# determine kicking leg
side = ballBearing < 0 # 0 = LEFT, 1 = RIGHT
if (side == LEFT): print "LEFT"
else: print "RIGHT"
ball_location = self.calcBallArea(ball_x, ball_y, side)
DEBUG_AREA = ('BALL_IN_KICKING_AREA', 'BALL_BETWEEN_LEGS', 'BALL_FRONT_NEAR', 'BALL_FRONT_FAR', 'BALL_SIDE_NEAR', 'BALL_SIDE_FAR', 'BALL_DIAGONAL')
print "AREA: %s" % DEBUG_AREA[ball_location]
self._actions.kick(kick_type=burst.actions.KICK_TYPE_STRAIGHT,kick_leg=LEFT, kick_offset=0.2)
def _approachBall(self):
print ("\nApproaching %s: (recently seen %s, dist: %3.3f, distSmoothed: %3.3f, bearing: %3.3f)"+"\n"+"-"*100) % (
self._target.name, self._target.recently_seen, self._target.dist, self._target.distSmoothed, self._target.bearing)
# TODO: we probably need a better solution? this can happen after we're aligned,
# when ball tracker finds the ball while a previous movement is still ON.
if self._movement_deferred:
print "LAST MOVEMENT STILL ON!!!"
#import pdb; pdb.set_trace()
return
if not self._target.recently_seen:
if self._ballFinder.stopped:
print "TARGET LOST, RESTARTING BALL FINDER"
self.switchToFinder(to_goal_finder=False)
else:
print "TARGET LOST, RELYING ON SEARCHER"
# TODO: searcher / searcher CB should take care of finding target, behavior should take care of turning when search fails
return
(side, kp_x, kp_y, kp_dist, kp_bearing, target_location, kick_side_offset) = calcTarget(self._target.distSmoothed, self._target.bearing)
side = self._side
### DECIDE ON NEXT MOVEMENT ###
# Ball inside kicking area, kick it
if target_location == BALL_BETWEEN_LEGS and (self._aligned_to_goal or not self._align_to_target):
# TODO: diagonalize the kick. It might be off target even if we think we are alligned
self.doKick(side)
return
if target_location in (BALL_IN_KICKING_AREA, BALL_BETWEEN_LEGS) and not self._aligned_to_goal and self._align_to_target and not self._diag_kick_tested:
print "NEW CODE: Searching goal post!"
self._diag_kick_tested = True
(ball_x, ball_y) = polar2cart(self._target.distSmoothed, self._target.bearing)
self._ballY_lastseen = ball_y
self._side_last = side
if self._ballFinder:
self._ballFinder.stop().onDone(lambda: self.doKick(self._side))
return
self._diag_kick_tested = False
# Use circle-strafing when near ball (TODO: area for strafing different from kicking-area)
# TODO: Use 2x circle strafing to get to the ball faster an not so accurate
#if target_location in (BALL_IN_KICKING_AREA, BALL_BETWEEN_LEGS, BALL_FRONT_NEAR) and not self._aligned_to_goal and self._align_to_target:
# self.logverbose("Aligning to goal! (switching to goal finder)")
# self._actions.setCameraFrameRate(20)
# self.switchToFinder(to_goal_finder=True)
# return
if target_location in (BALL_FRONT_NEAR, BALL_FRONT_FAR):
self.logverbose("Walking straight!")
self._actions.setCameraFrameRate(10)
self._movement_type = MOVE_FORWARD
self._movement_location = target_location
if self._obstacle_in_front and target_location == BALL_FRONT_FAR:
opposite_side_from_obstacle = self.getObstacleOppositeSide()
print "opposite_side_from_obstacle: %d" % opposite_side_from_obstacle
# if we do a significant side-stepping, our goal-alignment isn't worth much anymore...
self._aligned_to_goal = False
self._movement_type = MOVE_SIDEWAYS
self._movement_deferred = self._actions.changeLocationRelativeSideways(
0.0, 30.0*opposite_side_from_obstacle, walk=walks.SIDESTEP_WALK)
# self._movement_type = MOVE_CIRCLE_STRAFE
# if opposite_side_from_obstacle == -1:
# strafeMove = self._actions.executeCircleStrafeCounterClockwise
# else:
# strafeMove = self._actions.executeCircleStrafeClockwise
# self._movement_deferred = self._actions.executeCircleStraferInitPose().onDone(strafeMove)
else:
self._movement_deferred = self._actions.changeLocationRelative(min(kp_x*MOVEMENT_PERCENTAGE_FORWARD,MAX_FORWARD_WALK))
elif target_location in (BALL_IN_KICKING_AREA, BALL_SIDE_NEAR):
self.logverbose("Side-stepping!")
self._actions.setCameraFrameRate(10)
movementAmount = min(kp_y*MOVEMENT_PERCENTAGE_SIDEWAYS,MAX_SIDESTEP_WALK)
# if we do a significant side-stepping, our goal-alignment isn't worth much anymore...
if movementAmount > 20:
self._aligned_to_goal = False
self._movement_type = MOVE_SIDEWAYS
self._movement_location = target_location
# TODO: change numbers for side stepping. Does that 4 or 5 times.
self._movement_deferred = self._actions.changeLocationRelativeSideways(
0.0, movementAmount, walk=walks.SIDESTEP_WALK)
elif target_location in (BALL_DIAGONAL, BALL_SIDE_FAR):
self.logverbose("Turning!")
self._aligned_to_goal = False
self._actions.setCameraFrameRate(10)
movementAmount = kp_bearing*MOVEMENT_PERCENTAGE_TURN
# if we do a significant turn, our goal-alignment isn't worth much anymore...
if movementAmount > 10*DEG_TO_RAD:
self._aligned_to_goal = False
self._movement_type = MOVE_TURN
self._movement_location = target_location
self._movement_deferred = self._actions.turn(movementAmount)
else:
self.logverbose("!!!!!!!!!!!!!!!!!!!!!!!!!!! ERROR!!! ball location problematic!")
#import pdb; pdb.set_trace()
print "Movement STARTING!"
self._movement_deferred.onDone(lambda _, nextAction=self._approachBall: self._onMovementFinished(nextAction))
def getTargetPosition(_):
bearing, dist = target.bearing, target.dist
return polar2cart(dist, bearing)
def calcTarget(distSmoothed, bearing):
target_x, target_y = polar2cart(distSmoothed, bearing)
return calcTargetXY(target_x, target_y)
