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

# Modules/BehaviorControl/CommonXabsl2Symbols/ChallengeSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file ChallengeSymbols.cpp
    00003 *
    00004 * Implementation of class ChallengeSymbols.
    00005 *
    00006 * @author Thomas Röfer
    00007 */
    00008
    00009 #include "ChallengeSymbols.h"
    00010 #include "Tools/FieldDimensions.h"
    00011 #include "Tools/StringFunctions.h"
    00012
    00013 #include "Representations/Perception/SlamPercept.h"
    00014
    00015 #define slamPercept (*(SlamPercept *)specialPercept.slamData)
    00016
    00017 ChallengeSymbols::ChallengeSymbols(const BehaviorControlInterfaces& interfaces)
    00018 : BehaviorControlInterfaces(interfaces)
    00019 {
    00020 #ifdef _WIN32
    00021   InConfigFile stream("points.cfg");
    00022 #else
    00023   InConfigFile stream("/MS/points.cfg");
    00024 #endif
    00025   if(stream.exists())
    00026   {
    00027     for(int i = 0; i < 5; ++i)
    00028     {
    00029       stream >> targetPositions[i].translation.x >> targetPositions[i].translation.y;
    00030       if(getPlayer().getTeamColor() == Player::red)
    00031         targetPositions[i].translation *= -10;
    00032       else
    00033         targetPositions[i].translation *= 10;
    00034       challenge2OptimalRotation(targetPositions[i]);
    00035     }
    00036   }
    00037   lastIndex = 10;
    00038 }
    00039
    00040 void ChallengeSymbols::registerSymbols(Xabsl2Engine& engine)
    00041 {
    00042   lastSeenCenterCircle = SystemCall::getCurrentSystemTime ();
    00043
    00044   int i;
    00045   char s[256];
    00046
    00047   // "challenge-slam.current-target-point"
    00048   engine.registerDecimalInputSymbol("challenge-slam.current-target-point", &index);
    00049
    00050   engine.registerDecimalInputSymbol("challenge-slam.imgY", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageY);
    00051   engine.registerDecimalInputSymbol("challenge-slam.imgU", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageU);
    00052   engine.registerDecimalInputSymbol("challenge-slam.imgV", &((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageV);
    00053
    00054   engine.registerBooleanInputSymbol("challenge-slam.wlan-bearing-finished", &((SlamPercept *) specialPercept.slamData)->wlanBearing.finished);
    00055   engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-direction", &((SlamPercept *) specialPercept.slamData)->wlanBearing.direction);
    00056
    00057   engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-pos-center", &((SlamPercept *) specialPercept.slamData)->wlanBearing.posCenter);
    00058   engine.registerDecimalInputSymbol("challenge-slam.wlan-bearing-neg-center", &((SlamPercept *) specialPercept.slamData)->wlanBearing.negCenter);
    00059
    00060   engine.registerBooleanInputSymbol("challenge-slam.wlan-bearing-facing-blue", &((SlamPercept *) specialPercept.slamData)->wlanBearing.facingBlue);
    00061
    00062   engine.registerBooleanInputSymbol("challenge-slam.green-location-finished", &((SlamPercept *) specialPercept.slamData)->greenLocation.greenLocationFinished);
    00063
    00064   engine.registerBooleanInputSymbol("challenge-slam.angle-to-max-green-valid", &((SlamPercept *) specialPercept.slamData)->greenLocation.angleToMaxGreenValid);
    00065   engine.registerDecimalInputSymbol("challenge-slam.angle-to-max-green", &((SlamPercept *) specialPercept.slamData)->greenLocation.angleToMaxGreen);
    00066   engine.registerDecimalInputSymbol("challenge-slam.length-of-max-green", &((SlamPercept *) specialPercept.slamData)->greenLocation.lengthOfMaxGreen);
    00067
    00068   engine.registerBooleanInputSymbol("challenge-slam.turn-finished", &((SlamPercept *) specialPercept.slamData)->turnFinished);
    00069
    00070   engine.registerDecimalInputSymbol("challenge-slam.general-purpose", &((SlamPercept *) specialPercept.slamData)->GPVal);
    00071
    00072   /*
    00073   * Center circle symbols
    00074   */
    00075   engine.registerBooleanInputSymbol("challenge-slam.center-circle-visible", &slamPercept.centerCircleInformation.centerCircleIsVisible);
    00076   engine.registerDecimalInputSymbol("challenge-slam.direction-to-center-circle", &slamPercept.centerCircleInformation.directionToCenterCircle);
    00077   engine.registerDecimalInputSymbol("challenge-slam.distance-to-center-circle", &slamPercept.centerCircleInformation.distanceToCenterCircle);
    00078   engine.registerDecimalInputSymbol("challenge-slam.seen-center-circle-orientation", &slamPercept.centerCircleInformation.seenCenterCircleOrientation);
    00079
    00080   engine.registerDecimalInputSymbol("challenge-slam.time-since-center-circle-visible", &slamPercept.centerCircleInformation.timeSinceLastSeenCenterCircle);
    00081
    00082   // "challenge-slam.probable-direction"
    00083   engine.registerDecimalInputFunction("challenge-slam.probable-direction",this,
    00084     (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::getProbableOrientation );
    00085
    00086   // "challenge-slam.target-position.x"
    00087   engine.registerDecimalInputFunction("challenge-slam.target-position.x",this,
    00088     (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionX);
    00089   engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.x",
    00090                                                "challenge-slam.target-position.x.index",index);
    00091
    00092   // "challenge-slam.target-position.y"
    00093   engine.registerDecimalInputFunction("challenge-slam.target-position.y",this,
    00094     (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionY);
    00095   engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.y",
    00096                                                "challenge-slam.target-position.y.index",index);
    00097
    00098   // "challenge-slam.target-position.rotation"
    00099   engine.registerDecimalInputFunction("challenge-slam.target-position.rotation",this,
    00100     (double (Xabsl2FunctionProvider::*)())&ChallengeSymbols::_challenge2TargetPositionRotation);
    00101   engine.registerDecimalInputFunctionParameter("challenge-slam.target-position.rotation",
    00102                                                "challenge-slam.target-position.rotation.index",index);
    00103
    00104   // "challenge-slam.state"
    00105   engine.registerEnumeratedOutputSymbol("challenge-slam.state",(int*)&slamPercept.slamState);
    00106   for (i=0;i<SlamPercept::numOfSlamStates;i++)
    00107   {
    00108     sprintf(s,"challenge-slam.state.");
    00109   getXmlString(s+strlen(s),SlamPercept::getSlamStateName((SlamPercept::SlamState)i));
    00110     engine.registerEnumeratedOutputSymbolEnumElement("challenge-slam.state",s,i);
    00111   }
    00112 }
    00113
    00114 void ChallengeSymbols::update()
    00115 {
    00116 }
    00117
    00118 double ChallengeSymbols::_challenge2TargetPositionX()
    00119 {
    00120   return challenge2TargetPositionX (index);
    00121 };
    00122
    00123 double ChallengeSymbols::_challenge2TargetPositionY()
    00124 {
    00125   return challenge2TargetPositionY (index);
    00126 };
    00127
    00128 double ChallengeSymbols::_challenge2TargetPositionRotation()
    00129 {
    00130   return challenge2TargetPositionRotation (index);
    00131 };
    00132
    00133 double ChallengeSymbols::challenge2TargetPositionX(double index)
    00134 {
    00135   challenge2ShortestPath();
    00136   return targetPositions[int(index)].translation.x;
    00137 }
    00138
    00139 double ChallengeSymbols::challenge2TargetPositionY(double index)
    00140 {
    00141   challenge2ShortestPath();
    00142   return targetPositions[int(index)].translation.y;
    00143 }
    00144
    00145 double ChallengeSymbols::challenge2TargetPositionRotation(double index)
    00146 {
    00147   challenge2ShortestPath();
    00148   return toDegrees(targetPositions[int(index)].rotation);
    00149 }
    00150
    00151 double ChallengeSymbols::imageCharacteristicsAvgY ()
    00152 {
    00153   return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageY;
    00154 };
    00155
    00156 double ChallengeSymbols::imageCharacteristicsAvgU ()
    00157 {
    00158   return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageU;
    00159 };
    00160
    00161 double ChallengeSymbols::imageCharacteristicsAvgV ()
    00162 {
    00163   return ((SlamPercept *) specialPercept.slamData)->imageCharacteristic.averageV;
    00164 };
    00165
    00166
    00167 void ChallengeSymbols::challenge2OptimalRotation(Pose2D& pose)
    00168 {
    00169   if(pose.translation.x < -2000 ||
    00170      pose.translation.x > 700 && pose.translation.x < 2000)
    00171     pose.rotation = 0;
    00172   else if(pose.translation.x > 2000 ||
    00173           pose.translation.x < -700 && pose.translation.x > -2000)
    00174     pose.rotation = pi;
    00175   else if(pose.translation.y > 0)
    00176     pose.rotation = pi_2;
    00177   else
    00178     pose.rotation = -pi_2;
    00179
    00180 //    OUTPUT(idText,text,pose.translation.x << " " << pose.translation.y << " " << int(pose.rotation * 180 / pi));
    00181
    00182   // reach position with center of body
    00183   pose += Pose2D(Vector2<double>(50,0));
    00184 }
    00185
    00186 void ChallengeSymbols::challenge2ShortestPath()
    00187 {
    00188   // this is a hack to recalculate path on restart
    00189   if(index == 0 && lastIndex > 0)
    00190   {
    00191     Pose2D p[6];
    00192     p[0] = robotPose;
    00193     memcpy(p + 1, targetPositions, sizeof(targetPositions));
    00194     double minLength = 1e9;
    00195     challenge2ShortestPermutation(p, 1, minLength);
    00196   }
    00197   lastIndex = index;
    00198 }
    00199
    00200 void ChallengeSymbols::challenge2ShortestPermutation(Pose2D* p, int n, double& minLength)
    00201 {
    00202   if(n < 6)
    00203     for(int i = n; i < 6; ++i)
    00204     {
    00205       challenge2Swap(p, i, n);
    00206       challenge2ShortestPermutation(p, n + 1, minLength);
    00207       challenge2Swap(p, i, n);
    00208     }
    00209   else
    00210   {
    00211     double length = 0;
    00212     for(int i = 0; i < 5; ++i)
    00213       length += (p[i].translation - p[i + 1].translation).abs();
    00214     if(length < minLength)
    00215     {
    00216       memcpy(targetPositions, p + 1, sizeof(targetPositions));
    00217       minLength = length;
    00218     }
    00219   }
    00220 }
    00221
    00222 void ChallengeSymbols::challenge2Swap(Pose2D* p, int i, int j)
    00223 {
    00224   Pose2D t = p[i];
    00225   p[i] = p[j];
    00226   p[j] = t;
    00227 }
    00228
    00229 double ChallengeSymbols::getProbableOrientation ()
    00230 {
    00231   return 0;
    00232 }