Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005Symbols/OpenChallengeSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file OpenChallengeSymbols.cpp
    00003 *
    00004 * Implementation of class OpenChallengeSymbols.
    00005 *
    00006 * @author
    00007 */
    00008
    00009 #include "OpenChallengeSymbols.h"
    00010 #include "Tools/Math/Geometry.h"
    00011 #include "Tools/FieldDimensions.h"
    00012
    00013 OpenChallengeSymbols::OpenChallengeSymbols(const BehaviorControlInterfaces& interfaces)
    00014 : BehaviorControlInterfaces(interfaces)
    00015 {
    00016   /* to  test */
    00017 }
    00018
    00019
    00020 void OpenChallengeSymbols::registerSymbols(Xabsl2Engine& engine)
    00021 {
    00022
    00023   // "Symbols for the blind and guide dog Challenge 2005"
    00024
    00025   //"Blind Dog receives message to move forward"
    00026   engine.registerBooleanInputSymbol("move-message-received",
    00027     this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::blindDogMoveForward);
    00028
    00029   //"Guide dog sends message to move forward"
    00030   engine.registerBooleanInputSymbol("move-message-sent",
    00031     this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::guideDogMoveForward);
    00032
    00033   //"Blind dog receives message to turn"
    00034   engine.registerBooleanInputSymbol("turn-message-received",
    00035     this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::blindDogTurn);
    00036
    00037   //"Guide dog sends message to turn"
    00038   engine.registerBooleanInputSymbol("turn-message-sent",
    00039     this,(bool(Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::guideDogTellsBlindDogTurn);
    00040 }
    00041
    00042 void OpenChallengeSymbols::update()
    00043 {
    00044 }
    00045
    00046 //indicates if the guide dog tells the blind dog to move forward
    00047 bool OpenChallengeSymbols::guideDogMoveForward()
    00048 {
    00049   if(!ballModel.ballWasSeen) {
    00050     // OUTPUT(idText, text, "BALL NICHT GESEHEN!");
    00051     OUTPUT(idText, text, "Ball not seen. Known ball at : " << ballModel.getKnownPosition(BallModel::timeAfterWhichPropagatedAreUsed));
    00052     return false;
    00053   }
    00054
    00055   Vector2<double> ballPosition = ballModel.getKnownBallState().positionField;
    00056   // OUTPUT(idText, text, "ball at : " << ballPosition.x << ", " << ballPosition.y);
    00057   // OUTPUT(idText, text, "OWNS: " << playerPoseCollection.numberOfOwnPlayers);
    00058
    00059   for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    00060     {
    00061       Vector2<double> blindPosition = playerPoseCollection.getOwnPlayerPose(i).getPose().translation;
    00062     if(blindPosition.x == 0 && blindPosition.y == 0) continue;
    00063
    00064     OUTPUT(idText, text, "Distance = " << (blindPosition - ballPosition).abs());
    00065
    00066     if ((blindPosition - ballPosition).abs() > 100)   // This value can be changed, please test ...
    00067       {
    00068         outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::moveForward;
    00069 //    OUTPUT(idText, text, "MoveForward send");
    00070         return true;
    00071       }
    00072   }
    00073   return false;
    00074 }
    00075
    00076 bool OpenChallengeSymbols::blindDogMoveForward()
    00077 {
    00078   //if(role != BehaviorTeamMessage::goalie)
    00079
    00080   for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++){
    00081       if (teamMessageCollection[i].isActual())
    00082       {
    00083         // if(teamMessageCollection[i].behaviorTeamMessage.dynamicRole != BehaviorTeamMessage::goalie
    00084         if(teamMessageCollection[i].behaviorTeamMessage.message == BehaviorTeamMessage::moveForward)
    00085         {
    00086            // OUTPUT(idText, text, "MoveForward message received")
    00087            return true;
    00088         }
    00089       }
    00090     }
    00091
    00092   return false;
    00093 }
    00094
    00095 //indicates if the guide dog tells the blind dog to turn
    00096 bool OpenChallengeSymbols::guideDogTellsBlindDogTurn()
    00097 {
    00098   //if(role == BehaviorTeamMessage::goalie)
    00099   for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    00100     {
    00101       Vector2<double> ballPosition = ballModel.getKnownPosition(BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00102       Vector2<double> blindPosition = playerPoseCollection.getOwnPlayerPose(i).getPose().translation;
    00103       double orientation = playerPoseCollection.getOwnPlayerPose(i).getPose().rotation;
    00104       double direction = atan2(blindPosition.y - ballPosition.y, blindPosition.x - ballPosition.x);
    00105       if (fabs(orientation - direction) > 0.1)         // This value can be changed, please test ...
    00106       {
    00107         outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::turn;
    00108         return true;
    00109       }
    00110     }
    00111
    00112   return false;
    00113 }
    00114
    00115 bool OpenChallengeSymbols::blindDogTurn()
    00116 {
    00117   //if(role != BehaviorTeamMessage::goalie)
    00118   for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
    00119       {
    00120          if (teamMessageCollection[i].isActual())
    00121          {
    00122             if(teamMessageCollection[i].behaviorTeamMessage.message == BehaviorTeamMessage::turn)
    00123             {
    00124                OUTPUT(idText, text, "Turn message received")
    00125                return true;
    00126             }
    00127          }
    00128       }
    00129   return false;
    00130 }