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

# Modules/BehaviorControl/CommonXabsl2Symbols/ObstaclesSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file ObstaclesSymbols.cpp
    00003 *
    00004 * Implementation of class ObstaclesSymbols.
    00005 *
    00006 * @author Matthias Jüngel
    00007 */
    00008
    00009 #include "ObstaclesSymbols.h"
    00010
    00011 ObstaclesSymbols::ObstaclesSymbols(const BehaviorControlInterfaces& interfaces)
    00012 : BehaviorControlInterfaces(interfaces)
    00013 {
    00014   for(int i = 0; i < sizeOfArray; i++)
    00015   {
    00016     lastValuesLeft[i] = 0;
    00017     lastValuesRight[i] = 0;
    00018     lastValuesMiddle[i] = 0;
    00019   }
    00020
    00021 }
    00022
    00023
    00024 void ObstaclesSymbols::registerSymbols(Xabsl2Engine& engine)
    00025 {
    00026   // "obstacles.collision-front-left"
    00027   engine.registerBooleanInputSymbol("obstacles.collision-front-left",this,
    00028     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionFrontLeft);
    00029   // "obstacles.collision-front-right
    00030   engine.registerBooleanInputSymbol("obstacles.collision-front-right",this,
    00031     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionFrontRight);
    00032   // "obstacles.collision-hind-left"
    00033   engine.registerBooleanInputSymbol("obstacles.collision-hind-left",this,
    00034     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHindLeft);
    00035   // "obstacles.collision-hind-right"
    00036   engine.registerBooleanInputSymbol("obstacles.collision-hind-right",this,
    00037     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHindRight);
    00038   // "obstacles.collision-head"
    00039   engine.registerBooleanInputSymbol("obstacles.collision-head",this,
    00040     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionHead);
    00041   // "obstacles.collision-aggregate"
    00042   engine.registerBooleanInputSymbol("obstacles.collision-aggregate",this,
    00043     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionAggregate);
    00044   // "obstacles.collision-front-left"
    00045   engine.registerDecimalInputSymbol("obstacles.odometry-disturbance",this,
    00046     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getOdometryDisturbance);
    00047
    00048   // "obstacles.consecutive-collision-time-front-left"
    00049   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-front-left",this,
    00050     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeFrontLeft);
    00051   // "obstacles.collision-consecutive-front-time-left"
    00052   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-front-right",this,
    00053     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeFrontRight);
    00054   // "obstacles.collision-consecutive-front-time-left"
    00055   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-hind-left",this,
    00056     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHindLeft);
    00057   // "obstacles.collision-consecutive-front-time-left"
    00058   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-hind-right",this,
    00059     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHindRight);
    00060   // "obstacles.collision-consecutive-front-time-left"
    00061   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-head",this,
    00062     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeHead);
    00063   // "obstacles.collision-consecutive-front-time-left"
    00064   engine.registerDecimalInputSymbol("obstacles.consecutive-collision-time-aggregate",this,
    00065     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getConsecutiveCollisionTimeAggregate);
    00066
    00067   engine.registerDecimalInputSymbol("obstacles.min-distance-left",this,
    00068     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceLeft);
    00069   engine.registerDecimalInputSymbol("obstacles.min-distance-right",this,
    00070     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceRight);
    00071   engine.registerDecimalInputSymbol("obstacles.min-distance-middle",this,
    00072     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getMinDistanceMiddle);
    00073
    00074
    00075   engine.registerEnumeratedInputSymbol("obstacles.collision-side",this,
    00076     (int (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getCollisionSide);
    00077
    00078   engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","left",1);
    00079   engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","middle",0);
    00080   engine.registerEnumeratedInputSymbolEnumElement("obstacles.collision-side","right",2);
    00081
    00082
    00083   // "obstacles.robot-is-stuck"
    00084   engine.registerBooleanInputSymbol("obstacles.robot-is-stuck", &robotIsStuck);
    00085
    00086   // "obstacles.close"
    00087   engine.registerBooleanInputSymbol("obstacles.close", &obstaclesAreClose);
    00088
    00089   // "obstacles.opponents-close-to-ball"
    00090   engine.registerBooleanInputSymbol("obstacles.opponents-close-to-ball", &opponentsCloseToBall);
    00091
    00092   engine.registerBooleanInputSymbol("obstacles.obstacle-front",this,
    00093     (bool (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getObstacleInFront);
    00094   engine.registerDecimalInputSymbol("obstacles.obstacle-front-left",this,
    00095     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getNearestObstacleFrontLeft);
    00096   engine.registerDecimalInputSymbol("obstacles.obstacle-front-right",this,
    00097     (double (Xabsl2FunctionProvider::*)())&ObstaclesSymbols::getNearestObstacleFrontRight);
    00098
    00099 }
    00100
    00101 void ObstaclesSymbols::update()
    00102 {
    00103   // robot is stuck
    00104   if(obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0, pi_2, 300) > 0.2)
    00105   {
    00106     robotIsStuck = true;
    00107     obstaclesAreClose = true;
    00108   }
    00109   else
    00110   {
    00111     robotIsStuck = false;
    00112     obstaclesAreClose = (obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0, pi_2, 500) > 0.15);
    00113   }
    00114
    00115   // opponents close to ball
    00116   double angleWithMinDistance;
    00117   opponentsCloseToBall = (obstaclesModel.getMinimalDistanceInRange(
    00118     Geometry::angleTo(robotPose.getPose(), ballModel.seen.positionField),
    00119     fromDegrees(45),
    00120     angleWithMinDistance,
    00121     ObstaclesPercept::opponent) < ((int)Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField)) + 1000);
    00122 }
    00123
    00124 bool ObstaclesSymbols::getCollisionFrontLeft()
    00125 {
    00126   return robotState.getCollisionFrontLeft();
    00127 }
    00128
    00129 bool ObstaclesSymbols::getCollisionFrontRight()
    00130 {
    00131   return robotState.getCollisionFrontRight();
    00132 }
    00133
    00134 bool ObstaclesSymbols::getCollisionHindLeft()
    00135 {
    00136   return robotState.getCollisionHindLeft();
    00137 }
    00138
    00139 bool ObstaclesSymbols::getCollisionHindRight()
    00140 {
    00141   return robotState.getCollisionHindRight();
    00142 }
    00143
    00144 bool ObstaclesSymbols::getCollisionHead()
    00145 {
    00146   return robotState.getCollisionHead();
    00147 }
    00148
    00149 bool ObstaclesSymbols::getCollisionAggregate()
    00150 {
    00151   return robotState.getCollisionAggregate();
    00152 }
    00153
    00154 double ObstaclesSymbols::getOdometryDisturbance()
    00155 {
    00156   return robotState.getOdometryDisturbance();
    00157 }
    00158
    00159 double ObstaclesSymbols::getConsecutiveCollisionTimeFrontLeft()
    00160 {
    00161   return robotState.getConsecutiveCollisionTimeFrontLeft();
    00162 }
    00163
    00164 double ObstaclesSymbols::getConsecutiveCollisionTimeFrontRight()
    00165 {
    00166   return robotState.getConsecutiveCollisionTimeFrontRight();
    00167 }
    00168
    00169 double ObstaclesSymbols::getConsecutiveCollisionTimeHindLeft()
    00170 {
    00171   return robotState.getConsecutiveCollisionTimeHindLeft();
    00172 }
    00173
    00174 double ObstaclesSymbols::getConsecutiveCollisionTimeHindRight()
    00175 {
    00176   return robotState.getConsecutiveCollisionTimeHindRight();
    00177 }
    00178
    00179 double ObstaclesSymbols::getConsecutiveCollisionTimeHead()
    00180 {
    00181   return robotState.getConsecutiveCollisionTimeHead();
    00182 }
    00183
    00184 double ObstaclesSymbols::getConsecutiveCollisionTimeAggregate()
    00185 {
    00186   return robotState.getConsecutiveCollisionTimeAggregate();
    00187 }
    00188
    00189
    00190
    00191 double ObstaclesSymbols::getMinDistanceLeft()
    00192 {
    00193   double x;
    00194
    00195   for(int i = sizeOfArray-1; i > 0; i--)
    00196   {
    00197     lastValuesLeft[i] = lastValuesLeft[i-1];
    00198   }
    00199
    00200   lastValuesLeft[0] = obstaclesModel.getMinimalDistanceInRange(pi/4, pi/4, x);
    00201
    00202   double durchschnitt = 0;
    00203
    00204   for(int i = 0; i < sizeOfArray; i++)
    00205   {
    00206     durchschnitt = durchschnitt + lastValuesLeft[i];
    00207   }
    00208
    00209   durchschnitt = durchschnitt / sizeOfArray;
    00210
    00211   return durchschnitt;
    00212
    00213 }
    00214
    00215 double ObstaclesSymbols::getMinDistanceRight()
    00216 {
    00217   double x;
    00218
    00219   for(int i = sizeOfArray-1; i > 0; i--)
    00220   {
    00221     lastValuesRight[i] = lastValuesRight[i-1];
    00222   }
    00223
    00224   lastValuesRight[0] = obstaclesModel.getMinimalDistanceInRange(-pi/4, pi/4, x);
    00225
    00226   double durchschnitt = 0;
    00227
    00228   for(int i = 0; i < sizeOfArray; i++)
    00229   {
    00230     durchschnitt = durchschnitt + lastValuesRight[i];
    00231   }
    00232
    00233   durchschnitt = durchschnitt / sizeOfArray;
    00234
    00235   return durchschnitt;
    00236
    00237 }
    00238
    00239 double ObstaclesSymbols::getMinDistanceMiddle()
    00240 {
    00241   return obstaclesModel.corridorInFront;
    00242 }
    00243
    00244 int ObstaclesSymbols::getCollisionSide()
    00245 {
    00246   return robotState.getCollisionSide();
    00247 }
    00248
    00249 bool ObstaclesSymbols::getObstacleInFront()
    00250 {
    00251   if (this->obstaclesModel.getDistanceInDirection(0,pi/4)<800)
    00252   {
    00253     if (this->obstaclesModel.getDistanceInDirection(0,pi/4)<400) // very near obstacle
    00254       return (this->obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0,pi/4,400) > 0.3); //should not be too thin! (could be a line)
    00255     else
    00256       return (this->obstaclesModel.getPercentageOfLowDistanceObstaclesInRange(0,pi/4,800) > 0.1);
    00257   }
    00258   else
    00259     return false;
    00260   //~ return (this->obstaclesModel.getDistanceInDirection(0,pi/4)<800);
    00261 }
    00262
    00263 double ObstaclesSymbols::getNearestObstacleFrontRight()
    00264 {
    00265   double angle=0;
    00266   while (angle > -pi/2 && obstaclesModel.getDistanceInDirection(angle,pi/8)<800){
    00267     angle-=pi/16;
    00268   }
    00269   return fabs(angle);
    00270 //  return (this->obstaclesModel.getDistanceInDirection(-pi/4,pi/4));
    00271 }
    00272 double ObstaclesSymbols::getNearestObstacleFrontLeft()
    00273 {
    00274   double angle=0;
    00275   while (angle < pi/2 && obstaclesModel.getDistanceInDirection(angle,pi/8)<800){
    00276     angle+=pi/16;
    00277   }
    00278   return fabs(angle);
    00279 //  return (this->obstaclesModel.getDistanceInDirection(pi/4,pi/4));
    00280 }