예제 #1
0
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
예제 #2
0
    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)
예제 #3
0
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)
예제 #6
0
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)
예제 #7
0
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)
예제 #10
0
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