00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "GTStandardConverter.h"
00011 #include "Representations/Cognition/RobotPose.h"
00012 #include "Representations/Cognition/BallModel.h"
00013 #include "Representations/Cognition/PlayerPoseCollection.h"
00014 #include "Representations/Cognition/ObstaclesModel.h"
00015 #include "Representations/WLan/TeamMessageCollection.h"
00016 #include "Tools/FieldDimensions.h"
00017 #include <string>
00018
00019
00020
00021 GTStandardConverter::GTStandardConverter(const char* filename)
00022 {
00023 load(filename);
00024 }
00025
00026
00027 GTStandardConverter::GTStandardConverter()
00028 {
00029 }
00030
00031
00032 void GTStandardConverter::load(const char* filename)
00033 {
00034 std::string file(filename);
00035 potentialfields.load(file);
00036 freeGoalDesc.objectId = potentialfields.getIdFromObjectStateSymbol("free-goal-symbol");
00037 freeGoalDesc.objectName = "free-goal-symbol";
00038 goalieLookingPositionDesc.objectId = potentialfields.getIdFromObjectStateSymbol("goalie-lookat-symbol");
00039 goalieLookingPositionDesc.objectName = "goalie-lookat-symbol";
00040 goalieLookingPositionDesc.isActive = true;
00041 goaliePositionDesc.objectId = potentialfields.getIdFromObjectStateSymbol("goalie-position-symbol");
00042 goaliePositionDesc.objectName = "goalie-position-symbol";
00043 goaliePositionDesc.isActive = true;
00044 selfDesc.objectId = potentialfields.getIdFromObjectStateSymbol("self-symbol");
00045 selfDesc.objectName = "self-symbol";
00046 ballDesc.objectId = potentialfields.getIdFromObjectStateSymbol("ball-symbol");
00047 ballDesc.objectName = "ball-symbol";
00048 seenBallDesc.objectId = potentialfields.getIdFromObjectStateSymbol("seen-ball-symbol");
00049 seenBallDesc.objectName = "seen-ball-symbol";
00050 goalieDesc.objectId = potentialfields.getIdFromObjectStateSymbol("goalie-symbol");
00051 goalieDesc.objectName = "goalie-symbol";
00052 player2Desc.objectId = potentialfields.getIdFromObjectStateSymbol("player-2-symbol");
00053 player2Desc.objectName = "player-2-symbol";
00054 player3Desc.objectId = potentialfields.getIdFromObjectStateSymbol("player-3-symbol");
00055 player3Desc.objectName = "player-3-symbol";
00056 player4Desc.objectId = potentialfields.getIdFromObjectStateSymbol("player-4-symbol");
00057 player4Desc.objectName = "player-4-symbol";
00058 unknownTeammate1Desc.objectId = potentialfields.getIdFromObjectStateSymbol("unknown-teammate-1-symbol");
00059 unknownTeammate1Desc.objectName = "unknown-teammate-1-symbol";
00060 unknownTeammate2Desc.objectId = potentialfields.getIdFromObjectStateSymbol("unknown-teammate-2-symbol");
00061 unknownTeammate2Desc.objectName = "unknown-teammate-2-symbol";
00062 unknownTeammate3Desc.objectId = potentialfields.getIdFromObjectStateSymbol("unknown-teammate-3-symbol");
00063 unknownTeammate3Desc.objectName = "unknown-teammate-3-symbol";
00064 unknownTeammate4Desc.objectId = potentialfields.getIdFromObjectStateSymbol("unknown-teammate-4-symbol");
00065 unknownTeammate4Desc.objectName = "unknown-teammate-4-symbol";
00066 opponent1Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-1-symbol");
00067 opponent1Desc.objectName = "opponent-1-symbol";
00068 opponent2Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-2-symbol");
00069 opponent2Desc.objectName = "opponent-2-symbol";
00070 opponent3Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-3-symbol");
00071 opponent3Desc.objectName = "opponent-3-symbol";
00072 opponent4Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-4-symbol");
00073 opponent4Desc.objectName = "opponent-4-symbol";
00074 opponent5Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-5-symbol");
00075 opponent5Desc.objectName = "opponent-5-symbol";
00076 opponent6Desc.objectId = potentialfields.getIdFromObjectStateSymbol("opponent-6-symbol");
00077 opponent6Desc.objectName = "opponent-6-symbol";
00078 destination1Desc.objectId = potentialfields.getIdFromObjectStateSymbol("destination1-symbol");
00079 destination1Desc.objectName = "destination1-symbol";
00080 destination2Desc.objectId = potentialfields.getIdFromObjectStateSymbol("destination2-symbol");
00081 destination2Desc.objectName = "destination2-symbol";
00082 obstacleFrontDesc.objectId = potentialfields.getIdFromObjectStateSymbol("obstacle-front-symbol");
00083 obstacleFrontDesc.objectName = "obstacle-front-symbol";
00084 obstacleLeftDesc.objectId = potentialfields.getIdFromObjectStateSymbol("obstacle-left-symbol");
00085 obstacleLeftDesc.objectName = "obstacle-left-symbol";
00086 obstacleRightDesc.objectId = potentialfields.getIdFromObjectStateSymbol("obstacle-right-symbol");
00087 obstacleRightDesc.objectName = "obstacle-right-symbol";
00088 obstacleFrontLeftDesc.objectId = potentialfields.getIdFromObjectStateSymbol("obstacle-front-left-symbol");
00089 obstacleFrontLeftDesc.objectName = "obstacle-front-left-symbol";
00090 obstacleFrontRightDesc.objectId = potentialfields.getIdFromObjectStateSymbol("obstacle-front-right-symbol");
00091 obstacleFrontRightDesc.objectName = "obstacle-front-right-symbol";
00092
00093 angleToFrontObstacle = 0.0;
00094 angleToLeftObstacle = fromDegrees(72.0);
00095 angleToRightObstacle = fromDegrees(-72.0);
00096 angleToFrontLeftObstacle = fromDegrees(36.0);
00097 angleToFrontRightObstacle = fromDegrees(-36.0);
00098 openingAngle = fromDegrees(36.0);
00099 }
00100
00101
00102 void GTStandardConverter::execute(const RobotPose& robotPose,
00103 const BallModel& ballPosition,
00104 const PlayerPoseCollection& playerPoseCollection,
00105 const ObstaclesModel& obstaclesModel,
00106 const TeamMessageCollection& teamMessageCollection,
00107 PotentialfieldResult& result)
00108 {
00109 Pose2D dummy1, dummy2;
00110 execute(robotPose, ballPosition, playerPoseCollection,
00111 obstaclesModel, teamMessageCollection, dummy1, dummy2, 0, result);
00112 }
00113
00114
00115 void GTStandardConverter::execute(const RobotPose& robotPose,
00116 const BallModel& ballPosition,
00117 const PlayerPoseCollection& playerPoseCollection,
00118 const ObstaclesModel& obstaclesModel,
00119 const TeamMessageCollection& teamMessageCollection,
00120 const Pose2D& specialDestination1,
00121 const Pose2D& specialDestination2,
00122 const int& numberOfSpecialDestinations,
00123 PotentialfieldResult& result)
00124 {
00125 setData(robotPose, ballPosition, playerPoseCollection, obstaclesModel, teamMessageCollection);
00126 if(numberOfSpecialDestinations > 1)
00127 {
00128 destination1Desc.isActive = true;
00129 destination1Desc.pose.pos.x = specialDestination1.translation.x;
00130 destination1Desc.pose.pos.y = specialDestination1.translation.y;
00131 destination1Desc.pose.rotation = specialDestination1.rotation;
00132 destination1Desc.pose.probability = 1.0;
00133 destination2Desc.isActive = true;
00134 destination2Desc.pose.pos.x = specialDestination2.translation.x;
00135 destination2Desc.pose.pos.y = specialDestination2.translation.y;
00136 destination2Desc.pose.rotation = specialDestination2.rotation;
00137 destination2Desc.pose.probability = 1.0;
00138 }
00139 else if(numberOfSpecialDestinations > 0)
00140 {
00141 destination1Desc.isActive = true;
00142 destination1Desc.pose.pos.x = specialDestination1.translation.x;
00143 destination1Desc.pose.pos.y = specialDestination1.translation.y;
00144 destination1Desc.pose.rotation = specialDestination1.rotation;
00145 destination1Desc.pose.probability = 1.0;
00146 destination2Desc.isActive = false;
00147 }
00148 else
00149 {
00150 destination1Desc.isActive = false;
00151 destination2Desc.isActive = false;
00152 }
00153 potentialfields.setObjectState(destination1Desc);
00154 potentialfields.setObjectState(destination2Desc);
00155 potentialfields.execute(result);
00156 }
00157
00158
00159 void GTStandardConverter::setData(const RobotPose& robotPose,
00160 const BallModel& ballPosition,
00161 const PlayerPoseCollection& playerPoseCollection,
00162 const ObstaclesModel& obstaclesModel,
00163 const TeamMessageCollection& teamMessageCollection)
00164 {
00165
00166 PfPose ownPose;
00167 ownPose.rotation = robotPose.rotation;
00168 ownPose.pos.x = robotPose.translation.x;
00169 ownPose.pos.y = robotPose.translation.y;
00170 potentialfields.setOwnPose(ownPose);
00171 selfDesc.pose = ownPose;
00172 potentialfields.setObjectState(selfDesc);
00173
00174
00175 freeGoalDesc.pose.pos.x = (double)xPosOpponentGroundline;
00176 freeGoalDesc.pose.pos.y = 0.0;
00177 freeGoalDesc.pose.rotation = 0.0;
00178 freeGoalDesc.isActive = true;
00179 potentialfields.setObjectState(freeGoalDesc);
00180
00181
00182 bool ballKnown(ballPosition.getTimeSinceLastKnown(3000) < 5000);
00183 if(ballPosition.getKnownPosition(3000).x < -1000 && ballKnown &&
00184 ballPosition.getKnownPosition(3000).y > 500)
00185 {
00186 goaliePositionDesc.pose.pos.x = -1930;
00187 goaliePositionDesc.pose.pos.y = 350;
00188 }
00189 else if(ballPosition.getKnownPosition(3000).x < -1000 && ballKnown &&
00190 ballPosition.getKnownPosition(3000).y < -500)
00191 {
00192 goaliePositionDesc.pose.pos.x = -1930;
00193 goaliePositionDesc.pose.pos.y = -350;
00194 }
00195 else
00196 {
00197 goaliePositionDesc.pose.pos.x = -2000;
00198 goaliePositionDesc.pose.pos.y = 0;
00199 }
00200 potentialfields.setObjectState(goaliePositionDesc);
00201
00202
00203 if(SystemCall::getTimeSince(ballPosition.seen.timeWhenLastSeen) < 2000)
00204 {
00205 ballDesc.pose.pos.x = ballPosition.seen.x;
00206 ballDesc.pose.pos.y = ballPosition.seen.y;
00207 seenBallDesc.pose.pos.x = ballPosition.seen.x;
00208 seenBallDesc.pose.pos.y = ballPosition.seen.y;
00209 seenBallDesc.isActive = true;
00210 }
00211 else
00212 {
00213 ballDesc.pose.pos.x = ballPosition.getKnownPosition(2000).x;
00214 ballDesc.pose.pos.y = ballPosition.getKnownPosition(2000).y;
00215 seenBallDesc.pose.pos.x = ballPosition.getKnownPosition(2000).x;
00216 seenBallDesc.pose.pos.y = ballPosition.getKnownPosition(2000).y;
00217 seenBallDesc.isActive = false;
00218 }
00219 goalieLookingPositionDesc.isActive = true;
00220 if(ballPosition.getTimeSinceLastKnown(2000)>3000)
00221 {
00222 goalieLookingPositionDesc.pose.pos.x = 0;
00223 goalieLookingPositionDesc.pose.pos.y = 0;
00224 ballDesc.isActive = false;
00225 }
00226 else
00227 {
00228 goalieLookingPositionDesc.pose = ballDesc.pose;
00229 ballDesc.isActive = true;
00230 }
00231 potentialfields.setObjectState(ballDesc);
00232 potentialfields.setObjectState(seenBallDesc);
00233 potentialfields.setObjectState(goalieLookingPositionDesc);
00234
00235
00236 int numOfOpponents = playerPoseCollection.numberOfOpponentPlayers;
00237 PlayerPose playerPose;
00238 if(numOfOpponents > 0)
00239 {
00240 playerPose = playerPoseCollection.getOpponentPlayerPose(0);
00241 opponent1Desc.pose.pos.x = playerPose.getPose().translation.x;
00242 opponent1Desc.pose.pos.y = playerPose.getPose().translation.y;
00243 opponent1Desc.pose.rotation = playerPose.getPose().rotation;
00244 opponent1Desc.pose.probability = playerPose.getValidity();
00245 opponent1Desc.isActive = true;
00246 }
00247 else
00248 {
00249 opponent1Desc.isActive = false;
00250 }
00251 if(numOfOpponents > 1)
00252 {
00253 playerPose = playerPoseCollection.getOpponentPlayerPose(1);
00254 opponent2Desc.pose.pos.x = playerPose.getPose().translation.x;
00255 opponent2Desc.pose.pos.y = playerPose.getPose().translation.y;
00256 opponent2Desc.pose.rotation = playerPose.getPose().rotation;
00257 opponent2Desc.pose.probability = playerPose.getValidity();
00258 opponent2Desc.isActive = true;
00259 }
00260 else
00261 {
00262 opponent2Desc.isActive = false;
00263 }
00264 if(numOfOpponents > 2)
00265 {
00266 playerPose = playerPoseCollection.getOpponentPlayerPose(2);
00267 opponent3Desc.pose.pos.x = playerPose.getPose().translation.x;
00268 opponent3Desc.pose.pos.y = playerPose.getPose().translation.y;
00269 opponent3Desc.pose.rotation = playerPose.getPose().rotation;
00270 opponent3Desc.pose.probability = playerPose.getValidity();
00271 opponent3Desc.isActive = true;
00272 }
00273 else
00274 {
00275 opponent3Desc.isActive = false;
00276 }
00277 if(numOfOpponents > 3)
00278 {
00279 playerPose = playerPoseCollection.getOpponentPlayerPose(3);
00280 opponent4Desc.pose.pos.x = playerPose.getPose().translation.x;
00281 opponent4Desc.pose.pos.y = playerPose.getPose().translation.y;
00282 opponent4Desc.pose.rotation = playerPose.getPose().rotation;
00283 opponent4Desc.pose.probability = playerPose.getValidity();
00284 opponent4Desc.isActive = true;
00285 }
00286 else
00287 {
00288 opponent4Desc.isActive = false;
00289 }
00290 if(numOfOpponents > 4)
00291 {
00292 playerPose = playerPoseCollection.getOpponentPlayerPose(4);
00293 opponent5Desc.pose.pos.x = playerPose.getPose().translation.x;
00294 opponent5Desc.pose.pos.y = playerPose.getPose().translation.y;
00295 opponent5Desc.pose.rotation = playerPose.getPose().rotation;
00296 opponent5Desc.pose.probability = playerPose.getValidity();
00297 opponent5Desc.isActive = true;
00298 }
00299 else
00300 {
00301 opponent5Desc.isActive = false;
00302 }
00303 if(numOfOpponents > 5)
00304 {
00305 playerPose = playerPoseCollection.getOpponentPlayerPose(5);
00306 opponent6Desc.pose.pos.x = playerPose.getPose().translation.x;
00307 opponent6Desc.pose.pos.y = playerPose.getPose().translation.y;
00308 opponent6Desc.pose.rotation = playerPose.getPose().rotation;
00309 opponent6Desc.pose.probability = playerPose.getValidity();
00310 opponent6Desc.isActive = true;
00311 }
00312 else
00313 {
00314 opponent6Desc.isActive = false;
00315 }
00316 potentialfields.setObjectState(opponent1Desc);
00317 potentialfields.setObjectState(opponent2Desc);
00318 potentialfields.setObjectState(opponent3Desc);
00319 potentialfields.setObjectState(opponent4Desc);
00320 potentialfields.setObjectState(opponent5Desc);
00321 potentialfields.setObjectState(opponent6Desc);
00322
00323
00324 RobotPose teammatePose;
00325 int numOfTeammates = teamMessageCollection.numberOfTeamMessages;
00326 if(numOfTeammates > 0)
00327 {
00328 teammatePose = teamMessageCollection[0].robotPose;
00329 unknownTeammate1Desc.pose.pos.x = teammatePose.getPose().translation.x;
00330 unknownTeammate1Desc.pose.pos.y = teammatePose.getPose().translation.y;
00331 unknownTeammate1Desc.pose.rotation = teammatePose.getPose().rotation;
00332 unknownTeammate1Desc.pose.probability = teammatePose.getValidity();
00333 unknownTeammate1Desc.isActive = true;
00334 }
00335 else
00336 {
00337 unknownTeammate1Desc.isActive = false;
00338 }
00339 if(numOfTeammates > 1)
00340 {
00341 teammatePose = teamMessageCollection[0].robotPose;
00342 unknownTeammate2Desc.pose.pos.x = teammatePose.getPose().translation.x;
00343 unknownTeammate2Desc.pose.pos.y = teammatePose.getPose().translation.y;
00344 unknownTeammate2Desc.pose.rotation = teammatePose.getPose().rotation;
00345 unknownTeammate2Desc.pose.probability = teammatePose.getValidity();
00346 unknownTeammate2Desc.isActive = true;
00347 }
00348 else
00349 {
00350 unknownTeammate2Desc.isActive = false;
00351 }
00352 if(numOfTeammates > 2)
00353 {
00354 teammatePose = teamMessageCollection[0].robotPose;
00355 unknownTeammate3Desc.pose.pos.x = teammatePose.getPose().translation.x;
00356 unknownTeammate3Desc.pose.pos.y = teammatePose.getPose().translation.y;
00357 unknownTeammate3Desc.pose.rotation = teammatePose.getPose().rotation;
00358 unknownTeammate3Desc.pose.probability = teammatePose.getValidity();
00359 unknownTeammate3Desc.isActive = true;
00360 }
00361 else
00362 {
00363 unknownTeammate3Desc.isActive = false;
00364 }
00365 unknownTeammate4Desc.isActive = false;
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419 potentialfields.setObjectState(unknownTeammate1Desc);
00420 potentialfields.setObjectState(unknownTeammate2Desc);
00421 potentialfields.setObjectState(unknownTeammate3Desc);
00422 potentialfields.setObjectState(unknownTeammate4Desc);
00423
00424
00425
00426 goalieDesc.isActive = false;
00427 player2Desc.isActive = false;
00428 player3Desc.isActive = false;
00429 player4Desc.isActive = false;
00430 potentialfields.setObjectState(goalieDesc);
00431 potentialfields.setObjectState(player2Desc);
00432 potentialfields.setObjectState(player3Desc);
00433 potentialfields.setObjectState(player4Desc);
00434
00435
00436 PfVec relPos;
00437 int distanceToFrontObstacle = obstaclesModel.getDistanceInDirection(angleToFrontObstacle,openingAngle);
00438 if(distanceToFrontObstacle < 600)
00439 {
00440 obstacleFrontDesc.isActive = true;
00441 relPos.x = distanceToFrontObstacle;
00442 relPos.y = 0.0;
00443 relPos.rotate(ownPose.rotation);
00444 obstacleFrontDesc.pose.pos = (ownPose.pos+relPos);
00445 }
00446 else
00447 {
00448 obstacleFrontDesc.isActive = false;
00449 }
00450 int distanceToLeftObstacle = obstaclesModel.getDistanceInDirection(angleToLeftObstacle,openingAngle);
00451 if(distanceToLeftObstacle < 600)
00452 {
00453 obstacleLeftDesc.isActive = true;
00454 relPos.x = distanceToLeftObstacle;
00455 relPos.y = 0.0;
00456 relPos.rotate(ownPose.rotation);
00457 relPos.rotate(angleToLeftObstacle);
00458 obstacleLeftDesc.pose.pos = (ownPose.pos+relPos);
00459 }
00460 else
00461 {
00462 obstacleLeftDesc.isActive = false;
00463 }
00464 int distanceToRightObstacle = obstaclesModel.getDistanceInDirection(angleToRightObstacle,openingAngle);
00465 if(distanceToRightObstacle < 600)
00466 {
00467 obstacleRightDesc.isActive = true;
00468 relPos.x = distanceToRightObstacle;
00469 relPos.y = 0.0;
00470 relPos.rotate(ownPose.rotation);
00471 relPos.rotate(angleToRightObstacle);
00472 obstacleRightDesc.pose.pos = (ownPose.pos+relPos);
00473 }
00474 else
00475 {
00476 obstacleRightDesc.isActive = false;
00477 }
00478 int distanceToFrontLeftObstacle = obstaclesModel.getDistanceInDirection(angleToFrontLeftObstacle,openingAngle);
00479 if(distanceToFrontLeftObstacle < 600)
00480 {
00481 obstacleFrontLeftDesc.isActive = true;
00482 relPos.x = distanceToFrontLeftObstacle;
00483 relPos.y = 0.0;
00484 relPos.rotate(ownPose.rotation);
00485 relPos.rotate(angleToFrontLeftObstacle);
00486 obstacleFrontLeftDesc.pose.pos = (ownPose.pos+relPos);
00487 }
00488 else
00489 {
00490 obstacleFrontLeftDesc.isActive = false;
00491 }
00492 int distanceToFrontRightObstacle = obstaclesModel.getDistanceInDirection(angleToFrontRightObstacle,openingAngle);
00493 if(distanceToFrontRightObstacle < 600)
00494 {
00495 obstacleFrontRightDesc.isActive = true;
00496 relPos.x = distanceToFrontRightObstacle;
00497 relPos.y = 0.0;
00498 relPos.rotate(ownPose.rotation);
00499 relPos.rotate(angleToFrontRightObstacle);
00500 obstacleFrontRightDesc.pose.pos = (ownPose.pos+relPos);
00501 }
00502 else
00503 {
00504 obstacleFrontRightDesc.isActive = false;
00505 }
00506 potentialfields.setObjectState(obstacleFrontDesc);
00507 potentialfields.setObjectState(obstacleRightDesc);
00508 potentialfields.setObjectState(obstacleLeftDesc);
00509 potentialfields.setObjectState(obstacleFrontRightDesc);
00510 potentialfields.setObjectState(obstacleFrontLeftDesc);
00511 }
00512
00513
00514 void GTStandardConverter::getValueArray(const RobotPose& robotPose,
00515 const BallModel& ballPosition,
00516 const PlayerPoseCollection& playerPoseCollection,
00517 const ObstaclesModel& obstaclesModel,
00518 const TeamMessageCollection& teamMessageCollection,
00519 const std::string& fieldname, double x1, double y1,
00520 double x2, double y2, int xSteps, int ySteps, double value[], double& max)
00521 {
00522 setData(robotPose, ballPosition, playerPoseCollection, obstaclesModel, teamMessageCollection);
00523 potentialfields.getValueArray(fieldname, x1, y1, x2, y2, xSteps, ySteps, value, max);
00524 }
00525
00526
00527 void GTStandardConverter::getDirectionArray(const RobotPose& robotPose,
00528 const BallModel& ballPosition,
00529 const PlayerPoseCollection& playerPoseCollection,
00530 const ObstaclesModel& obstaclesModel,
00531 const TeamMessageCollection& teamMessageCollection,
00532 const std::string& fieldname, double x1, double y1,
00533 double x2, double y2, int xSteps, int ySteps, PfVec directions[])
00534 {
00535 setData(robotPose, ballPosition, playerPoseCollection, obstaclesModel, teamMessageCollection);
00536 potentialfields.getDirectionArray(fieldname, x1, y1, x2, y2, xSteps, ySteps, directions);
00537 }
00538
00539
00540 void GTStandardConverter::getFieldNames(std::vector<std::string>& fieldNames)
00541 {
00542 fieldNames = potentialfields.getFieldNames();
00543 }
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555