GT2005: Modules/BehaviorControl/CommonXabsl2Symbols/AngleSymbols.cpp Source File

00001 /** 
00002 * @file AngleSymbols.cpp
00003 *
00004 * Implementation of class AngleSymbols.
00005 *
00006 * @author Matthias Jüngel
00007 */
00008 
00009 #include "AngleSymbols.h"
00010 #include "Platform/SystemCall.h"
00011 #include "Tools/Math/Geometry.h"
00012 #include "Tools/FieldDimensions.h"
00013 #include "Tools/Debugging/DebugDrawings.h"
00014 #include "Tools/StringFunctions.h"
00015 
00016 AngleSymbols::AngleSymbols(const BehaviorControlInterfaces& interfaces)
00017 : BehaviorControlInterfaces(interfaces)
00018 {
00019   angleShownByLEDs = undefined;
00020   angles[undefined] = 0;
00021 }
00022 
00023 
00024 void AngleSymbols::registerSymbols(Xabsl2Engine& engine)
00025 {
00026   // localisation based:
00027   engine.registerDecimalInputSymbol("angle.angle-to-center-of-field",this,
00028     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToCenterOfField);
00029 
00030   engine.registerDecimalInputSymbol("angle.angle-to-bb-supporter",this,
00031     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToBbSupporter);
00032 
00033   engine.registerDecimalInputSymbol("angle.angle-to-opponent-goal",this,
00034     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToOpponentGoal);
00035 
00036   engine.registerDecimalInputSymbol("angle.angle-to-point-behind-opponent-goal",this,
00037     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToPointBehindOpponentGoal);
00038 
00039   engine.registerDecimalInputSymbol("angle.angle-to-center-of-opponent-penalty-area",this,
00040     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getAngleToCenterOfOpponentPenaltyArea);
00041   
00042   // vision + odometry (non seen cases) based
00043   engine.registerDecimalInputSymbol("angle.seen-angle-to-opponent-goal",this,
00044     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getSeenAngleToOpponentGoal);
00045   engine.registerDecimalInputSymbol("angle.time-since-last-seen-opponent-goal",this,
00046     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTimeSinceLastSeenOpponentGoal);
00047   engine.registerBooleanInputSymbol("angle.trust-seen-angle-to-opponent-goal",this,
00048     (bool (Xabsl2FunctionProvider::*)())&AngleSymbols::getTrustOfSeenAngleToOpponentGoal);
00049   
00050   
00051   
00052   
00053   // ...
00054 
00055   // combined: vision + localisation based
00056   engine.registerDecimalInputSymbol("angle.perfect-angle-to-opponent-goal",this,
00057     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getPerfectAngleToOpponentGoal);
00058     
00059   engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal",this,
00060     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoal);
00061   
00062   engine.registerDecimalInputSymbol("angle.time-since-opponent-goal-detected",this,
00063     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTimeSinceOpponentGoalDetection);
00064 
00065   engine.registerDecimalInputSymbol("angle.best-width-of-opponent-goal",this,
00066     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestWidthOfOpponentGoal);
00067 
00068   engine.registerDecimalInputSymbol("angle.best-angle-to-opponent-goal-no-obstacles",this,
00069     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleToOpponentGoalNoObstacles);
00070   
00071   engine.registerDecimalInputSymbol("angle.best-width-of-opponent-goal-no-obstacles",this,
00072     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestWidthOfOpponentGoalNoObstacles);
00073 
00074   engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal",this,
00075     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoal);
00076   
00077   engine.registerDecimalInputSymbol("angle.best-angle-away-from-own-goal-no-obstacles",this,
00078     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles);
00079   
00080   engine.registerDecimalInputSymbol("angle.goal-kick-angle",this,
00081     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getGoalieGoalKickAngle);
00082 
00083   engine.registerDecimalInputSymbol("angle.test-angle",this,
00084     (double (Xabsl2FunctionProvider::*)())&AngleSymbols::getTestAngle);
00085 
00086 
00087   engine.registerEnumeratedOutputSymbol("angle.angle-shown-by-leds",(int*)&angleShownByLEDs);
00088 
00089   int i;
00090   char s[256];
00091   for (i = 0; i < numberOfAngles; i++)
00092   {
00093     sprintf(s,"angle.");
00094     getXmlString(s+strlen(s),getAngleName((Angles)i));
00095     engine.registerEnumeratedOutputSymbolEnumElement("angle.angle-shown-by-leds",s,i);
00096   }
00097 }
00098 
00099 void AngleSymbols::update()
00100 {
00101   calculateLocalisationBasedAngles();
00102   calculateVisionBasedAngles();
00103   calculateCombinedAngles();
00104   setTestAngle();
00105   drawAngleShownByLeds();
00106   drawAngles();
00107 }
00108 
00109 // localisation based
00110 double AngleSymbols::getAngleToCenterOfField()              {angleIsUsedByTheCurrentBehavior[angleToCenterOfField] = true; return toDegrees(normalize(angles[angleToCenterOfField])); }
00111 double AngleSymbols::getAngleToBbSupporter()              {angleIsUsedByTheCurrentBehavior[angleToBbSupporter] = true; return toDegrees(normalize(angles[angleToBbSupporter])); }
00112 double AngleSymbols::getAngleToOpponentGoal()               {angleIsUsedByTheCurrentBehavior[angleToOpponentGoal] = true; return toDegrees(normalize(angles[angleToOpponentGoal])); }
00113 double AngleSymbols::getAngleToPointBehindOpponentGoal()    {angleIsUsedByTheCurrentBehavior[angleToPointBehindOpponentGoal] = true; return toDegrees(normalize(angles[angleToPointBehindOpponentGoal])); }
00114 double AngleSymbols::getAngleToCenterOfOpponentPenaltyArea() {angleIsUsedByTheCurrentBehavior[angleToCenterOfOpponentPenaltyArea] = true; return toDegrees(normalize(angles[angleToCenterOfOpponentPenaltyArea])); }
00115 
00116 // combined: vision + odometry modelling + localisation
00117 double AngleSymbols::getPerfectAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[perfectAngleToOpponentGoal] = true; return toDegrees(normalize(angles[perfectAngleToOpponentGoal])); }
00118 // combined: vision + localisation
00119 double AngleSymbols::getBestAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[bestAngleToOpponentGoal] = true; return toDegrees(normalize(angles[bestAngleToOpponentGoal])); }
00120 double AngleSymbols::getBestAngleToOpponentGoalNoObstacles() {angleIsUsedByTheCurrentBehavior[bestAngleToOpponentGoalNoObstacles] = true; return toDegrees(normalize(angles[bestAngleToOpponentGoalNoObstacles])); }
00121 double AngleSymbols::getBestWidthOfOpponentGoal()            {angleIsUsedByTheCurrentBehavior[bestWidthOfOpponentGoal] = true; return toDegrees(normalize(angles[bestWidthOfOpponentGoal])); }
00122 double AngleSymbols::getBestWidthOfOpponentGoalNoObstacles() {angleIsUsedByTheCurrentBehavior[bestWidthOfOpponentGoalNoObstacles] = true; return toDegrees(normalize(angles[bestWidthOfOpponentGoalNoObstacles])); }
00123 double AngleSymbols::getBestAngleAwayFromOwnGoal()           {angleIsUsedByTheCurrentBehavior[bestAngleAwayFromOwnGoal] = true; return toDegrees(normalize(angles[bestAngleAwayFromOwnGoal])); }
00124 double AngleSymbols::getBestAngleAwayFromOwnGoalNoObstacles(){angleIsUsedByTheCurrentBehavior[bestAngleAwayFromOwnGoalNoObstacles] = true; return toDegrees(normalize(angles[bestAngleAwayFromOwnGoalNoObstacles])); }
00125 double AngleSymbols::getGoalieGoalKickAngle()                {angleIsUsedByTheCurrentBehavior[goalieGoalKickAngle] = true; return toDegrees(normalize(angles[goalieGoalKickAngle])); }
00126 
00127 double AngleSymbols::getTestAngle()                           {angleIsUsedByTheCurrentBehavior[testAngle] = true; return toDegrees(normalize(angles[testAngle])); }
00128 double AngleSymbols::getSeenAngleToOpponentGoal()            {angleIsUsedByTheCurrentBehavior[seenAngleToOpponentGoal] = true; return toDegrees(normalize(angles[seenAngleToOpponentGoal])); }
00129 
00130 void AngleSymbols::calculateLocalisationBasedAngles()
00131 {
00132 
00133   angles[angleToCenterOfField]     = Geometry::angleTo(robotPose, Vector2<double>(0.0,0.0));
00134   angles[angleToBbSupporter]     = Geometry::angleTo(robotPose, Vector2<double>((xPosOpponentGroundline - 300),0.0));
00135   angles[angleToCenterOfOpponentPenaltyArea] = Geometry::angleTo(robotPose, Vector2<double>((xPosOpponentPenaltyArea + xPosOpponentGroundline) / 2.0, 0.0));
00136   angles[angleToLeftOpponentGoalPost]  = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosLeftGoal));
00137   angles[angleToRightOpponentGoalPost] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGroundline,yPosRightGoal));
00138   if(angles[angleToLeftOpponentGoalPost] < angles[angleToRightOpponentGoalPost])
00139   {
00140     angles[angleToLeftOpponentGoalPost] += pi2;
00141   }
00142   angles[angleToOpponentGoal] = (angles[angleToLeftOpponentGoalPost] + angles[angleToRightOpponentGoalPost]) / 2.0;
00143 
00144   angles[angleToLeftOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosLeftGoal));
00145   angles[angleToRightOpponentGoalCorner] = Geometry::angleTo(robotPose, Vector2<double>(xPosOpponentGoal,yPosRightGoal));
00146   if(angles[angleToLeftOpponentGoalCorner] < angles[angleToRightOpponentGoalCorner])
00147   {
00148     angles[angleToLeftOpponentGoalCorner] += pi2;
00149   }
00150   angles[angleToPointBehindOpponentGoal] = (angles[angleToLeftOpponentGoalCorner] + angles[angleToRightOpponentGoalCorner]) / 2.0;
00151 }
00152 
00153 void AngleSymbols::calculateVisionBasedAngles()
00154 {
00155   angles[angleToFreePartOfOpponentGoal] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::opponentGoal];
00156   angles[widthOfFreePartOfOpponentGoal] = obstaclesModel.widthOfFreePartOfGoal[ObstaclesModel::opponentGoal];
00157   angles[angleToFreePartOfOpponentGoalLeft] = 
00158     angles[angleToFreePartOfOpponentGoal] + angles[widthOfFreePartOfOpponentGoal] / 2.0;
00159   angles[angleToFreePartOfOpponentGoalRight] = 
00160     angles[angleToFreePartOfOpponentGoal] - angles[widthOfFreePartOfOpponentGoal] / 2.0;
00161   angles[seenAngleToOpponentGoal] = obstaclesModel.angleToSeenGoal[ObstaclesModel::opponentGoal];
00162 }
00163 double AngleSymbols::getTimeSinceLastSeenOpponentGoal()
00164 {
00165   return (double) SystemCall::getTimeSince(obstaclesModel.timeOfSeenGoal[ObstaclesModel::opponentGoal]);
00166 }
00167 
00168 double AngleSymbols::getTimeSinceOpponentGoalDetection()
00169 {
00170   return (double) SystemCall::getTimeSince(obstaclesModel.lastTimeFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal]);
00171 }
00172 
00173 bool AngleSymbols::getTrustOfSeenAngleToOpponentGoal()
00174 {
00175   return obstaclesModel.trustSeenGoal[ObstaclesModel::opponentGoal];
00176 }
00177 void AngleSymbols::calculateCombinedAngles()
00178 {
00179   // perfect angle to opponent goal
00180   /*if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal])
00181   {
00182     angles[perfectAngleToOpponentGoal] = angles[angleToFreePartOfOpponentGoal];
00183     angleUsedForPerfectAngle = angleToFreePartOfOpponentGoal;
00184   }
00185   else */
00186   if( SystemCall::getCurrentSystemTime() - obstaclesModel.timeOfSeenGoal[ObstaclesModel::opponentGoal] < 10000 && obstaclesModel.trustSeenGoal[ObstaclesModel::opponentGoal])
00187   {
00188     angles[perfectAngleToOpponentGoal] = angles[seenAngleToOpponentGoal];
00189     angleUsedForPerfectAngle = seenAngleToOpponentGoal;
00190   }
00191   else
00192   {
00193     angles[perfectAngleToOpponentGoal] = angles[angleToOpponentGoal];
00194     angleUsedForPerfectAngle = angleToOpponentGoal;
00195   }
00196 
00197   //best angle to opponent goal ///////////
00198   if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::opponentGoal])
00199   {
00200     angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToFreePartOfOpponentGoal];
00201     angles[bestWidthOfOpponentGoalNoObstacles] = angles[widthOfFreePartOfOpponentGoal];
00202   }
00203   else
00204   {
00205     angles[bestAngleToOpponentGoalNoObstacles] = angles[angleToOpponentGoal];
00206     angles[bestWidthOfOpponentGoalNoObstacles] = 0;
00207   }
00208   
00209   /*
00210   * this was nice but will lead to shooting ball outside the field. hopefully one day some kind of passing 
00211   * will take care of this.
00212 */  
00213   if(FieldDimensions::distanceToOpponentPenaltyArea(robotPose.translation) > 300)
00214   {
00215     int distanceToNextTeammate = 10000;
00216     double angleToNextTeammate = 0;
00217     bool foundTeammate = false;
00218 
00219     for(int i = 0; i < playerPoseCollection.numberOfOwnPlayers; i++)
00220     {
00221       PlayerPose currentPose = playerPoseCollection.getOwnPlayerPose(i);
00222       int currentDistance = (int)((currentPose.getPose().translation - robotPose.translation).abs());
00223       double currentAngle = normalize(Geometry::angleTo(robotPose, currentPose.getPose().translation));
00224 
00225       if(currentDistance < distanceToNextTeammate)
00226       {
00227         foundTeammate = true;
00228         angleToNextTeammate = currentAngle;
00229         distanceToNextTeammate = currentDistance;
00230       }
00231     }
00232     
00233     double leftAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorLeft(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
00234     double rightAngleToOpponentGoal = obstaclesModel.getAngleOfNextFreeSectorRight(pi/8.0, angles[bestAngleToOpponentGoalNoObstacles], 600);
00235     
00236     if(foundTeammate && 
00237       fabs(
00238       fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) -
00239       fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
00240       )
00241       < fromDegrees(20) )
00242     {
00243       if(angleToNextTeammate > 0) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
00244       else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
00245     }
00246     else
00247     {
00248       if(
00249         fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - leftAngleToOpponentGoal)) <
00250         fabs(normalize(angles[bestAngleToOpponentGoalNoObstacles] - rightAngleToOpponentGoal))
00251         ) angles[bestAngleToOpponentGoal] = leftAngleToOpponentGoal;
00252       else angles[bestAngleToOpponentGoal] = rightAngleToOpponentGoal;
00253     }
00254 
00255     if(fabs(angles[bestAngleToOpponentGoal] - angles[bestAngleToOpponentGoalNoObstacles]) > fromDegrees(70))
00256     {
00257       angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
00258     }
00259     angles[bestWidthOfOpponentGoal] = 0;
00260   }
00261   else
00262   {
00263     angles[bestAngleToOpponentGoal] = angles[bestAngleToOpponentGoalNoObstacles];
00264     angles[bestWidthOfOpponentGoal] = angles[bestWidthOfOpponentGoalNoObstacles];
00265   }
00266   //best angle away from own goal ///////////
00267   if(obstaclesModel.angleToFreePartOfGoalWasDetermined[ObstaclesModel::ownGoal])
00268   {
00269     angles[bestAngleAwayFromOwnGoalNoObstacles] = obstaclesModel.angleToFreePartOfGoal[ObstaclesModel::ownGoal] + pi;
00270   }
00271   else
00272   {
00273     double angleToLeftOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosLeftGoal));
00274     double angleToRightOwnGoalPost = Geometry::angleTo(robotPose, Vector2<double>(xPosOwnGroundline,yPosRightGoal));
00275     
00276     if(angleToLeftOwnGoalPost < angleToRightOwnGoalPost)
00277     {
00278       angleToLeftOwnGoalPost += pi2;
00279     }
00280     angles[bestAngleAwayFromOwnGoalNoObstacles] = (angleToLeftOwnGoalPost + angleToRightOwnGoalPost) / 2.0;
00281   }
00282 
00283   if(
00284     robotPose.translation.y > yPosLeftSideline - 350 || 
00285     robotPose.translation.y < yPosRightSideline + 350
00286     )
00287   {
00288     angles[bestAngleAwayFromOwnGoalNoObstacles] = normalize(-robotPose.getAngle());
00289   }
00290 
00291   angles[bestAngleAwayFromOwnGoal] = angles[bestAngleAwayFromOwnGoalNoObstacles];
00292 /*
00293   // goal kick angle ///////////
00294   double angle;
00295 //  if(motionInfo.executedMotionRequest.walkType != MotionRequest::turnWithBall)
00296   if(headControlMode.headControlMode != HeadControlMode::catchBall)
00297   {
00298     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeftAndRight);
00299   }
00300   else if(motionInfo.executedMotionRequest.walkRequest.walkParams.rotation < 0)
00301   {
00302     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchRight);
00303   }
00304   else
00305   {
00306     angle = obstaclesModel.getAngleOfLargeGapInRange2(0, pi, ObstaclesModel::searchLeft);
00307   }
00308 
00309   angles[goalieGoalKickAngle] = normalize(angle + robotPose.getAngle());
00310   if(toDegrees(angles[goalieGoalKickAngle]) > 65) angles[goalieGoalKickAngle] = fromDegrees(65);
00311   if(toDegrees(angles[goalieGoalKickAngle]) < -65) angles[goalieGoalKickAngle] = fromDegrees(-65);
00312   angles[goalieGoalKickAngle] = normalize(angles[goalieGoalKickAngle] - robotPose.getAngle());
00313 
00314   if(robotPose.rotation > fromDegrees(95) || robotPose.rotation < fromDegrees(-95) )
00315   {
00316     angles[goalieGoalKickAngle] = angles[bestAngleAwayFromOwnGoal];
00317   }
00318 */
00319   angles[goalieGoalKickAngle] = -robotPose.getAngle();
00320 }
00321 
00322 double AngleSymbols::getAngle(Angles id)
00323 {
00324   return angles[id];
00325 }
00326 
00327 void AngleSymbols::drawAngleShownByLeds()
00328 {
00329   LINE(behavior_kickAngles, 
00330     robotPose.getPose().translation.x, 
00331     robotPose.getPose().translation.y,
00332     robotPose.getPose().translation.x + cos(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
00333     robotPose.getPose().translation.y + sin(angles[angleShownByLEDs] + robotPose.getAngle()) * 1500,
00334     30, Drawings::ps_solid, Drawings::light_gray
00335     );
00336 
00337   DEBUG_DRAWING_FINISHED(behavior_kickAngles);
00338 }
00339 
00340 #define ANGLEDRAWING(angleID, color) \
00341   NDECLARE_DEBUGDRAWING("behavior:symbols:angles:"#angleID, "drawingOnField", "shows the " #angleID); \
00342   NLINE("behavior:symbols:angles:"#angleID, \
00343     robotPose.getPose().translation.x, \
00344     robotPose.getPose().translation.y, \
00345     robotPose.getPose().translation.x + cos(angles[angleID] + robotPose.getAngle()) * 1800, \
00346     robotPose.getPose().translation.y + sin(angles[angleID] + robotPose.getAngle()) * 1800, \
00347     60, Drawings::ps_solid, color); 
00348 
00349 
00350 void AngleSymbols::drawAngles()
00351 {
00352   ANGLEDRAWING(angleToCenterOfField, Drawings::red);
00353   ANGLEDRAWING(angleToCenterOfOpponentPenaltyArea, Drawings::blue);
00354   ANGLEDRAWING(angleToOpponentGoal, Drawings::yellow);
00355   ANGLEDRAWING(bestAngleToOpponentGoal, Drawings::gray);
00356   ANGLEDRAWING(perfectAngleToOpponentGoal, Drawings::yellowOrange);
00357 
00358   ANGLEDRAWING(seenAngleToOpponentGoal, Drawings::skyblue);
00359   ANGLEDRAWING(testAngle, Drawings::red);
00360 
00361 //  ANGLEDRAWING(angleToFreePartOfOpponentGoalLeft, Drawings::black);
00362 //  ANGLEDRAWING(angleToFreePartOfOpponentGoalRight, Drawings::black);
00363 
00364   ANGLEDRAWING(angleToPointBehindOpponentGoal, Drawings::orange);
00365 //  ANGLEDRAWING(bestWidthOfOpponentGoal, Drawings:);
00366   ANGLEDRAWING(bestAngleToOpponentGoalNoObstacles, Drawings::skyblue);
00367 //  ANGLEDRAWING(bestWidthOfOpponentGoalNoObstacles, Drawings:);
00368   ANGLEDRAWING(bestAngleAwayFromOwnGoal, Drawings::pink);
00369   ANGLEDRAWING(bestAngleAwayFromOwnGoalNoObstacles, Drawings::dark_gray);
00370   ANGLEDRAWING(goalieGoalKickAngle, Drawings::black);
00371 
00372   NDECLARE_DEBUGDRAWING("behavior:symbols:angles:_currentlyUsedAngle", "drawingOnField", "shows all angles that are currently used by the behavior");
00373   DEBUG_RESPONSE("Behavior:output currently used angles", );
00374   for(int currentAngle = 0; currentAngle < numberOfAngles; currentAngle++) 
00375   {
00376     if(
00377       angleIsUsedByTheCurrentBehavior[currentAngle] && 
00378       (Angles)currentAngle != bestWidthOfOpponentGoal &&
00379       (Angles)currentAngle != bestWidthOfOpponentGoalNoObstacles
00380     )
00381     {
00382       NLINE("behavior:symbols:angles:_currentlyUsedAngle",
00383         robotPose.getPose().translation.x, 
00384         robotPose.getPose().translation.y, 
00385         robotPose.getPose().translation.x + cos(angles[currentAngle] + robotPose.getAngle()) * 2000,
00386         robotPose.getPose().translation.y + sin(angles[currentAngle] + robotPose.getAngle()) * 2000,
00387         25, Drawings::ps_solid, Drawings::light_gray);
00388       DEBUG_RESPONSE("Behavior:output currently used angles", 
00389         OUTPUT(idText, text, getAngleName((Angles)currentAngle) << angles[currentAngle] ); );
00390     }
00391     angleIsUsedByTheCurrentBehavior[currentAngle] = false;
00392   }
00393 }
00394 
00395 
00396 void AngleSymbols::setTestAngle()
00397 {
00398   angles[testAngle] = angles[bestAngleToOpponentGoal];
00399   DEBUG_RESPONSE("Behavior:testAngle=bestAngleToOpponentGoal", angles[testAngle] = angles[bestAngleToOpponentGoal];);
00400   DEBUG_RESPONSE("Behavior:testAngle=bestAngleToOpponentGoalNoObstacles", angles[testAngle] = angles[bestAngleToOpponentGoalNoObstacles];);
00401   DEBUG_RESPONSE("Behavior:testAngle=bestAngleAwayFromOwnGoal", angles[testAngle] = angles[bestAngleAwayFromOwnGoal];);
00402   DEBUG_RESPONSE("Behavior:testAngle=seenAngleToOpponentGoal", angles[testAngle] = angles[seenAngleToOpponentGoal];);
00403 
00404 
00405   int number = 0;
00406   MODIFY("behavior:test angle enum", number);
00407   DEBUG_RESPONSE("Behavior:testAngle==angle[testangleenum]", angles[testAngle] = angles[(Angles)number];);
00408 //  DEBUG_RESPONSE("Behavior:testAngle=", angles[testAngle] = angles[]);
00409 }