def tick(blackboard):
"""
This is the main entry point from C++ into our Python behaviours and back.
More specifically it is the bridge from which C++ calls Python inside
the runswift executable, and receives a BehaviourRequest back.
Currently called in `robot/perception/behaviour/python/PythonSkill.cpp`,
the `PythonSkill::execute()` C++ function, and explicitly the line
`behaviour_tick(bb)`.
:param blackboard: The runswift Blackboard, a bunch of things
stored in global memory.
:return: A `robot.BehaviourRequest()` instance, defined in C++ inside
`robot/types/BehaviourRequest.hpp`.
"""
# Update all blackboard dependent helper modules.
Global.update(blackboard)
TeamStatus.update(blackboard)
FieldGeometry.update(blackboard)
Timer.update(blackboard)
LedOverride.reset()
Sonar.update(blackboard)
# Set the HeadSkill
HeadSkill.singleton.resetRequestState()
global skill_instance
if not skill_instance:
skill_instance = skill_instance_factory(blackboard)
if isinstance(skill_instance, BehaviourTask):
# On every tick of the perception thread, we update the blackboard,
# tick the skill, and then return the resulting behaviour request.
skill_instance.world.update(blackboard)
skill_instance.world.b_request = robot.BehaviourRequest()
skill_instance.world.b_request.actions = robot.All()
skill_instance.tick()
request = skill_instance.world.b_request
else:
# Backwards compat for old style skills if called directly via -s.
request = skill_instance.tick(blackboard)
headRequest = HeadSkill.singleton.tick(blackboard)
request.actions.head = headRequest.actions.head
# LED colouring.
if len(blackboard.vision.uncertain_balls) > 0:
request.actions.leds.rightEye = LEDColour.blue
elif len(blackboard.vision.balls) > 0:
request.actions.leds.rightEye = LEDColour.red
else:
request.actions.leds.rightEye = LEDColour.off
if Global.amILost():
request.actions.leds.leftEye = LEDColour.off
else:
request.actions.leds.leftEye = LEDColour.cyan
return request
def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
extraObstacles=[]):
# LedOverride.override(LedOverride.leftEye, Constants.LEDColour.off)
# LedOverride.override(LedOverride.rightEye, Constants.LEDColour.off)
self.keepFacing = keepFacing
# Save destination for walkingTo
self.world.b_request.walkingToX = int(x)
self.world.b_request.walkingToY = int(y)
if relative:
new = FieldGeometry.addRrToRobot(Global.myPose(), x, y)
self.world.b_request.walkingToX = int(new[0])
self.world.b_request.walkingToY = int(new[1])
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
for i in range(0, len(extraObstacles)):
extraObstacles[
i] = FieldGeometry.globalPointToRobotRelativePoint(
extraObstacles[i])
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
# always keep heading the same after avoidance
facingTurn = MathUtil.normalisedTheta(facingTurn)
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
if useAvoidance:
vectorToTarget = self.calculateDestinationViaObstacles(
vectorToTarget, extraObstacles)
# avoidance doesn't change which way robot's facing
self.currentTarget = vectorToTarget
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
# myPose = Global.myPose()
# print "Pos: (%5.0f, %5.0f < %2.2f) - CurrTarget: (%5.0f, %5.0f < %2.2f) fac?%s, rel?%s, flt: (%4.0f, %4.0f < T%2.2f, F%2.2f)" % (
# myPose.x, myPose.y, degrees(myPose.theta), x, y, degrees(theta), "Y" if keepFacing else "N", "Y" if relative else "N", forward, left, degrees(targetHeading), degrees(facingTurn)
# )
self.currentState.tick(forward, left, targetHeading, facingTurn)
def tick(blackboard):
# Update all blackboard dependent helper modules.
Global.update(blackboard)
TeamStatus.update(blackboard)
FieldGeometry.update(blackboard)
Timer.update(blackboard)
Sonar.update(blackboard)
global skill_instance
if not skill_instance:
skill = blackboard.behaviour.skill
# Load the module and the class we're going to use.
found_skill = False
SkillClass = None
behaviour_packages = ["roles", "skills", "test"]
for package in behaviour_packages:
if skill not in [
name for _, name, _ in pkgutil.iter_modules(
["/home/nao/data/behaviours/%s" % package])
]:
Log.info("%s wasn't in %s, skipping import attempt.", skill,
package)
continue
try:
skill_module = __import__("%s.%s" % (package, skill),
fromlist=[skill])
# Access the class so we can do some reflection.
SkillClass = getattr(skill_module, skill)
found_skill = True
Log.info("Successfully imported %s from %s.%s", skill, package,
skill)
break
except ImportError, e:
Log.error("%s %s", package, e)
Log.error(traceback.format_exc())
if not found_skill:
raise ImportError(
"Could not find skill: %s in any of our behaviour folders." %
skill)
if issubclass(SkillClass, BehaviourTask):
new_world = world.World(blackboard) # It's a whole new world.
skill_instance = SkillClass(new_world)
else:
parentSkill = DummySkill(blackboard)
skill_instance = SkillClass(blackboard, parentSkill)
def _tick(self, x, y, theta, urgency=0, keepFacing=False, relative=False, useAvoidance=True, useOnlySonarAvoid=False):
urgency = min(1.0, urgency)
self.keepFacing = keepFacing
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(Vector2D.Vector2D(x, y), theta)
facingTurn = MathUtil.normalisedTheta(facingTurn)
self.currentTarget = vectorToTarget
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
# forward/left are used for final adjustments, rotate to mean pos after the turn is made
if vectorToTarget.isShorterThan(400) or keepFacing:
forward = vectorToTarget.x * cos(-facingTurn) - vectorToTarget.y * sin(-facingTurn)
left = vectorToTarget.x * sin(-facingTurn) + vectorToTarget.y * cos(-facingTurn)
else:
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
self.currentState.tick(forward, left, targetHeading, facingTurn)
def closestToTeamBall():
teamBall = blackboard.localisation.teamBall
if teamBall.contributors == 0:
return False
myPos = blackboard.localisation.robotPos
turnAngle = abs(FieldGeometry.angleToTeamBallToGoal(teamBall.pos, myPos))
myDist = math.hypot(teamBall.pos.x - myPos.x, teamBall.pos.y - myPos.y) + turnAngle*(1000.0/math.pi)
otherDist = minWeightedDistanceToTeamBall()[0]
return myDist < (otherDist * 1.4)
def closestToTeamBall():
teamBall = blackboard.localisation.teamBall
if teamBall.contributors == 0:
return False
myPos = blackboard.localisation.robotPos
turnAngle = abs(FieldGeometry.angleToTeamBallToGoal(teamBall.pos, myPos))
myDist = math.hypot(teamBall.pos.x - myPos.x, teamBall.pos.y -
myPos.y) + turnAngle * (1000.0 / math.pi)
otherDist = minWeightedDistanceToTeamBall()[0]
return myDist < (otherDist * 1.4)
def tick(blackboard):
# Update all blackboard dependent helper modules.
Global.update(blackboard)
TeamStatus.update(blackboard)
FieldGeometry.update(blackboard)
Timer.update(blackboard)
Sonar.update(blackboard)
global skill_instance
if not skill_instance:
skill = blackboard.behaviour.skill
# Load the module and the class we're going to use.
found_skill = False
SkillClass = None
behaviour_packages = [ "roles", "skills", "test" ]
for package in behaviour_packages:
if skill not in [name for _, name, _ in pkgutil.iter_modules(["/home/nao/data/behaviours/%s" % package])]:
Log.info("%s wasn't in %s, skipping import attempt.", skill, package)
continue
try:
skill_module = __import__("%s.%s" % (package, skill), fromlist=[skill])
# Access the class so we can do some reflection.
SkillClass = getattr(skill_module, skill)
found_skill = True
Log.info("Successfully imported %s from %s.%s", skill, package, skill)
break
except ImportError, e:
Log.error("%s %s", package, e)
Log.error(traceback.format_exc())
if not found_skill:
raise ImportError("Could not find skill: %s in any of our behaviour folders." % skill)
if issubclass(SkillClass, BehaviourTask):
new_world = world.World(blackboard) # It's a whole new world.
skill_instance = SkillClass(new_world)
else:
parentSkill = DummySkill(blackboard)
skill_instance = SkillClass(blackboard, parentSkill)
def minWeightedDistanceToTeamBall():
incapacitated = blackboard.receiver.incapacitated
minDist = robot.FULL_FIELD_LENGTH
minDistPlayer = blackboard.gameController.player_number
teamData = blackboard.receiver.data
teamBallPos = blackboard.localisation.teamBall.pos
for i in xrange(robot.ROBOTS_PER_TEAM):
robotPos = teamData[i].robotPos
lostCount = teamData[i].lostCount
if (i+1) != blackboard.gameController.player_number and not incapacitated[i] and i+1 != 1:
if not math.isnan(robotPos.x):
turnAngle = abs(FieldGeometry.angleToTeamBallToGoal(teamBallPos, robotPos))
weightedDist = math.hypot(teamBallPos.x - robotPos.x, teamBallPos.y - robotPos.y) + turnAngle*(1000.0/math.pi)
if weightedDist < minDist:
minDist = weightedDist
minDistPlayer = i+1
return minDist, minDistPlayer
def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
useOnlySonarAvoid=False):
urgency = min(1.0, urgency)
self.keepFacing = keepFacing
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
facingTurn = MathUtil.normalisedTheta(facingTurn)
self.currentTarget = vectorToTarget
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
# forward/left are used for final adjustments, rotate to mean pos after the turn is made
if vectorToTarget.isShorterThan(400) or keepFacing:
forward = vectorToTarget.x * cos(
-facingTurn) - vectorToTarget.y * sin(-facingTurn)
left = vectorToTarget.x * sin(
-facingTurn) + vectorToTarget.y * cos(-facingTurn)
else:
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
self.currentState.tick(forward, left, targetHeading, facingTurn)
def minWeightedDistanceToTeamBall():
incapacitated = blackboard.receiver.incapacitated
minDist = robot.FULL_FIELD_LENGTH
minDistPlayer = blackboard.gameController.player_number
teamData = blackboard.receiver.data
teamBallPos = blackboard.localisation.teamBall.pos
for i in xrange(robot.ROBOTS_PER_TEAM):
robotPos = teamData[i].robotPos
lostCount = teamData[i].lostCount
if (i + 1
) != blackboard.gameController.player_number and not incapacitated[
i] and i + 1 != 1:
if not math.isnan(robotPos.x):
turnAngle = abs(
FieldGeometry.angleToTeamBallToGoal(teamBallPos, robotPos))
weightedDist = math.hypot(
teamBallPos.x - robotPos.x, teamBallPos.y -
robotPos.y) + turnAngle * (1000.0 / math.pi)
if weightedDist < minDist:
minDist = weightedDist
minDistPlayer = i + 1
return minDist, minDistPlayer
