00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "GT2004BehaviorControl.h"
00011 #include "Tools/Player.h"
00012
00013 GT2004BehaviorControl::GT2004BehaviorControl(BehaviorControlInterfaces& interfaces)
00014 : Xabsl2BehaviorControl(interfaces,SolutionRequest::gt2004),
00015 calibrationSymbols(interfaces),
00016 angleSymbols(interfaces),
00017 ballSymbols(interfaces),
00018 configurationSymbols(interfaces),
00019 headAndTailSymbols(interfaces),
00020 joystickSymbols(interfaces),
00021 kickSelectionSymbols(interfaces,"GT2004.kst"),
00022 ledAndSoundSymbols(interfaces),
00023 mathFunctions(interfaces),
00024 motionRequestSymbols(interfaces),
00025 obstaclesSymbols(interfaces),
00026 roboCupGameManagerSymbols(interfaces),
00027 robotPoseSymbols(interfaces),
00028 robotStateSymbols(interfaces),
00029 specialVisionSymbols(interfaces),
00030 strategySymbols(interfaces),
00031 evolutionSymbols(interfaces, gt2004ParametersSet, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00032 openchallengeSymbols(interfaces),
00033 challengeSymbols(interfaces),
00034 dttSymbols(interfaces,&actualTacticChooser),
00035 simpleBasicBehaviors(interfaces,errorHandler),
00036 evolutionBasicBehaviors(interfaces,errorHandler, gt2004ParametersSet, gt2004ExtraParameters, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00037 potentialFieldBasicBehaviors(interfaces,errorHandler),
00038 commonBasicBehaviors(interfaces,errorHandler),
00039 kickLogger(interfaces),
00040 gt2004CurrentIndex((int)GT2004ParametersSet::no_turn_0_fast),
00041 evolutionMode(1),
00042 collectedBeliefs(interfaces)
00043 {
00044 Xabsl2FileInputSource file("Xabsl2/gt04-ic.dat");
00045 init(file);
00046 ASSERT(pEngine);
00047
00048 measurementRequest = gt2004ParametersSet.getParameters(gt2004CurrentIndex)->requestedMotion;
00049 for (int i=0;i<=127;i++)
00050 {
00051 gt2004ParametersCalibration[i]=Pose2D(0,0,0);
00052 }
00053 gt2004ExtraParameters.index=GT2004ParametersSet::numberOfParameters;
00054 quickStop = false;
00055
00056
00057 chooserRaterInterfaces.lastChoosenOption = RateableOptions::noOption;
00058
00059
00060 optionRating[RateableOptions::defaultOptionRating] =
00061 new DefaultOptionRating(interfaces,collectedBeliefs,chooserRaterInterfaces);
00062
00063
00064 actualOptionRating = optionRating[RateableOptions::defaultOptionRating];
00065
00066
00067
00068 tacticChooser[RateableOptions::defaultTacticChooser] =
00069 new DefaultTacticChooser(interfaces,collectedBeliefs,chooserRaterInterfaces);
00070
00071
00072 actualTacticChooser = tacticChooser[RateableOptions::defaultTacticChooser];
00073
00074
00075 }
00076
00077 GT2004BehaviorControl::~GT2004BehaviorControl()
00078 {
00079 delete(optionRating[RateableOptions::defaultOptionRating]);
00080 optionRating[RateableOptions::defaultOptionRating] = NULL;
00081 delete(tacticChooser[RateableOptions::defaultTacticChooser]);
00082 tacticChooser[RateableOptions::defaultTacticChooser] = NULL;
00083 actualTacticChooser = NULL;
00084 }
00085
00086 void GT2004BehaviorControl::registerSymbolsAndBasicBehaviors()
00087 {
00088 simpleBasicBehaviors.registerBasicBehaviors(*pEngine);
00089 evolutionBasicBehaviors.registerBasicBehaviors(*pEngine);
00090 potentialFieldBasicBehaviors.registerBasicBehaviors(*pEngine);
00091 commonBasicBehaviors.registerBasicBehaviors(*pEngine);
00092 calibrationSymbols.registerSymbols(*pEngine);
00093 angleSymbols.registerSymbols(*pEngine);
00094 ballSymbols.registerSymbols(*pEngine);
00095 configurationSymbols.registerSymbols(*pEngine);
00096 headAndTailSymbols.registerSymbols(*pEngine);
00097 joystickSymbols.registerSymbols(*pEngine);
00098 ledAndSoundSymbols.registerSymbols(*pEngine);
00099 kickSelectionSymbols.registerSymbols(*pEngine);
00100 mathFunctions.registerSymbols(*pEngine);
00101 motionRequestSymbols.registerSymbols(*pEngine);
00102 obstaclesSymbols.registerSymbols(*pEngine);
00103 roboCupGameManagerSymbols.registerSymbols(*pEngine);
00104 robotPoseSymbols.registerSymbols(*pEngine);
00105 robotStateSymbols.registerSymbols(*pEngine);
00106 specialVisionSymbols.registerSymbols(*pEngine);
00107 strategySymbols.registerSymbols(*pEngine);
00108 evolutionSymbols.registerSymbols(*pEngine);
00109 openchallengeSymbols.registerSymbols(*pEngine);
00110 challengeSymbols.registerSymbols(*pEngine);
00111 dttSymbols.registerSymbols(*pEngine);
00112 }
00113
00114 void GT2004BehaviorControl::preExecute()
00115 {
00116 for(int i = 0; i < 14; i++) ledRequest.faceLED[i] = LEDRequest::oooo;
00117 ledRequest.backFrontWhiteLED = LEDRequest::oooo;
00118 ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
00119 ledRequest.backRearWhiteLED = LEDRequest::oooo;
00120 ledRequest.backMiddleOrangeLED = LEDRequest::oooo;
00121
00122 ledRequest.headWhiteLED = LEDRequest::oooo;
00123 ledRequest.headOrangeLED = LEDRequest::oooo;
00124
00125
00126 outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::none;
00127
00128
00129 if (ballModel.ballWasSeen)
00130 {
00131
00132 ledRequest.headWhiteLED = LEDRequest::llll;
00133 motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
00134 }
00135 else if (SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen)
00136 > BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
00137 && SystemCall::getTimeSince(ballModel.communicated.timeWhenLastObserved) < 3000)
00138 {
00139
00140 motionRequest.tailRequest.tailRequestID = TailRequest::wagHorizontalFast;
00141 }
00142 else
00143 {
00144
00145 motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
00146 }
00147
00148
00149 headControlMode.headControlMode = HeadControlMode::lookToStars;
00150
00151 soundRequest.soundID = SoundRequest::none;
00152
00153 angleSymbols.angleShownByLEDs = AngleSymbols::undefined;
00154
00155
00156
00157 collectedBeliefs.update();
00158
00159
00160 actualOptionRating->rateOptions();
00161
00162
00163 dttSymbols.update();
00164
00165 if ((dttSymbols.actualOption == RateableOptions::gotoBitePos1)
00166 ||(dttSymbols.actualOption == RateableOptions::bitePos1)
00167 ||(dttSymbols.actualOption == RateableOptions::movePos1)
00168 )
00169 ledRequest.faceLED[1] = LEDRequest::llll;
00170 if ((dttSymbols.actualOption == RateableOptions::gotoBitePos2)
00171 ||(dttSymbols.actualOption == RateableOptions::bitePos2)
00172 ||(dttSymbols.actualOption == RateableOptions::movePos2)
00173 )
00174 ledRequest.faceLED[0] = LEDRequest::llll;
00175 if ((dttSymbols.actualOption == RateableOptions::gotoBitePos3)
00176 ||(dttSymbols.actualOption == RateableOptions::bitePos3)
00177 ||(dttSymbols.actualOption == RateableOptions::movePos3)
00178 )
00179 ledRequest.faceLED[3] = LEDRequest::llll;
00180 if ((dttSymbols.actualOption == RateableOptions::gotoBitePos4)
00181 ||(dttSymbols.actualOption == RateableOptions::bitePos4)
00182 ||(dttSymbols.actualOption == RateableOptions::movePos4)
00183 )
00184 ledRequest.faceLED[2] = LEDRequest::llll;
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 collectedBeliefs.broadcast();
00222
00223
00224 collectedBeliefs.trigger();
00225 }
00226
00227 void GT2004BehaviorControl::execute()
00228 {
00229 angleSymbols.update();
00230 ballSymbols.update();
00231 configurationSymbols.update();
00232 obstaclesSymbols.update();
00233 roboCupGameManagerSymbols.update();
00234 robotPoseSymbols.update();
00235 strategySymbols.update();
00236 evolutionSymbols.update();
00237 openchallengeSymbols.update();
00238
00239 potentialFieldBasicBehaviors.update();
00240
00241
00242 preExecute();
00243
00244
00245 executeEngine();
00246
00247
00248 postExecute();
00249
00250
00251 kickLogger.execute();
00252 }
00253
00254 void GT2004BehaviorControl::postExecute()
00255 {
00256
00257 if(outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::penalized &&
00258 ((robotState.getState() == RobotState::crashed)||
00259 (robotState.getState() == RobotState::rollLeft)||
00260 (robotState.getState() == RobotState::rollRight)))
00261 {
00262 if ((motionInfo.executedMotionRequest.motionType == MotionRequest::walk &&
00263 motionInfo.executedMotionRequest.walkRequest.walkType != WalkRequest::upsideDown)||
00264 (motionInfo.executedMotionRequest.motionType == MotionRequest::stand))
00265 {
00266 motionRequest.motionType = MotionRequest::getup;
00267 }
00268 }
00269
00270
00271 if (getPlayer().getTeamColor() == Player::blue)
00272 {
00273 ledRequest.backFrontBlueLED = LEDRequest::llll;
00274 if(
00275 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
00276 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
00277 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
00278 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
00279 {
00280 ledRequest.backFrontWhiteLED = LEDRequest::oooo;
00281 }
00282 ledRequest.backRearRedLED = LEDRequest::oooo;
00283 }
00284 else if (getPlayer().getTeamColor() == Player::red)
00285 {
00286 ledRequest.backFrontBlueLED = LEDRequest::oooo;
00287 ledRequest.backRearRedLED = LEDRequest::llll;
00288 if(
00289 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
00290 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
00291 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
00292 outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
00293 {
00294 ledRequest.backRearWhiteLED = LEDRequest::oooo;
00295 }
00296 }
00297 else
00298 {
00299 ledRequest.backFrontBlueLED = LEDRequest::lloo;
00300 ledRequest.backRearRedLED = LEDRequest::lloo;
00301 }
00302
00303 if(
00304 outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::sleep &&
00305 outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::initial)
00306 {
00307
00308 switch (strategySymbols.role)
00309 {
00310 case BehaviorTeamMessage::striker:
00311 ledRequest.faceLED[13] = LEDRequest::llll;
00312 break;
00313 case BehaviorTeamMessage::offensiveSupporter:
00314 ledRequest.faceLED[13] = LEDRequest::oooo;
00315 ledRequest.faceLED[12] = LEDRequest::lloo;
00316 break;
00317 case BehaviorTeamMessage::defensiveSupporter:
00318 ledRequest.faceLED[13] = LEDRequest::oooo;
00319 break;
00320 default:
00321 ledRequest.faceLED[13] = LEDRequest::oooo;
00322 }
00323
00324 if(getPlayer().getPlayerNumber() == Player::one)
00325 {
00326 if (ballModel.ballState.ballRollsByLeft)
00327 ledRequest.faceLED[1] = LEDRequest::llll;
00328 if (ballModel.ballState.ballRollsByRight)
00329 ledRequest.faceLED[0] = LEDRequest::llll;
00330 if (ballModel.ballState.ballRollsFast)
00331 {
00332 ledRequest.faceLED[2] = LEDRequest::llll;
00333 ledRequest.faceLED[3] = LEDRequest::llll;
00334 }
00335 if (ballModel.ballState.ballRollsTowardsRobot)
00336 {
00337 ledRequest.faceLED[1] = LEDRequest::llll;
00338 ledRequest.faceLED[0] = LEDRequest::llll;
00339 }
00340 }
00341
00342 if(angleSymbols.angleShownByLEDs == AngleSymbols::undefined)
00343 {
00344 ledRequest.faceLED[10] = LEDRequest::lolo;
00345 }
00346 else
00347 {
00348
00349 double bestAngle;
00350 bestAngle = toDegrees(normalize(angleSymbols.getAngle(angleSymbols.angleShownByLEDs)));
00351
00352 if (bestAngle > 75) ledRequest.faceLED[ 7] = LEDRequest::lolo;
00353 else if(bestAngle > 45) ledRequest.faceLED[ 7] = LEDRequest::llll;
00354 else if(bestAngle > 15) ledRequest.faceLED[ 9] = LEDRequest::llll;
00355 else if(bestAngle > -15) ledRequest.faceLED[10] = LEDRequest::llll;
00356 else if(bestAngle > -45) ledRequest.faceLED[ 8] = LEDRequest::llll;
00357 else if(bestAngle > -75) ledRequest.faceLED[ 6] = LEDRequest::llll;
00358 else ledRequest.faceLED[ 6] = LEDRequest::lolo;
00359 }
00360
00361
00362 if (obstaclesSymbols.robotIsStuck)
00363 {
00364 ledRequest.faceLED[4] = LEDRequest::llll;
00365 ledRequest.faceLED[5] = LEDRequest::llll;
00366 }
00367 else if (obstaclesSymbols.obstaclesAreClose)
00368 {
00369 ledRequest.faceLED[11] = LEDRequest::llll;
00370 }
00371 }
00372
00373
00374 if (headControlMode.headControlMode == HeadControlMode::lookToStars)
00375 {
00376 soundRequest.soundID = SoundRequest::rob101;
00377 }
00378
00379 if(sensorDataBuffer.frame[0].data[SensorData::chin] == 1)
00380 soundRequest.soundID = SoundRequest::bark2;
00381
00382
00383 if(sensorDataBuffer.lastFrame().data[SensorData::chin] == 1)
00384 {
00385 if(
00386 robotState.getButtonPressed(BodyPercept::backBack) &&
00387 robotState.getButtonDuration(BodyPercept::backBack) > 1000 &&
00388 profiler.profilerCollectMode == GTXabsl2Profiler::collectProfiles
00389 )
00390 {
00391 profiler.profilerCollectMode = GTXabsl2Profiler::dontCollectProfiles;
00392 profiler.profilerWriteMode = GTXabsl2Profiler::writeCompleteProfiles;
00393 soundRequest.soundID = SoundRequest::bing01;
00394 }
00395
00396 if(
00397 robotState.getButtonPressed(BodyPercept::backFront) &&
00398 robotState.getButtonDuration(BodyPercept::backFront) > 1000
00399 )
00400 {
00401 profiler.profilerCollectMode = GTXabsl2Profiler::collectProfiles;
00402 soundRequest.soundID = SoundRequest::bing03;
00403 }
00404 }
00405
00406 if(!quickStop)
00407 {
00408 if(
00409 robotState.getButtonPressed(BodyPercept::chin) &&
00410 robotState.getButtonDuration(BodyPercept::chin) > 500 &&
00411
00412 robotState.getButtonPressed(BodyPercept::backMiddle) &&
00413 robotState.getButtonDuration(BodyPercept::backMiddle) > 500
00414 )
00415 quickStop = true;
00416 }
00417 else
00418 {
00419 if(
00420 robotState.getButtonPressed(BodyPercept::head) &&
00421 robotState.getButtonDuration(BodyPercept::head) > 100 &&
00422 robotState.getButtonDuration(BodyPercept::head) < 700
00423 )
00424 quickStop = false;
00425 }
00426
00427 if(quickStop)
00428 {
00429 if(robotState.getButtonPressed(BodyPercept::backFront)) debugHeadControlMode.headControlMode = HeadControlMode::none;
00430 if(robotState.getButtonPressed(BodyPercept::backMiddle)) debugHeadControlMode.headControlMode = HeadControlMode::searchForBall;
00431 if(robotState.getButtonPressed(BodyPercept::backBack)) debugHeadControlMode.headControlMode = HeadControlMode::stayAsForced;
00432
00433 if(debugHeadControlMode.headControlMode != HeadControlMode::none)
00434 {
00435 headControlMode = debugHeadControlMode;
00436 }
00437
00438 motionRequest.motionType = MotionRequest::stand;
00439 ledRequest.backFrontWhiteLED = LEDRequest::oooo;
00440 ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
00441 ledRequest.backRearWhiteLED = LEDRequest::oooo;
00442
00443 ledRequest.backFrontBlueLED = LEDRequest::llll;
00444 ledRequest.backMiddleOrangeLED = LEDRequest::llll;
00445 ledRequest.backRearRedLED = LEDRequest::llll;
00446
00447 if(debugHeadControlMode.headControlMode == HeadControlMode::none) ledRequest.backFrontWhiteLED = LEDRequest::llll;
00448 if(debugHeadControlMode.headControlMode == HeadControlMode::searchForBall) ledRequest.backMiddleWhiteLED = LEDRequest::llll;
00449 if(debugHeadControlMode.headControlMode == HeadControlMode::stayAsForced) ledRequest.backRearWhiteLED = LEDRequest::llll;
00450 }
00451 }
00452
00453 bool GT2004BehaviorControl::handleMessage(InMessage& message)
00454 {
00455 switch (message.getMessageID())
00456 {
00457 case idGT2004EvolutionRequest:
00458 {
00459 int mode;
00460 message.bin >> mode;
00461 switch (mode)
00462 {
00463 case 1:
00464 {
00465 message.bin >> gt2004CurrentIndex;
00466 message.bin >> evolutionMode;
00467 if (gt2004CurrentIndex==(int)GT2004ParametersSet::numberOfParameters)
00468 {
00469 message.bin >> measurementRequest;
00470 if (GT2004ParametersSet::getSpeed(measurementRequest)<0.05)
00471 {
00472 GT2004ParametersSet::setSpeed(measurementRequest,0.05);
00473 }
00474 gt2004ExtraParameters.requestedMotion=measurementRequest;
00475 OUTPUT(idText,text,"GT2003BehavContr: GT2004EvolutionRequest 'measure (" << measurementRequest.translation.x << ", " << measurementRequest.translation.y << ", " << measurementRequest.rotation << ")' received");
00476 }
00477 else
00478 {
00479 int mirror=GT2004ParametersSet::getIndexOfMirror(gt2004CurrentIndex);
00480
00481 if ((mirror>=0)&&(mirror>gt2004CurrentIndex))
00482 {
00483 gt2004CurrentIndex=mirror;
00484 }
00485 measurementRequest=gt2004ParametersSet.getParameters(gt2004CurrentIndex)->requestedMotion;
00486 OUTPUT(idText,text,"GT2003BehavContr: GT2004EvolutionRequest 'measure " << GT2004ParametersSet::getIndexString(gt2004CurrentIndex) << "' received (" << measurementRequest.translation.x << ", " << measurementRequest.translation.y << ", " << measurementRequest.rotation << ")");
00487 }
00488 break;
00489 }
00490 case 2:
00491 gt2004ParametersSet.save();
00492 OUTPUT(idText,text,"GT2003BehavContr: GT2004EvolutionRequest 'save parametersSet' executed");
00493 break;
00494 case 3:
00495 {
00496 gt2004ParametersSet.load();
00497 for (int i=0;i<(int)GT2004ParametersSet::numberOfParameters;i++)
00498 {
00499 gt2004ParametersCalibration[i]=Pose2D(0,0,0);
00500 }
00501 OUTPUT(idText,text,"GT2003BehavContr: GT2004EvolutionRequest 'load parametersSet' executed and calibration resetted");
00502 break;
00503 }
00504 default:
00505 OUTPUT(idText,text,"GT2003BehavContr: unknown GT2004EvolutionRequest received");
00506 break;
00507 }
00508 return true;
00509 }
00510 case idGT2004Parameters:
00511 {
00512 GT2004Parameters param;
00513 message.bin >> param;
00514 if (param.index<GT2004ParametersSet::numberOfParameters)
00515 {
00516 *gt2004ParametersSet.getParameters(param.index) = param;
00517 OUTPUT(idText,text,"GT2003BehavContr: GT2004Parameters " << GT2004ParametersSet::getIndexString(param.index) << " received (" << param.requestedMotion.translation.x << ", " << param.requestedMotion.translation.y << ", " << param.requestedMotion.rotation << ")");
00518 }
00519 else
00520 {
00521 gt2004ExtraParameters = param;
00522 OUTPUT(idText,text,"GT2003BehavContr: extra GT2004Parameters received (" << param.requestedMotion.translation.x << ", " << param.requestedMotion.translation.y << ", " << param.requestedMotion.rotation << ")");
00523 }
00524 gt2004ParametersCalibration[param.index]=Pose2D(0,0,0);
00525
00526 gt2004Parameters = param;
00527 walkParameterTimeStamp = SystemCall::getCurrentSystemTime();
00528 return true;
00529 }
00530 default:
00531
00532 return
00533 (Xabsl2BehaviorControl::handleMessage(message) ||
00534 kickSelectionSymbols.handleMessage(message) ||
00535 actualTacticChooser->handleMessage(message)
00536 );
00537 }
00538 }
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656