Modules/BehaviorControl/GT2005BehaviorControl/GT2005Symbols/OpenChallengeSymbols.cpp

Go to the documentation of this file.

/** 
* @file OpenChallengeSymbols.cpp
*
* Implementation of class OpenChallengeSymbols.
*
* @author 
*/

#include "OpenChallengeSymbols.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"

OpenChallengeSymbols::OpenChallengeSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
  /* to  test */
}


void OpenChallengeSymbols::registerSymbols(Xabsl2Engine& engine)
{

  // "Symbols for the blind and guide dog Challenge 2005"

  //"Blind Dog receives message to move forward"
  engine.registerBooleanInputSymbol("move-message-received",
    this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::blindDogMoveForward);

  //"Guide dog sends message to move forward"
  engine.registerBooleanInputSymbol("move-message-sent",
    this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::guideDogMoveForward);

  //"Blind dog receives message to turn"
  engine.registerBooleanInputSymbol("turn-message-received",
    this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::blindDogTurn);

  //"Guide dog sends message to turn"
  engine.registerBooleanInputSymbol("turn-message-sent",
    this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::guideDogTellsBlindDogTurn);
}

void OpenChallengeSymbols::update()
{
}

//indicates if the guide dog tells the blind dog to move forward
bool OpenChallengeSymbols::guideDogMoveForward() 
{
  if(!ballModel.ballWasSeen) {
    // OUTPUT(idText, text, "BALL NICHT GESEHEN!"); 
    OUTPUT(idText, text, "Ball not seen. Known ball at : " << ballModel.getKnownPosition(BallModel::timeAfterWhichPropagatedAreUsed));
    return false;
  }

  Vector2<double> ballPosition = ballModel.getKnownBallState().positionField; 
  // OUTPUT(idText, text, "ball at : " << ballPosition.x << ", " << ballPosition.y);
  // OUTPUT(idText, text, "OWNS: " << playerPoseCollection.numberOfOwnPlayers);

  for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    {
      Vector2<double> blindPosition = playerPoseCollection.getOwnPlayerPose(i).getPose().translation;
    if(blindPosition.x == 0 && blindPosition.y == 0) continue;

    OUTPUT(idText, text, "Distance = " << (blindPosition - ballPosition).abs());

    if ((blindPosition - ballPosition).abs() > 100)   // This value can be changed, please test ...
      {
        outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::moveForward;
//    OUTPUT(idText, text, "MoveForward send");
        return true;
      }
  }
  return false;
}

bool OpenChallengeSymbols::blindDogMoveForward() 
{
  //if(role != BehaviorTeamMessage::goalie)

  for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++){
      if (teamMessageCollection[i].isActual())
      {
        // if(teamMessageCollection[i].behaviorTeamMessage.dynamicRole != BehaviorTeamMessage::goalie 
        if(teamMessageCollection[i].behaviorTeamMessage.message == BehaviorTeamMessage::moveForward)
        {
           // OUTPUT(idText, text, "MoveForward message received")
           return true;
        }
      }
    }
  
  return false;
}

//indicates if the guide dog tells the blind dog to turn
bool OpenChallengeSymbols::guideDogTellsBlindDogTurn()
{
  //if(role == BehaviorTeamMessage::goalie)
  for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    {
      Vector2<double> ballPosition = ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
      Vector2<double> blindPosition = playerPoseCollection.getOwnPlayerPose(i).getPose().translation;
      double orientation = playerPoseCollection.getOwnPlayerPose(i).getPose().rotation;
      double direction = atan2(blindPosition.y - ballPosition.y, blindPosition.x - ballPosition.x);
      if (fabs(orientation - direction) > 0.1)         // This value can be changed, please test ...
      {
        outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::turn;
        return true;
      }
    }
  
  return false;
}

bool OpenChallengeSymbols::blindDogTurn()
{
  //if(role != BehaviorTeamMessage::goalie)
  for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
      {
         if (teamMessageCollection[i].isActual())
         {
            if(teamMessageCollection[i].behaviorTeamMessage.message == BehaviorTeamMessage::turn)
            {
               OUTPUT(idText, text, "Turn message received")
               return true;
            }
         }
      }
  return false;
}

---