class TargetFinderTester(InitialBehavior):
def __init__(self, actions):
InitialBehavior.__init__(self, actions=actions, name=self.__class__.__name__)
def _start(self, firstTime=False):
#self.targets=[self._world.opposing_lp, self._world.opposing_rp]
self.targets=[self._world.ball]
self._ballFinder = TargetFinder(actions=self._actions, targets=self.targets, start=True)
self._ballFinder.setOnTargetFoundCB(self.onTargetFound)
self._ballFinder.setOnTargetLostCB(self.onTargetLost)
self._ballFinder.setOnSearchFailedCB(self.onSearchFailed)
def onSearchFailed(self):
self.log('Search Failed')
self._ballFinder.stop().onDone(self.onStopped)
def onTargetFound(self):
self.log('Found it')
self._actions.say('Found it!')
self._ballFinder.stop().onDone(self.onStopped)
def onTargetLost(self):
self.log('Lost it')
self._actions.say('Lost it!')
def onStopped(self):
self.log('Stopped')
self._actions.say('Stopped')
self._eventmanager.callLater(1.0, self._eventmanager.quit)
class DiagonalTester(InitialBehavior):
def __init__(self, actions):
InitialBehavior.__init__(self, actions=actions, name=self.__class__.__name__, initial_pose=poses.STRAIGHT_WALK_INITIAL_POSE)
self._ballFinder = TargetFinder(actions=actions, targets=[self._world.ball], start=False)
self._ballFinder.setOnTargetFoundCB(self.ball_found)
self._ballFinder.setOnTargetLostCB(self.ball_lost)
def _start(self, firstTime=False):
self._actions.executeHeadMove(poses.HEAD_MOVE_FRONT_FAR).onDone(self.find_goal)
def find_goal(self):
self._actions.localize().onDone(self.found_goal)
def found_goal(self):
if fabs(self._world.opposing_lp.bearing - self._world.opposing_rp.bearing)<0.05:
print "ERROR: Goal posts are the same one..., left post is: ", self._world.opposing_lp.bearing, " right post is: ", self._world.opposing_rp.bearing
lambda: self._eventmanager.callLater(0.5, self._start)
else:
opposing_lp, opposing_rp = self._world.opposing_lp, self._world.opposing_rp
t = self._world.time
dtl, dtr = t - opposing_lp.update_time, t - opposing_rp.update_time
print "Goal Posts Bearings: %3.2f, %3.2f (seens %s, %s), (updated %3.2f seconds ago)" % (
opposing_lp.bearing, opposing_rp.bearing, opposing_lp.seen, opposing_rp.seen, max(dtl, dtr))
self._actions.executeHeadMove(poses.HEAD_MOVE_FRONT_BOTTOM).onDone(
lambda: self._eventmanager.callLater(0.5, self._ballFinder.start))
#self._eventmanager.register(self.printBall, EVENT_BALL_IN_FRAME)
print "So, goal center is ", (opposing_lp.bearing + opposing_rp.bearing)/2
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 ball_lost(self):
print "BALL LOST"
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)
class FPSTester(InitialBehavior):
def __init__(self, actions):
InitialBehavior.__init__(self, actions=actions, name=self.__class__.__name__)
def _start(self, firstTime=False):
self.targets=[self._world.ball]
self._actions.setCameraFrameRate(20)
self._ballFinder = TargetFinder(actions=self._actions, targets=self.targets, start=True)
self._ballFinder.setOnTargetFoundCB(self.onTargetFound)
self._ballFinder.setOnTargetLostCB(self.onTargetLost)
def onTargetFound(self):
self.log('Found it (changing to FPS 1)')
self._actions.setCameraFrameRate(1)
def onTargetLost(self):
self.log('Lost it (changing to FPS 20)')
self._actions.setCameraFrameRate(20)
class InitialKicker(Behavior):
def __init__(self, actions, align_to_target=True, side=LEFT):
super(InitialKicker, self).__init__(actions = actions, name = 'InitialKicker')
self.verbose = True
self._side = side
self._align_to_target = align_to_target
self._sonar = self._world.robot.sonar
self._eventmanager.register(self.onObstacleSeen, EVENT_OBSTACLE_SEEN)
self._eventmanager.register(self.onObstacleLost, EVENT_OBSTACLE_LOST)
self._eventmanager.register(self.onObstacleInFrame, EVENT_OBSTACLE_IN_FRAME)
self._ballFinder = TargetFinder(actions=actions, targets=[self._world.ball], start=False)
self._ballFinder.setOnTargetFoundCB(self._approachBall)
self._ballFinder.setOnTargetLostCB(self._stopOngoingMovement)
#self.target_left_right_posts = target_left_right_posts
#self._goalFinder = TargetFinder(actions=actions, targets=self.target_left_right_posts, start=False)
#self._goalFinder.setOnTargetFoundCB(self.onGoalFound)
self._currentFinder = None
def _start(self, firstTime=False):
self._aligned_to_goal = False
self._diag_kick_tested = False
self._movement_deferred = None
self._movement_type = None
self._movement_location = None
self._is_strafing = False
self._is_strafing_init_done = False
self._obstacle_in_front = None
self._target = self._world.ball
self._actions.setCameraFrameRate(20)
# kicker initial position
self._actions.executeMove(poses.STRAIGHT_WALK_INITIAL_POSE).onDone(
lambda: self.switchToFinder(to_goal_finder=False))
def _stop(self):
print "KICKING STOPS!!!"
self._clearMovement(clearFootsteps = True)
stop_bd = succeedBurstDeferred(self)
if self._currentFinder:
print "STOPPING CURRENT FINDER: %s" % self._currentFinder.name
stop_bd = self._currentFinder.stop()
return stop_bd
################################################################################
# Handling movements
#
def _clearMovement(self, clearFootsteps = False):
if self._movement_deferred:
self._movement_deferred.clear()
if clearFootsteps and self._movement_type in (MOVE_FORWARD, MOVE_SIDEWAYS, MOVE_TURN, MOVE_ARC):
print "CLEARING FOOTSTEPS!"
self._actions.clearFootsteps()
self._movement_deferred = None
self._movement_type = None
self._movement_location = None
def _onMovementFinished(self, nextAction):
print "Movement DONE!"
self._clearMovement(clearFootsteps = False)
nextAction()
def _stopOngoingMovement(self, forceStop = False):
# stop movement if we're forced or if it's a long walk-forward move
shouldStopMovement = forceStop or (self._movement_type == MOVE_FORWARD and self._movement_location == BALL_FRONT_FAR)
if shouldStopMovement:
self._clearMovement(clearFootsteps = True)
print "Kicking: _stopOngoingMovement: current movement %s (forceStop = %s)" % (shouldStopMovement and "STOPPED" or "CONTINUES", forceStop)
################################################################################
# Sonar callbacks
#
def onObstacleSeen(self):
self._obstacle_in_front = self._sonar.getLastReading()
print "Obstacle seen (on %s, distance of %f)!" % (self._obstacle_in_front)
if self._movement_deferred:
# if walking forward and ball is far, stop
if self._movement_type == MOVE_FORWARD and self._movement_location == BALL_FRONT_FAR:
print "NOTE: Obstacle seen while a movement is in progress, movement STOPPED"
self._stopOngoingMovement(forceStop = True)
self._eventmanager.callLater(0.5, self._approachBall)
else:
print "NOTE: Obstacle seen while a movement is in progress, movement CONTINUES"
def onObstacleLost(self):
print "Obstacle lost!"
self._obstacle_in_front = None
def onObstacleInFrame(self):
#print "Obstacle in frame!"
self._obstacle_in_front = self._sonar.getLastReading()
#print "Obstacle seen (on %s, distance of %f)!" % (self._obstacle_in_front)
def getObstacleOppositeSide(self):
if self._obstacle_in_front == None:
print "NO OBSTACLE DATA?"
opposite_side_from_obstacle = 0
elif self._obstacle_in_front[0] == "center":
opposite_side_from_obstacle = random.choice((-1,1))
elif self._obstacle_in_front[0] == "left":
opposite_side_from_obstacle = -1
elif self._obstacle_in_front[0] == "right":
opposite_side_from_obstacle = 1
return opposite_side_from_obstacle
################################################################################
# _approachBall helpers (XXX - should they be submethods of _approachBall? would
# make it cleared to understand the relationship, not require this comment)
#
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 doKick(self, side):
# Look for goal, decide if can skip adjustments and kick ball diagonally
self.logverbose("Kicking!")
if self._currentFinder:
self._currentFinder.stop()
self._currentFinder = None
# self.logverbose("kick_side_offset: %3.3f" % (kick_side_offset))
self._actions.setCameraFrameRate(10)
self._movement_type = MOVE_KICK
self._movement_location = BALL_BETWEEN_LEGS
#if self._obstacle_in_front and self._obstacle_in_front[0] == "center":
# TODO: Change to angle-kick towards left/right side of goal (except for Goalie)
self._movement_deferred = self._actions.inside_kick(burst.actions.KICK_TYPE_INSIDE, side)
#else:
# self._movement_deferred = self._actions.adjusted_straight_kick(side, kick_side_offset)
self._movement_deferred.onDone(self.stop)
def switchToFinder(self, to_goal_finder=False):
from_finder = self._ballFinder
to_finder = self._ballFinder
# if to_goal_finder:
# from_finder, to_finder = self._ballFinder, self._ballFinder
# else:
# switch to bottom camera when we look for the ball
# --- DONT DO THIS UNTIL IMOPS CODE DOES THE SWITCHING, or segfault for you ---
#self._actions.setCamera(burst_consts.CAMERA_WHICH_BOTTOM_CAMERA)
pass
stop_bd = from_finder.stop()
print "SwitchToFinder: calling %s.start" % (to_finder.name)
self._currentFinder = to_finder
# TODO: Yet More Hacks
doit = lambda f: self._eventmanager.callLater(0.0, f) if not stop_bd.completed() else stop_bd.onDone(f)
doit(to_finder.start)
################################################################################
# Checking goal position before kicking
# def searchGoalPost(self):
# self._currentFinder = None
# self._actions.searcher.search_one_of(self.target_left_right_posts, center_on_targets=False).onDone(self.onSearchGoalPostOver)
#def onSearchGoalPostOver(self):
# self._actions.say('onSearchGoalPostOver')
#
# # calculate target bearing (position inside goal)
# nearestGoalpost = None
# if self.target_left_right_posts[0].centered_self.sighted:
# nearestGoalpost = self.target_left_right_posts[0]
# if self.target_left_right_posts[1].centered_self.sighted:
# if not nearestGoalpost or abs(nearestGoalpost.bearing) > self.target_left_right_posts[1].bearing:
# nearestGoalpost = self.target_left_right_posts[1]
#
# if nearestGoalpost is None:
# self._eventmanager.callLater(0.0, self._approachBall)
# return
#
# # Add offset to the diagonal kick (so we'll align not on the actual goalpost, but on about 1/4 of the goal)
# targetBearing = nearestGoalpost.bearing
# if nearestGoalpost == self._world.opposing_lp:
# targetBearing = targetBearing + 0.5/2
# elif nearestGoalpost == self._world.opposing_rp:
# targetBearing = targetBearing - 0.5/2 # TODO: Move to const, calibrate value (cover half-goal? goal? differs for different distances?)
#
# # check if diagonal kick is viable
# # use self._ballY_lastseen
# kick_side_offset = getKickingType(self, targetBearing, self._ballY_lastseen, self._side_last, margin=0)
# if kick_side_offset is None:
# self._eventmanager.callLater(0.0, self._approachBall)
# else:
# # do diagonal kick
# self.doKick(self._side_last)
################################################################################
# Strafing
#def onGoalFound(self):
#
# self.logverbose('onGoalFound')
# if not self._is_strafing:
# self.goalpost_to_track = self._goalFinder.getTargets()[0]
# # Add offset to the goalpost align (so we'll align not on the actual goalpost, but on about 1/4 of the goal)
# if self.goalpost_to_track == self._world.opposing_lp:
# self.alignLeftLimit = -0.5
# self.alignRightLimit = 0
# elif self.goalpost_to_track == self._world.opposing_rp:
# self.alignLeftLimit = 0
# self.alignRightLimit = 0.5 # TODO: Move to const, calibrate value (cover half-goal? goal? differs for different distances?)
# g = self.goalpost_to_track
# self.logverbose('onGoalFound: found %s at %s, %s (%s)' % (g.name, g.centerX, g.centerY, g.seen))
# self.strafe()
#def strafe(self):
# self._is_strafing = True
# if not self.goalpost_to_track.seen:
# self.logverbose("strafe: goal post not seen")
# # Eran: Needed? won't goal-post searcher wake us up? Can't this create a case where strafe is called twice?
# self._eventmanager.callLater(self._eventmanager.dt, self.strafe)
# return
# self.logverbose("strafe: goal post seen")
# self.logverbose("%s bearing is %s. Left is %s, Right is %s" % (self.goalpost_to_track.name, self.goalpost_to_track.bearing, self.alignLeftLimit, self.alignRightLimit))
# # TODO: Add align-to-goal-center support
# if self.goalpost_to_track.bearing < self.alignLeftLimit:
# strafeMove = self._actions.executeCircleStrafeClockwise
# elif self.goalpost_to_track.bearing > self.alignRightLimit:
# strafeMove = self._actions.executeCircleStrafeCounterClockwise
# else:
# self._is_strafing = False
# self._is_strafing_init_done = False
# self.logverbose("Aligned position reached! (starting ball search)")
# self._aligned_to_goal = True
# self._actions.setCameraFrameRate(20)
# if self._goalFinder:
# self._goalFinder.stop().onDone(self.refindBall)
# else:
# self.refindBall()
# return
# # TODO: FPS=10 removed for now (for accurate feedback), might be needed for stable circle-strafing!
# #self._actions.setCameraFrameRate(10)
# self._movement_type = MOVE_CIRCLE_STRAFE
# self._movement_location = BALL_BETWEEN_LEGS
# if not self._is_strafing_init_done:
# self.logverbose("Aligning and strafing...")
# self._is_strafing_init_done = True
# self._movement_deferred = self._actions.executeCircleStraferInitPose().onDone(strafeMove)
# else:
# self.logverbose("Strafing...")
# self._movement_deferred = strafeMove()
# # We use call later to allow the strafing to handle the correct image (otherwise we get too much strafing)
# nextAction = lambda _: self._onMovementFinished(lambda: self._eventmanager.callLater(0.2, self.strafe))
# print "Movement STARTING! (strafing)"
# self._movement_deferred.onDone(nextAction)
def refindBall(self):
self._currentFinder = None
self._actions.executeHeadMove(poses.HEAD_MOVE_FRONT_BOTTOM).onDone(
# TODO: Fix distSmooth after moving head - this is just a workaround
lambda: self._eventmanager.callLater(0.5,
lambda: self.switchToFinder(to_goal_finder=False)))
class BallKicker(Behavior):
def __init__(self, actions, target_left_right_posts, align_to_target=True):
super(BallKicker, self).__init__(actions = actions, name = 'BallKicker')
self.verbose = True
self._align_to_target = align_to_target
self._sonar = self._world.robot.sonar
self._eventmanager.register(self.onObstacleSeen, EVENT_OBSTACLE_SEEN)
self._eventmanager.register(self.onObstacleLost, EVENT_OBSTACLE_LOST)
self._eventmanager.register(self.onObstacleInFrame, EVENT_OBSTACLE_IN_FRAME)
self._ballFinder = TargetFinder(actions=actions, targets=[self._world.ball], start=False)
self._ballFinder.setOnTargetFoundCB(self._approachBall)
self._ballFinder.setOnTargetLostCB(self._stopOngoingMovement)
self._ballFinder.setOnSearchFailedCB(self._onBallSearchFailed)
self.target_left_right_posts = target_left_right_posts
self._goalFinder = TargetFinder(actions=actions, targets=self.target_left_right_posts, start=False)
self._goalFinder.setOnTargetFoundCB(self.onGoalFound)
self._goalFinder.setOnSearchFailedCB(self._onGoalSearchFailed)
self._currentFinder = None
def _start(self, firstTime=False):
self._aligned_to_goal = True
self._diag_kick_tested = False
self._movement_deferred = None
self._movement_type = None
self._movement_location = None
self._is_strafing = False
self._is_strafing_init_done = False
self._obstacle_in_front = None
self._target = self._world.ball
self._initBallMovements()
self._numGoalFailedStrafes = GOAL_FAILURES_BEFORE_GIVEUP # do this number, then giveup, walk 1 m, return to approachBall
# kicker initial position
self._actions.executeMove(poses.STRAIGHT_WALK_INITIAL_POSE).onDone(
lambda: self.switchToFinder(to_goal_finder=False))
def _stop(self):
print "KICKING STOPS!!!"
self._clearMovement(clearFootsteps = True)
stop_bd = succeedBurstDeferred(self)
if self._currentFinder:
print "STOPPING CURRENT FINDER: %s" % self._currentFinder.name
stop_bd = self._currentFinder.stop()
return stop_bd
################################################################################
# Handling movements
#
def _clearMovement(self, clearFootsteps = False):
if self._movement_deferred:
self._movement_deferred.clear()
if clearFootsteps and self._movement_type in (MOVE_FORWARD, MOVE_SIDEWAYS, MOVE_TURN, MOVE_ARC):
print "CLEARING FOOTSTEPS!"
self._actions.clearFootsteps()
self._movement_deferred = None
self._movement_type = None
self._movement_location = None
def _onMovementFinished(self, nextAction):
print "Movement DONE!"
self._clearMovement(clearFootsteps = False)
nextAction()
def _stopOngoingMovement(self, forceStop = False):
# stop movement if we're forced or if it's a long walk-forward move
shouldStopMovement = forceStop or (self._movement_type == MOVE_FORWARD and self._movement_location == BALL_FRONT_FAR)
if shouldStopMovement:
self._clearMovement(clearFootsteps = True)
print "Kicking: _stopOngoingMovement: current movement %s (forceStop = %s)" % (shouldStopMovement and "STOPPED" or "CONTINUES", forceStop)
################################################################################
# Sonar callbacks
#
def onObstacleSeen(self):
self._obstacle_in_front = self._sonar.getLastReading()
print "Obstacle seen (on %s, distance of %f)!" % (self._obstacle_in_front)
if self._movement_deferred:
# if walking forward and ball is far, stop
if self._movement_type == MOVE_FORWARD and self._movement_location == BALL_FRONT_FAR:
print "NOTE: Obstacle seen while a movement is in progress, movement STOPPED"
self._stopOngoingMovement(forceStop = True)
self._eventmanager.callLater(0.5, self._approachBall)
else:
print "NOTE: Obstacle seen while a movement is in progress, movement CONTINUES"
def onObstacleLost(self):
print "Obstacle lost!"
self._obstacle_in_front = None
def onObstacleInFrame(self):
#print "Obstacle in frame!"
self._obstacle_in_front = self._sonar.getLastReading()
#print "Obstacle seen (on %s, distance of %f)!" % (self._obstacle_in_front)
def getObstacleOppositeSide(self):
if self._obstacle_in_front == None:
print "NO OBSTACLE DATA?"
opposite_side_from_obstacle = 0
elif self._obstacle_in_front[0] == "center":
opposite_side_from_obstacle = random.choice((-1,1))
elif self._obstacle_in_front[0] == "left":
opposite_side_from_obstacle = -1
elif self._obstacle_in_front[0] == "right":
opposite_side_from_obstacle = 1
return opposite_side_from_obstacle
################################################################################
# Ball finding functionality - better then just plain old search!
#
#
def _initBallMovements(self):
self._ball_search_first_failure = True
def _onBallSearchFailed(self):
# we are here after searching for a ball. Just make sure it did actually fail.
if self._world.ball in self._actions.searcher.seen_objects:
print "@ _onBallSearchFailed called, but ball is visible - proceeding as usual"
return self._approachBall()
print "@ Ball Search Failed!!!! Stopping ball Finder"
self._ballFinder.stop()
if self._movement_deferred:
print "@ Movement Deferred - waiting for it to complete and not doing anything here"
return
if self._ball_search_first_failure:
self._ball_search_first_failure = False
print "@ Turning, and rerunning search"
self._movement_deferred = self._actions.turn(pi)
self._movement_type = MOVE_TURN
self._movement_deferred.onDone(lambda: self._onMovementFinished(self._ballSearch_restart))
else:
# TODO
self._onSecondSearchFail_LocalizeStrategy()
# TODO
#self._onSecondSearchFail_GotoGoalStrategy()
def _ballSearch_restart(self):
print "@ Ball search: restarting search"
self.switchToFinder(to_goal_finder=False)
# Own Goal Seen Strategy: Turn towards closest goal, walk forward
# according to dist Strategy
def _onSecondSearchFail_GotoGoalStrategy(self):
print "TODO - starting ball finder again."
self._ballFinder.start()
# Localization strategy: localize, then move to location
def _onSecondSearchFail_LocalizeStrategy(self):
# not first time, localize
print "@ Calling localize"
self._actions.localize().onDone(self._onSecondSearchFail_LocalizeStrategy_LocalizeOver)
def _onSecondSearchFail_LocalizeStrategy_LocalizeOver():
# so we are localized.
# Let's see where we are:
print "In SecondSearchFaile, LocalizeStrategy, Localize Over"
x, y = self.robot.world_x, self.robot.world_y
if x > MIDFIELD_X:
target_x, target_y = 180.0, 0.0 # Our penalty
else:
target_x, target_y = MIDFIELD_X + 120.0, 0.0 # Their penalty
import approacher
dx, dy, dh = approacher.getTargetPosition(target_x, target_y, 0.0) # heading towards opposite goal regardles of penalty.
self._actions.changeLocationRelative(dx, dy, dh).onDone(self._ballSearch_restart)
################################################################################
# _approachBall helpers (XXX - should they be submethods of _approachBall? would
# make it cleared to understand the relationship, not require this comment)
#
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:
#import pdb; pdb.set_trace()
if not self._actions.isMotionInProgress() and not self._actions.isWalkInProgress():
print "ignoring current movement deferred and erasing it"
self._clearMovement(clearFootsteps = False)
else:
print "LAST MOVEMENT STILL ON!!!"
return
if not self._target.recently_seen:
if self._ballFinder.stopped:
print "TARGET LOST, STARTING BALL FINDER"
self.switchToFinder(to_goal_finder=False)
else:
print "TARGET LOST, RESTARTING BALL FINDER"
self._ballFinder.stop() # TODO - onDone
self.switchToFinder(to_goal_finder=False)
# 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)
### DECIDE ON NEXT MOVEMENT ###
# Ball inside kicking area, kick it
if target_location == BALL_IN_KICKING_AREA: # 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 aligned
print "DIAGONAL KICK: Searching goal post 1"
self._diag_kick_tested = True
self._diag_kick_forced = True
(ball_x, ball_y) = polar2cart(self._target.distSmoothed, self._target.bearing)
self._ballY_lastseen = ball_y
self._side_last = side
self._kick_side_offset = kick_side_offset
#if self._ballFinder:
# self._ballFinder.stop().onDone(self.searchGoalPost)
# return
#else:
# self.log("NO BALL FINDER 1???")
self.doKick(side, kick_side_offset)
return
if target_location in (BALL_FRONT_NEAR, BALL_FRONT_FAR):
self.logverbose("Walking straight!")
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(max(kp_x*MOVEMENT_PERCENTAGE_FORWARD,MIN_FORWARD_WALK),MAX_FORWARD_WALK))
elif target_location in (BALL_BETWEEN_LEGS, BALL_SIDE_NEAR):
self.logverbose("Side-stepping!")
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
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 doKick(self, side, kick_side_offset):
# Look for goal, decide if can skip adjustments and kick ball diagonally
self.logverbose("Kicking!")
if self._currentFinder:
self._currentFinder.stop()
self._currentFinder = None
self.logverbose("kick_side_offset: %3.3f" % (kick_side_offset))
self._movement_type = MOVE_KICK
self._movement_location = BALL_IN_KICKING_AREA
if self._obstacle_in_front and self._obstacle_in_front[0] == "center":
# TODO: Change to angle-kick towards left/right side of goal (except for Goalie)
self._movement_deferred = self._actions.inside_kick(burst.actions.KICK_TYPE_INSIDE, side)
else:
self._movement_deferred = self._actions.adjusted_straight_kick(side, kick_side_offset)
self._movement_deferred.onDone(self.stop)
def switchToFinder(self, to_goal_finder=False):
from_finder, to_finder = self._goalFinder, self._ballFinder
if to_goal_finder:
from_finder, to_finder = self._ballFinder, self._goalFinder
else:
# switch to bottom camera when we look for the ball
# --- DONT DO THIS UNTIL IMOPS CODE DOES THE SWITCHING, or segfault for you ---
#self._actions.setCamera(burst_consts.CAMERA_WHICH_BOTTOM_CAMERA)
pass
stop_bd = from_finder.stop()
print "SwitchToFinder: calling %s.start" % (to_finder.name)
self._currentFinder = to_finder
# TODO: Yet More Hacks
doit = lambda f: self._eventmanager.callLater(0.0, f) if not stop_bd.completed() else stop_bd.onDone(f)
if not to_finder.stopped:
print "SwitchToFinder: Target finder not stopped. Stopping it."
to_finder.stop()
self._eventmanager.callLater(0.5, to_finder.start)
else:
doit(to_finder.start)
################################################################################
# Checking goal position before kicking
def searchGoalPost(self):
self._currentFinder = None
self._actions.searcher.search_one_of(self.target_left_right_posts, center_on_targets=False).onDone(self.onSearchGoalPostOver)
def onSearchGoalPostOver(self):
self._actions.say('onSearchGoalPostOver')
# calculate target bearing (position inside goal)
nearestGoalpost = None
if self.target_left_right_posts[0].centered_self.sighted:
nearestGoalpost = self.target_left_right_posts[0]
if self.target_left_right_posts[1].centered_self.sighted:
if not nearestGoalpost or abs(nearestGoalpost.bearing) > self.target_left_right_posts[1].bearing:
nearestGoalpost = self.target_left_right_posts[1]
if nearestGoalpost is None:
self.logverbose("LOST GOAL WHILE TRYING TO KICK?! (switching to goal finder)")
self._aligned_to_goal = False
self._diag_kick_tested = True
if self._diag_kick_forced:
self.logverbose("SUPPOSED TO BE ALIGNED BUT CAN'T SEE GOAL! KICKING ANYWAY!")
self.doKick(self._side_last, self._kick_side_offset)
return
self._onGoalSearchFailed()
#self._eventmanager.callLater(0.0, self._approachBall)
#TODO: RESTART GOAL/Ball TARGET_FINDER
return
self._actions.headTowards(nearestGoalpost).onDone(lambda _, nearestGoalpost=nearestGoalpost:
self.onSearchGoalPostOverAfterHeadTowards(nearestGoalpost))
def onSearchGoalPostOverAfterHeadTowards(self, nearestGoalpost):
print "AFTER HEAD MOVE TOWARDS %s" % (nearestGoalpost.name)
# Add offset to the diagonal kick (so we'll align not on the actual goalpost, but on about 1/4 of the goal)
targetBearing = nearestGoalpost.bearing
if nearestGoalpost == self._world.opposing_lp:
targetBearing = targetBearing + 0.5/3
elif nearestGoalpost == self._world.opposing_rp:
targetBearing = targetBearing - 0.5/3 # TODO: Move to const, calibrate value (cover half-goal? goal? differs for different distances?)
# check if diagonal kick is viable
# use self._ballY_lastseen
kick_side_offset = getKickingType(self, targetBearing, self._ballY_lastseen, self._side_last, margin=0)
if kick_side_offset is None:
if self._diag_kick_forced:
self.logverbose("SUPPOSED TO BE ALIGNED BUT CAN'T SEE GOAL! KICKING ANYWAY!")
self.doKick(self._side_last, self._kick_side_offset)
return
self._eventmanager.callLater(0.0, self._approachBall)
else:
# do diagonal kick
self.doKick(self._side_last, kick_side_offset)
################################################################################
# Strafing
def onGoalFound(self):
self.logverbose('onGoalFound')
if not self._is_strafing:
self.goalpost_to_track = self._goalFinder.getTargets()[0]
# Add offset to the goalpost align (so we'll align not on the actual goalpost, but on about 1/4 of the goal)
if self.goalpost_to_track == self._world.opposing_lp:
self.alignLeftLimit = -0.5
self.alignRightLimit = 0
elif self.goalpost_to_track == self._world.opposing_rp:
self.alignLeftLimit = 0
self.alignRightLimit = 0.5 # TODO: Move to const, calibrate value (cover half-goal? goal? differs for different distances?)
g = self.goalpost_to_track
self.logverbose('onGoalFound: found %s at %s, %s (%s)' % (g.name, g.centerX, g.centerY, g.seen))
self.strafe()
def _onGoalSearchFailed(self):
self._numGoalFailedStrafes -= 1
if self._numGoalFailedStrafes <= 0:
print "@ Goal not found, but limit of strafes reached - 1 m forward and back to approach"
self._numGoalFailedStrafes = GOAL_FAILURES_BEFORE_GIVEUP
self.switchToFinder(to_goal_finder=False)
self._movement_type = MOVE_FORWARD
self._movement_deferred = self._actions.changeLocationRelative(MOVE_WHEN_GOAL_LOST_GOOD)
self._movement_deferred.onDone(lambda _, nextAction=self._approachBall: self._onMovementFinished(nextAction))
return
print "@ Goal not found - doing some strafing (how do I do this for 180 degrees?) - %s left" % self._numGoalFailedStrafes
#self._actions.searcher.stop()
self._movement_type = MOVE_TURN
#self._actions.turn(pi)
num_circle_strafes = 8
self._movement_deferred = self._actions.executeCircleStraferInitPose()
for i in xrange(num_circle_strafes):
self._movement_deferred = self._movement_deferred.onDone(self._actions.executeCircleStrafeCounterClockwise)
#self._movement_deferred = self._actions.turn(pi)
self._movement_deferred.onDone(lambda: self._onMovementFinished)
self._movement_deferred.onDone(self.restartGoalFinderAfterFailure)
def restartGoalFinderAfterFailure(self):
self.log("Restarting goal finder")
self._eventmanager.callLater(0.5, lambda: self.switchToFinder(to_goal_finder=True))
count_strafe = 1
def strafe(self):
self._is_strafing = True
if not self.goalpost_to_track.seen:
self.logverbose("strafe: goal post not seen")
# Eran: Needed? won't goal-post searcher wake us up? Can't this create a case where strafe is called twice?
self._eventmanager.callLater(self._eventmanager.dt, self.strafe)
return
self.logverbose("strafe: goal post seen")
self.logverbose("%s bearing is %s (seen %s, all %s). Left is %s, Right is %s" % (
self.goalpost_to_track.name,
self.goalpost_to_track.bearing,
self.goalpost_to_track.seen, str(['%3.2f (c %3.2f) %s %s' % (x.bearing,
x.centered_self.bearing,
'seen' if x.seen else 'not seen', 'centered' if x.centered else 'not centered')
for x in self._world.opposing_goal.left_right_unknown]),
self.alignLeftLimit,
self.alignRightLimit))
# TODO: Add align-to-goal-center support
if self.goalpost_to_track.bearing < self.alignLeftLimit:
#strafeMove = lambda: self._actions.executeCircleStrafeClockwise().onDone(self._actions.executeCircleStrafeClockwise)
strafeMove = self._actions.executeCircleStrafeClockwise
print "#### About to do a clockwise strafe"
elif self.goalpost_to_track.bearing > self.alignRightLimit:
#strafeMove = lambda: self._actions.executeCircleStrafeCounterClockwise().onDone(self._actions.executeCircleStrafeCounterClockwise)
strafeMove = self._actions.executeCircleStrafeCounterClockwise
print "#### About to do a counter clockwise strafe"
else:
self._is_strafing = False
self._is_strafing_init_done = False
self.logverbose("Aligned position reached! (starting ball search)")
self._aligned_to_goal = True
if self._goalFinder:
self._goalFinder.stop().onDone(self.refindBall)
else:
self.refindBall()
return
self._movement_type = MOVE_CIRCLE_STRAFE
self._movement_location = BALL_BETWEEN_LEGS
if not self._is_strafing_init_done:
self.logverbose("Aligning and strafing...")
self._is_strafing_init_done = True
self._movement_deferred = self._actions.executeCircleStraferInitPose().onDone(strafeMove)
else:
self.logverbose("Strafing...")
self._movement_deferred = strafeMove()
# We use call later to allow the strafing to handle the correct image (otherwise we get too much strafing)
nextAction = lambda _: self._onMovementFinished(lambda: self._eventmanager.callLater(0.2, self.strafe))
print "Movement STARTING! (strafing)"
self._movement_deferred.onDone(nextAction)
def refindBall(self):
self._currentFinder = None
self._actions.executeHeadMove(poses.HEAD_MOVE_FRONT_BOTTOM).onDone(
# TODO: Fix distSmooth after moving head - this is just a workaround
lambda: self._eventmanager.callLater(0.5,
lambda: self.switchToFinder(to_goal_finder=False)))
class Goalie(InitialBehavior):
def __init__(self, actions):
InitialBehavior.__init__(self, actions=actions, name=self.__class__.__name__, initial_pose=poses.SIT_POS)
self._world.ball.shouldComputeIntersection = True
self.targetFinder = TargetFinder(actions=self._actions, targets=[self._world.ball], start=False)
self.targetFinder.setOnTargetFoundCB(self.targetFound)
self.targetFinder.setOnTargetLostCB(self.targetLost)
self.targetFinder.setOnSearchFailedCB(self.searchFailed)
def _start(self, firstTime = False):
self.targetLostTime = self._world.time
self.targetFoundTime = self.targetLostTime + SEARCH_TIME + 0.1
self._eventmanager.register(self.leap, EVENT_BALL_BODY_INTERSECT_UPDATE)
self.readyToLeap()
def readyToLeap(self):
self._actions.say("Ready to leap")
self._actions.setCameraFrameRate(20)
self.isLeaping = False
self.targetFinder.start()
def targetFound(self):
self.targetFoundTime = self._world.time
def targetLost(self):
self.targetLostTime = self._world.time
def searchFailed(self):
print "at searchFailed"
# ball search failed, needs to be restarted
if self._actions.searcher.stopped:
self.log("Restarting target finder since searcher is stopped")
if not self.targetFinder.stopped:
self.log("TODO - targetFinder should have been stopped at searchFailure")
self.targetFinder.stop()
self._eventmanager.callLater(0.5, self.targetFinder.start)
leap_time = -1
def leap(self, stopped=False):
if self.isLeaping: return
if self.targetFoundTime - self.targetLostTime > SEARCH_TIME:
if self.targetFoundTime + WAITING_FOR_NEW_DATA > self._world.time: #waiting WAITING_FOR_NEW_DATA: meaning not entering the method
print "LEAP ABORTED, NOT ENOUGH DATA!"
return
print "Checking if leap is necessary"
if self._world.time == self.leap_time:
print "LEAP TWICE IN SAME TURN!!!! BUG!!!!"
return
self.leap_time = self._world.time
if self._world.ball.isTimeCalc and not \
(FAST_BALL_TIME < self._world.ball.time_intersection < SLOW_BALL_TIME):
print "According to time calculation, decided not to leap"
print "Time of arrival: ", self._world.ball.time_intersection
return
if (-(HALF_GOAL_WIDTH + ERROR_IN_LENGTH) < self._world.ball.body_isect < 0):
print "LEAPING RIGHT************###############"
self.isLeaping = True
self.targetFinder.stop()
if realLeap:
print "real leap right safe"
self._player.unregisterFallHandling()
self._actions.executeLeapRight().onDone(self.gettingUpRight)
else:
self._actions.say("right.")
self.gettingUpRight()
elif 0 < self._world.ball.body_isect < (HALF_GOAL_WIDTH + ERROR_IN_LENGTH):
print "LEAPING LEFT@@@@@@@@@@@@@@@@@@@@@@@@@@"
self.isLeaping = True
self.targetFinder.stop()
if realLeap:
print "real leap left safe"
self._player.unregisterFallHandling()
self._actions.executeLeapLeft().onDone(self.gettingUpLeft)
else:
self._actions.say("left.")
self.gettingUpLeft()
else:
print "Decided not to leap right now..."
def gettingUpRight(self):
print "getting up right"
if realLeap:
self._actions.executeToBellyFromLeapRight().onDone(lambda _=None: self.getUpBelly())
else:
self.onGettingUpDone()
def gettingUpLeft(self):
print "getting up left"
if realLeap:
self._actions.executeToBellyFromLeapLeft().onDone(lambda _=None: self.getUpBelly())
else:
self.onGettingUpDone()
def getUpBelly(self):
self._actions.executeGettingUpBelly().onDone(lambda _=None: self.onGettingUpDone())
def onGettingUpDone(self):
print "Getting up done"
self._player.registerFallHandling()
if realLeap:
# AlignmentAfterLeap(self._actions, side).start().onDone(lambda _=None: self._actions.executeMove(poses.SIT_POS).onDone(self.readyToLeap))
#AlignmentAfterLeap(self._actions, side).start().onDone(self.readyToLeap)
if not self._player._main_behavior.stopped:
print "stopping existing behavior before turning into Kicker"
self._player._main_behavior.stop() # TODO - use returned bd
self._player.turnToKicker()
else:
self.readyToLeap()