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

# Processes/CMD/Cognition.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Processes/CMD/Cognition.cpp
    00003 * Implementation of Cognition
    00004 *
    00005 * @author <a href="mailto:dueffert@informatik.hu-berlin.de">Uwe Düffert</a>
    00006 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Jüngel</a>
    00007 * @author <a href="mailto:martin@martin-loetzsch.de">Martin Lötzsch</a>
    00008 * @author <a href="mailto:risler@sim.informatik.tu-darmstadt.de">Max Risler</a>
    00009 * @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
    00010 */
    00011
    00012 #include "Cognition.h"
    00013 #include "Representations/Perception/JPEGImage.h"
    00014 #include "Representations/Perception/JPEGImage2.h"
    00015 #include "Representations/Perception/LowResImage.h"
    00016 #include "Tools/Debugging/Stopwatch.h"
    00017 #include "Tools/RobotConfiguration.h"
    00018 #include "Platform/GTAssert.h"
    00019 #include "Tools/Location.h"
    00020 #include "Tools/Player.h"
    00021
    00022
    00023 #ifdef APERIOS1_3_2
    00024   #include <ERA201D1.h>
    00025 #endif
    00026
    00027
    00028 Cognition::Cognition() :
    00029 #ifdef NO_PROCESS_FRAMEWORK
    00030 Process(theDebugReceiver,theDebugSender),
    00031 #endif
    00032 #ifndef NO_PROCESS_FRAMEWORK
    00033 INIT_DEBUGGING,
    00034 INIT_RECEIVER_SENSORDATA(true),
    00035 INIT_RECEIVER_IMAGE(false),
    00036 INIT_RECEIVER(PackageMotionCognition,false),
    00037 INIT_RECEIVER(OdometryData,false),
    00038 INIT_NET_RECEIVER(TeamMessage1,false),
    00039 INIT_NET_RECEIVER(TeamMessage2,false),
    00040 INIT_NET_RECEIVER(TeamMessage3,false),
    00041 INIT_NET_SENDER(TeamMessage1,false),
    00042 INIT_NET_SENDER(TeamMessage2,false),
    00043 INIT_NET_SENDER(TeamMessage3,false),
    00044 #ifdef FIVEDOGS
    00045 INIT_NET_RECEIVER(TeamMessage4,false),
    00046 INIT_NET_SENDER(TeamMessage4,false),
    00047 #endif
    00048 INIT_NET_RECEIVER(GameControlData,false),
    00049 INIT_SENDER(PackageCognitionMotion,false),
    00050 INIT_SENDER(Trace, false),
    00051 INIT_RECEIVER(Trace, false),
    00052 #endif
    00053 processImage(false),
    00054 processSensorData(true),
    00055 processPercepts(false),
    00056 colorTable((ColorTable&) *colorTableBuffer),
    00057 traceStored(false)
    00058 {
    00059 #ifndef _WIN32
    00060   setTrace(theTraceSender.getShared());
    00061 #endif
    00062   frameNumber = 0;
    00063   lastFrameNumber = 0;
    00064   fpsCounter = 0;
    00065   fpsLastMeasureTime = SystemCall::getCurrentSystemTime();
    00066
    00067   //  PRINT("Cognition::Cognition");
    00068   debugOut.setSize(200000);
    00069   debugIn.setSize(400000);
    00070
    00071   // tell the GTCamWorldstate wether the Robot is blue
    00072   gtCamWorldStateIN.setFlip(getPlayer().getTeamColor() == Player::blue);
    00073   gtCamWorldStateOUT.setFlip(getPlayer().getTeamColor() == Player::blue);
    00074
    00075   // the new colortable selector module is created here
    00076   ColorTableModInterfaces colorTableModInterfaces(colorTable);
    00077   pColorTableMod = new ColorTableModSelector(moduleHandler, colorTableModInterfaces);
    00078
    00079   SensorDataProcessorInterfaces sensorDataProcessorInterfaces(
    00080     theSensorDataBufferReceiver,
    00081     theImageReceiver.frameNumber,
    00082     thePackageMotionCognitionReceiver.motionInfo,
    00083     bodyPercept,
    00084     bodyPosture,
    00085     cameraMatrix,
    00086     psdPercept);
    00087   pSensorDataProcessor = new SensorDataProcessorSelector(moduleHandler, sensorDataProcessorInterfaces);
    00088
    00089   //  PRINT("Cognition - SensorDataProcessor created");
    00090
    00091   ImageProcessorInterfaces imageProcessorInterfaces(
    00092     theImageReceiver,
    00093     cameraMatrix,
    00094     colorTable,
    00095     thePackageCognitionMotionSender.robotPose,
    00096     thePackageCognitionMotionSender.ballModel,
    00097     thePackageCognitionMotionSender.playerPoseCollection,
    00098     thePackageCognitionMotionSender.robotState,
    00099     calibrationRequest,
    00100     thePackageCognitionMotionSender.landmarksPercept,
    00101     ballPercept,
    00102     linesPercept,
    00103     edgesPercept,
    00104     playersPercept,
    00105     obstaclesPercept,
    00106     specialPercept);
    00107   pImageProcessor = new ImageProcessorSelector(moduleHandler, imageProcessorInterfaces);
    00108
    00109   //  PRINT("Cognition - ImageProcessor created");
    00110
    00111
    00112   RobotStateDetectorInterfaces robotStateDetectorInterfaces(
    00113     bodyPercept,
    00114     collisionPercept,
    00115     thePackageCognitionMotionSender.robotState);
    00116   pRobotStateDetector = new RobotStateDetectorSelector(moduleHandler, robotStateDetectorInterfaces);
    00117
    00118   //  PRINT("Cognition - RobotStateDetector created");
    00119
    00120   BallLocatorInterfaces ballLocatorInterfaces(
    00121     theOdometryDataReceiver,
    00122     cameraMatrix,
    00123     ballPercept,
    00124     bodyPercept,
    00125     thePackageCognitionMotionSender.landmarksPercept,
    00126     thePackageMotionCognitionReceiver.motionInfo,
    00127     obstaclesPercept,
    00128     thePackageCognitionMotionSender.robotPose,
    00129     selfLocatorSamples,
    00130     calibrationRequest,
    00131     theSensorDataBufferReceiver,
    00132     theGameControlDataReceiver,
    00133     gtCamWorldStateOUT,
    00134     robotPoseCollection,
    00135     thePackageCognitionMotionSender.ballModel,
    00136     ballLocatorSamples,
    00137     ballHandling);
    00138   pBallLocator = new BallLocatorSelector(moduleHandler, ballLocatorInterfaces);
    00139
    00140   //  PRINT("Cognition - BallLocator created");
    00141
    00142   TeamBallLocatorInterfaces teamBallLocatorInterfaces(
    00143     thePackageCognitionMotionSender.ballModel.seen,
    00144     theOdometryDataReceiver,
    00145     thePackageCognitionMotionSender.robotPose,
    00146     teamMessageCollection,
    00147     robotPoseCollection,
    00148     thePackageCognitionMotionSender.ballModel.communicated,
    00149     ballLocatorSamples,
    00150     theGameControlDataReceiver,
    00151     ballPercept,
    00152     cameraMatrix,
    00153     thePackageCognitionMotionSender.obstaclesModel);
    00154   pTeamBallLocator = new TeamBallLocatorSelector(moduleHandler,teamBallLocatorInterfaces);
    00155
    00156   //  PRINT("Cognition - TeamBallLocator created");
    00157   PlayersLocatorInterfaces playersLocatorInterfaces(
    00158     thePackageCognitionMotionSender.robotPose,
    00159     teamMessageCollection,
    00160     theOdometryDataReceiver,
    00161     gtCamWorldStateOUT,
    00162     //thePackageCognitionMotionSender.obstaclesModel,
    00163     thePackageCognitionMotionSender.ballModel,
    00164     playersPercept,
    00165     thePackageCognitionMotionSender.playerPoseCollection,
    00166     passCorridorCollection,
    00167     playersModel);
    00168   pPlayersLocator = new PlayersLocatorSelector(moduleHandler, playersLocatorInterfaces);
    00169
    00170   //  PRINT("Cognition - PlayersLocator created");
    00171
    00172   ObstaclesLocatorInterfaces obstaclesLocatorInterfaces(
    00173     obstaclesPercept,
    00174     linesPercept,
    00175     thePackageCognitionMotionSender.landmarksPercept,
    00176     psdPercept,
    00177     cameraMatrix,
    00178     theOdometryDataReceiver,
    00179     thePackageCognitionMotionSender.playerPoseCollection,
    00180     thePackageCognitionMotionSender.robotPose,
    00181     thePackageCognitionMotionSender.ballModel,
    00182     gtCamWorldStateOUT,
    00183     thePackageCognitionMotionSender.obstaclesModel);
    00184   pObstaclesLocator = new ObstaclesLocatorSelector(moduleHandler, obstaclesLocatorInterfaces);
    00185
    00186   //  PRINT("Cognition - ObstaclesLocator created");
    00187
    00188   SelfLocatorInterfaces selfLocatorInterfaces(
    00189     thePackageCognitionMotionSender.landmarksPercept,
    00190     linesPercept,
    00191     edgesPercept,
    00192     specialPercept,
    00193     psdPercept,
    00194     obstaclesPercept,
    00195     collisionPercept,
    00196     theOdometryDataReceiver,
    00197     cameraMatrix,
    00198     thePackageCognitionMotionSender.robotState,
    00199     gtCamWorldStateOUT,
    00200     theGameControlDataReceiver,
    00201     thePackageCognitionMotionSender.robotPose,
    00202     selfLocatorSamples,
    00203     thePackageCognitionMotionSender.landmarksState,
    00204     robotPoseCollection);
    00205   pSelfLocator = new SelfLocatorSelector(moduleHandler, selfLocatorInterfaces);
    00206
    00207   //  PRINT("Cognition - SelfLocator created");
    00208
    00209   const BehaviorControlInterfaces behaviorControlInterfaces(
    00210     thePackageCognitionMotionSender.robotPose,
    00211     thePackageCognitionMotionSender.ballModel,
    00212     thePackageCognitionMotionSender.playerPoseCollection,
    00213     thePackageCognitionMotionSender.obstaclesModel,
    00214     thePackageCognitionMotionSender.robotState,
    00215     thePackageMotionCognitionReceiver.motionInfo,
    00216     specialPercept,
    00217     teamMessageCollection,
    00218     joystickData,
    00219     theOdometryDataReceiver,
    00220     selfLocatorSamples,
    00221     theSensorDataBufferReceiver,
    00222     psdPercept,
    00223     gtCamWorldStateOUT,
    00224     passCorridorCollection,
    00225     playersModel,
    00226     theGameControlDataReceiver,
    00227     thePackageCognitionMotionSender.motionRequest,
    00228     thePackageCognitionMotionSender.ledRequest,
    00229     thePackageCognitionMotionSender.headControlMode,
    00230     thePackageCognitionMotionSender.soundRequest,
    00231     thePackageCognitionMotionSender.invKinWalkingParameters,
    00232     thePackageCognitionMotionSender.gt2004WalkingParameters,
    00233     thePackageCognitionMotionSender.walkParameterTimeStamp,
    00234     specialVisionRequest, calibrationRequest, outgoingBehaviorTeamMessage,
    00235 //****for GT2005WalkingEngine****
    00236     thePackageCognitionMotionSender.gt2005Parameters,
    00237     thePackageMotionCognitionReceiver.gt2005DebugData,
    00238     cameraMatrix,
    00239     ballHandling);
    00240
    00241   pBehaviorControl = new BehaviorControlSelector(moduleHandler, behaviorControlInterfaces);
    00242
    00243   //  PRINT("Cognition - BehaviorControl created");
    00244
    00245
    00246   SensorBehaviorControlInterfaces sensorBehaviorControlInterfaces(
    00247     theImageReceiver,
    00248     theSensorDataBufferReceiver,
    00249     cameraMatrix,
    00250     colorTable,
    00251     theOdometryDataReceiver,
    00252     thePackageCognitionMotionSender.robotState,
    00253     joystickData,
    00254     teamMessageCollection,
    00255     thePackageCognitionMotionSender.soundRequest,
    00256     thePackageCognitionMotionSender.motionRequest,
    00257     thePackageCognitionMotionSender.ledRequest,
    00258     thePackageCognitionMotionSender.headControlMode
    00259     );
    00260   pSensorBehaviorControl = new SensorBehaviorControlSelector(moduleHandler, sensorBehaviorControlInterfaces);
    00261
    00262   //  PRINT("Cognition - SensorBehaviorControl created");
    00263
    00264   SpecialVisionInterfaces specialVisionInterfaces(
    00265     theImageReceiver,
    00266     specialVisionRequest,
    00267     colorTable,
    00268     thePackageCognitionMotionSender.robotPose,
    00269     specialPercept,
    00270     *this);
    00271   pSpecialVision = new DefaultSpecialVision(specialVisionInterfaces);
    00272
    00273   //  PRINT("Cognition - SpecialVision created");
    00274
    00275   CollisionDetectorInterfaces collisionDetectorInterfaces(
    00276     theSensorDataBufferReceiver,
    00277     thePackageMotionCognitionReceiver.motionInfo,
    00278     collisionPercept);
    00279   pCollisionDetector = new CollisionDetectorSelector(moduleHandler, collisionDetectorInterfaces);
    00280
    00281   //  PRINT("Cognition - CollisionDetection created");
    00282
    00283   GTCamInterfaces gtCamInterfaces(gtCamWorldStateIN, gtCamWorldStateOUT);
    00284   pGTCamSelector = new GTCamSelector(moduleHandler, gtCamInterfaces);
    00285
    00286 #ifndef NO_PROCESS_FRAMEWORK
    00287   teamMessageCollection.setInTeamMessages(theTeamMessage1Receiver);
    00288   teamMessageCollection.setOutTeamMessages(theTeamMessage1Sender);
    00289   teamMessageCollection.setInTeamMessages(theTeamMessage2Receiver);
    00290   teamMessageCollection.setOutTeamMessages(theTeamMessage2Sender);
    00291   teamMessageCollection.setInTeamMessages(theTeamMessage3Receiver);
    00292   teamMessageCollection.setOutTeamMessages(theTeamMessage3Sender);
    00293 #ifdef FIVEDOGS
    00294   teamMessageCollection.setInTeamMessages(theTeamMessage4Receiver);
    00295   teamMessageCollection.setOutTeamMessages(theTeamMessage4Sender);
    00296 #endif
    00297 #endif
    00298
    00299   thePackageCognitionMotionSender.watchdog=&watchdog;
    00300
    00301 // Print WLAN Status
    00302 #ifdef APERIOS1_3_2
    00303   if (frameNumber == 1) {
    00304   EtherDriverGetWLANSettingsMsg myEtherMsg;
    00305   ERA201D1_GetWLANSettings(&myEtherMsg);
    00306   cout << "ESSID: " << myEtherMsg.essid << " *** "
    00307        << "Channel: " << myEtherMsg.channel << " *** "
    00308        << "Status: " << myEtherMsg.status << " *** "
    00309        << endl << flush;
    00310   }
    00311 #endif
    00312 }
    00313
    00314
    00315 Cognition::~Cognition()
    00316 {
    00317   delete pImageProcessor;
    00318   delete pSensorDataProcessor;
    00319   delete pRobotStateDetector;
    00320   delete pBallLocator;
    00321   delete pTeamBallLocator;
    00322   delete pPlayersLocator;
    00323   delete pObstaclesLocator;
    00324   delete pSelfLocator;
    00325   delete pBehaviorControl;
    00326   delete pSensorBehaviorControl;
    00327   delete pSpecialVision;
    00328   delete pCollisionDetector;
    00329   delete pColorTableMod;
    00330   delete pGTCamSelector;
    00331 #ifdef APERIOS1_3_2
    00332 #ifndef NDEBUG
    00333   char* t =_etherStatusStr[0];
    00334   t[0] = t[1];
    00335 #endif
    00336 #endif
    00337 }
    00338
    00339 void Cognition::performFpsMeasure()
    00340 {
    00341   unsigned long currentTime = SystemCall::getCurrentSystemTime();
    00342
    00343   if(currentTime - fpsLastMeasureTime >= 1000)
    00344   {
    00345
    00346     thePackageCognitionMotionSender.ledRequest.cognitionFps = fpsCounter;
    00347     fpsCounter = 0;
    00348     fpsLastMeasureTime = currentTime;
    00349   }
    00350   else
    00351       ++fpsCounter;
    00352 }
    00353
    00354 int Cognition::main()
    00355 {
    00356 /* perform the fps measure */
    00357 #ifdef NDEBUG
    00358   performFpsMeasure();
    00359 #else
    00360   DEBUG_RESPONSE("Processes:Cognition - measure fps", performFpsMeasure(); );
    00361 #endif
    00362
    00363 #ifndef _WIN32
    00364   theUDPHandler.doRegularStuff();
    00365 #endif
    00366
    00367 #ifndef NO_PROCESS_FRAMEWORK
    00368   if (theImageReceiver.receivedNew())
    00369 #endif
    00370   {
    00371     processImage = true;
    00372     frameNumber = theImageReceiver.frameNumber;
    00373   }
    00374
    00375 #ifndef NO_PROCESS_FRAMEWORK
    00376   if (theSensorDataBufferReceiver.receivedNew())
    00377 #endif
    00378   {
    00379     processSensorData = true;
    00380 #ifndef _WIN32
    00381     frameNumber = theSensorDataBufferReceiver.frame[0].frameNumber;
    00382 #endif
    00383   }
    00384
    00385   DEBUG_RESPONSE("automated requests:cognition main begin", OUTPUT(idProcessBegin,bin,"c"));
    00386
    00387 #ifndef _WIN32
    00388   if (SystemCall::getTimeSince(thePackageMotionCognitionReceiver.timeStamp) > 2000 &&
    00389       theSensorDataBufferReceiver.frame[0].frameNumber > 3000)
    00390   {
    00391     OUTPUT(idText, text, "Cognition: no packages from Motion for 2000ms");
    00392     if(!traceStored && &theTraceReceiver.getShared())
    00393     {
    00394       OutTextFile stream("/MS/motion.log");
    00395       stream << theTraceReceiver.getShared();
    00396       traceStored = true;
    00397     }
    00398   }
    00399 #endif
    00400
    00401   GT_TRACE
    00402
    00403   // Run GTCam
    00404   pGTCamSelector->execute();
    00405
    00406   WATCH(sendSensorData,idSensorData,bin,theSensorDataBufferReceiver);
    00407   DEBUG_RESPONSE("send representation:open-r:sensorData", OUTPUT(idSensorData,bin,theSensorDataBufferReceiver));
    00408   DEBUG_RESPONSE("send representation:open-r:sensorData lastFrame", OUTPUT(idSensorData,bin,theSensorDataBufferReceiver.frame[0]));
    00409
    00410   GT_TRACE
    00411
    00412   if (processSensorData)
    00413   {
    00414     STOP_TIME_ON_REQUEST(sensorDataProcessor, pSensorDataProcessor->execute(); );
    00415     NSTOP_TIME_ON_REQUEST(sensorDataProcessor, pSensorDataProcessor->execute(); );
    00416     STOP_TIME_ON_REQUEST(collisionDetector, pCollisionDetector->execute(); );
    00417     NSTOP_TIME_ON_REQUEST(collisionDetector, pCollisionDetector->execute(); );
    00418     STOP_TIME_ON_REQUEST(robotStateDetector, pRobotStateDetector->execute(); );
    00419     NSTOP_TIME_ON_REQUEST(robotStateDetector, pRobotStateDetector->execute(); );
    00420   }
    00421
    00422   DEBUG_RESPONSE("Processes:Cognition - frameNumberInfo",
    00423     OUTPUT(idText, text, "image frame number" << theImageReceiver.frameNumber << " " << processImage);
    00424     OUTPUT(idText, text, "num of frames" << theSensorDataBufferReceiver.numOfFrames << " " << processSensorData);
    00425   );
    00426
    00427   DEBUG_RESPONSE("automated requests:StreamSpecification", OUTPUT(idStreamSpecification, bin, getStreamHandler()));
    00428 #ifdef NEWDEBUGGING
    00429   DEBUG_RESPONSE("automated requests:DrawingManager", OUTPUT(idDrawingManager, bin, getDrawingManager()));
    00430 #endif
    00431
    00432 #ifndef NO_PROCESS_FRAMEWORK
    00433   if (!processImage && !theDebugReceiver.receivedNew())
    00434   {
    00435     // DEBUG_RESPONSE("automated requests:cognition main finished", OUTPUT(idProcessFinished,bin,"c"));
    00436     return 0; // the process received SensorData but not an image
    00437   }
    00438 #endif
    00439
    00440   teamMessageCollection.processMessages();
    00441
    00442   INFO(sendOdometryData,idOdometryData,bin,theOdometryDataReceiver);
    00443   DEBUG_RESPONSE("send representation:motion:odometryData (received by cognition)", OUTPUT(idOdometryData,bin,theOdometryDataReceiver));
    00444
    00445   INFO(sendPlayerConfig,idText,text,"player: " << Player::getTeamColorName(getPlayer().getTeamColor())
    00446     << " " << Player::getPlayerNumberName(getPlayer().getPlayerNumber()));
    00447   DEBUG_RESPONSE("send representation:game:playerConfig", OUTPUT(idText,text,"player: " << Player::getTeamColorName(getPlayer().getTeamColor())
    00448     << " " << Player::getPlayerNumberName(getPlayer().getPlayerNumber())));
    00449
    00450   if (processImage)
    00451   {
    00452     WATCH(sendImage,idImage,bin,SEND_IMAGE(theImageReceiver,cameraMatrix));
    00453     INFO(sendJPEGImage,idJPEGImage,bin,JPEGImage(theImageReceiver) << cameraMatrix);
    00454     INFO(sendLowResImage,idLowResImage,bin,LowResImage(theImageReceiver) << cameraMatrix);
    00455     DEBUG_RESPONSE("send representation:open-r:image", OUTPUT(idImage,bin,SEND_IMAGE(theImageReceiver,cameraMatrix)));
    00456     DEBUG_RESPONSE("send representation:open-r:jpegImage", OUTPUT(idJPEGImage,bin,JPEGImage(theImageReceiver) << cameraMatrix));
    00457     DEBUG_RESPONSE("send representation:open-r:jpegImage2", OUTPUT(idJPEGImage2,bin,JPEGImage2(theImageReceiver) << cameraMatrix));
    00458     DEBUG_RESPONSE("send representation:open-r:cameraMatrix", OUTPUT(idCameraMatrix,bin, cameraMatrix));
    00459
    00460     DEBUG_RESPONSE("send representation:open-r:cameraParameters", OUTPUT(idCameraParameters,text,this->getCameraParameters() ));
    00461     STOP_TIME_ON_REQUEST(imageProcessor, pImageProcessor->execute(); );
    00462     NSTOP_TIME_ON_REQUEST(imageProcessor, pImageProcessor->execute(); );
    00463     STOP_TIME_ON_REQUEST(specialVision,  pSpecialVision->execute();  );
    00464     NSTOP_TIME_ON_REQUEST(specialVision,  pSpecialVision->execute();  );
    00465     processPercepts = true;
    00466   }
    00467
    00468   GT_TRACE
    00469
    00470   INFO(sendColorTable64, idColorTable64, bin, *(const ColorTable64*)&colorTable);
    00471   pColorTableMod->execute();
    00472
    00473   if (processPercepts)
    00474   {
    00475     GT_TRACE
    00476     STOP_TIME_ON_REQUEST(selfLocator, pSelfLocator->execute();  );
    00477     NSTOP_TIME_ON_REQUEST(selfLocator, pSelfLocator->execute();  );
    00478     MODIFY( "cognition:robotPose", thePackageCognitionMotionSender.robotPose);
    00479     GT_TRACE
    00480     STOP_TIME_ON_REQUEST(ballLocator, pBallLocator->execute();  );
    00481     NSTOP_TIME_ON_REQUEST(ballLocator, pBallLocator->execute();  );
    00482     GT_TRACE
    00483     STOP_TIME_ON_REQUEST(teamBallLocator, pTeamBallLocator->execute();  );
    00484     NSTOP_TIME_ON_REQUEST(teamBallLocator, pTeamBallLocator->execute();  );
    00485     GT_TRACE
    00486     STOP_TIME_ON_REQUEST(obstaclesLocator, pObstaclesLocator->execute(); );
    00487     NSTOP_TIME_ON_REQUEST(obstaclesLocator, pObstaclesLocator->execute(); );
    00488     GT_TRACE
    00489     STOP_TIME_ON_REQUEST(playersLocator, pPlayersLocator->execute(); );
    00490     NSTOP_TIME_ON_REQUEST(playersLocator, pPlayersLocator->execute(); );
    00491   }
    00492
    00493   DEBUG_RESPONSE("no wlan:send image if opp goal seen",
    00494   {
    00495       for(int i = 0; i < thePackageCognitionMotionSender.landmarksPercept.numberOfGoals; ++i)
    00496       {
    00497         if( thePackageCognitionMotionSender.landmarksPercept.goals[i].color == ((getPlayer().getTeamColor()==Player::blue) ? yellow : skyblue))
    00498         {
    00499           JPEGImage* temp = new JPEGImage(theImageReceiver);
    00500           OUTPUT(idJPEGImage, bin, *temp << cameraMatrix);
    00501           delete temp;
    00502           break;
    00503         }
    00504       }
    00505     }
    00506   );
    00507
    00508   DEBUG_RESPONSE("no wlan:send image when ball seen",
    00509       if( ballPercept.ballWasSeen)
    00510       {
    00511         JPEGImage* temp = new JPEGImage(theImageReceiver);
    00512         OUTPUT(idJPEGImage, bin, *temp << cameraMatrix);
    00513         delete temp;
    00514       }
    00515     );
    00516     DEBUG_RESPONSE("no wlan:save image (ball or opponent goal seen)",
    00517       if( ballPercept.ballWasSeen)
    00518       {
    00519         JPEGImage* temp = new JPEGImage(theImageReceiver);
    00520         OUTPUT(idJPEGImage, bin, *temp << cameraMatrix);
    00521         delete temp;
    00522       }
    00523       else
    00524       {
    00525         for(int i = 0; i < thePackageCognitionMotionSender.landmarksPercept.numberOfGoals; ++i)
    00526         {
    00527           if( thePackageCognitionMotionSender.landmarksPercept.goals[i].color == ((getPlayer().getTeamColor()==Player::blue) ? yellow : skyblue))
    00528           {
    00529             JPEGImage* temp = new JPEGImage(theImageReceiver);
    00530             OUTPUT(idJPEGImage, bin, *temp << cameraMatrix);
    00531             delete temp;
    00532           }
    00533         }
    00534       }
    00535     );
    00536   MODIFY( "cognition:robotPose", thePackageCognitionMotionSender.robotPose);
    00537   MODIFY( "cognition:ballPercept", ballPercept);
    00538   MODIFY( "cognition:landmarksPercept", thePackageCognitionMotionSender.landmarksPercept);
    00539   MODIFY( "cognition:linesPercept", linesPercept);
    00540   MODIFY( "cognition:edgesPercept", edgesPercept);
    00541   MODIFY( "cognition:playersPercept", playersPercept);
    00542   MODIFY( "cognition:obstaclesPercept", obstaclesPercept);
    00543   MODIFY( "cognition:psdPercept", psdPercept);
    00544   MODIFY( "cognition:collisionPercept", collisionPercept);
    00545
    00546   DEBUG_RESPONSE("send representation:test", OUTPUT(idRobotPose, bin, thePackageCognitionMotionSender.robotPose));
    00547   DEBUG_RESPONSE("send representation:percepts:collision", OUTPUT(idCollisionPercept, bin, collisionPercept));
    00548   DEBUG_RESPONSE("send representation:percepts:ball", OUTPUT(idBallPercept, bin, ballPercept << thePackageCognitionMotionSender.robotPose));
    00549   DEBUG_RESPONSE("send representation:percepts:landmarks", OUTPUT(idLandmarksPercept, bin, thePackageCognitionMotionSender.landmarksPercept << thePackageCognitionMotionSender.robotPose));
    00550   DEBUG_RESPONSE("send representation:percepts:edges", OUTPUT(idEdgesPercept, bin, edgesPercept << thePackageCognitionMotionSender.robotPose));
    00551   DEBUG_RESPONSE("send representation:percepts:lines", OUTPUT(idLinesPercept, bin, linesPercept << thePackageCognitionMotionSender.robotPose));
    00552   DEBUG_RESPONSE("send representation:percepts:obstacles", OUTPUT(idObstaclesPercept, bin, obstaclesPercept << thePackageCognitionMotionSender.robotPose));
    00553   DEBUG_RESPONSE("send representation:percepts:free part of goal", OUTPUT(idObstaclesPercept, bin, obstaclesPercept << thePackageCognitionMotionSender.robotPose));
    00554   DEBUG_RESPONSE("send representation:percepts:players", OUTPUT(idPlayersPercept, bin, playersPercept << thePackageCognitionMotionSender.robotPose));
    00555   DEBUG_RESPONSE("send representation:percepts:all percepts", OUTPUT(idPercepts,bin,SEND_PERCEPTS(thePackageCognitionMotionSender.robotPose, cameraMatrix, theImageReceiver.cameraInfo,
    00556     ballPercept,thePackageCognitionMotionSender.landmarksPercept,linesPercept,edgesPercept,playersPercept, obstaclesPercept, psdPercept, collisionPercept)));
    00557
    00558
    00559   WATCH(sendPercepts,idPercepts,bin,SEND_PERCEPTS( thePackageCognitionMotionSender.robotPose, cameraMatrix, theImageReceiver.cameraInfo,
    00560     ballPercept,thePackageCognitionMotionSender.landmarksPercept,linesPercept,edgesPercept,playersPercept, obstaclesPercept, psdPercept, collisionPercept));
    00561
    00562   MODIFY( "cognition:ballModel", thePackageCognitionMotionSender.ballModel);
    00563   MODIFY( "cognition:playerPoseCollection", thePackageCognitionMotionSender.playerPoseCollection);
    00564   MODIFY( "cognition:obstaclesModel", thePackageCognitionMotionSender.obstaclesModel);
    00565   // MODIFY( "cognition:robotState", thePackageCognitionMotionSender.robotState);
    00566
    00567   DEBUG_RESPONSE("send representation:cognition:robot pose", OUTPUT(idRobotPose, bin, thePackageCognitionMotionSender.robotPose));
    00568   DEBUG_RESPONSE("send representation:cognition:ball model", OUTPUT(idBallModel, bin, thePackageCognitionMotionSender.ballModel << thePackageCognitionMotionSender.robotPose));
    00569   DEBUG_RESPONSE("send representation:cognition:world state", OUTPUT(
    00570     idWorldState,bin,SEND_WORLD_STATE(
    00571     thePackageCognitionMotionSender.robotPose,
    00572     thePackageCognitionMotionSender.ballModel,
    00573     thePackageCognitionMotionSender.playerPoseCollection,
    00574     thePackageCognitionMotionSender.obstaclesModel,
    00575     thePackageCognitionMotionSender.robotState,
    00576     cameraMatrix,
    00577     theImageReceiver.cameraInfo
    00578     )));
    00579
    00580   WATCH(sendWorldState,idWorldState,bin,SEND_WORLD_STATE(
    00581     thePackageCognitionMotionSender.robotPose,
    00582     thePackageCognitionMotionSender.ballModel,
    00583     thePackageCognitionMotionSender.playerPoseCollection,
    00584     thePackageCognitionMotionSender.obstaclesModel,
    00585     thePackageCognitionMotionSender.robotState,
    00586     cameraMatrix,
    00587     theImageReceiver.cameraInfo
    00588     ));
    00589
    00590   GT_TRACE
    00591
    00592   STOP_TIME_ON_REQUEST(behaviorControl,  pBehaviorControl->execute();  );
    00593   NSTOP_TIME_ON_REQUEST(behaviorControl,  pBehaviorControl->execute();  );
    00594   GT_TRACE
    00595   STOP_TIME_ON_REQUEST(sensorBehaviorControl, pSensorBehaviorControl->execute();  );
    00596   NSTOP_TIME_ON_REQUEST(sensorBehaviorControl, pSensorBehaviorControl->execute();  );
    00597
    00598   MODIFY("cognition:headcontrolmode", thePackageCognitionMotionSender.headControlMode);
    00599   INFO(sendSpecialPercept, idSpecialPercept, bin, getPlayer() << specialPercept << cameraMatrix);
    00600   DEBUG_RESPONSE("send representation:percepts:special", OUTPUT(idSpecialPercept, bin, getPlayer() << specialPercept << cameraMatrix));
    00601
    00602   /** "button based" sending of representations */
    00603   DEBUG_RESPONSE("no wlan:send jpeg image when back pressed",
    00604   if(thePackageCognitionMotionSender.robotState.getAnyBackButtonPressed() && thePackageCognitionMotionSender.robotState.getAnyBackButtonDuration() < 10)
    00605   {OUTPUT(idJPEGImage,bin,JPEGImage(theImageReceiver) << cameraMatrix); } );
    00606
    00607   DEBUG_RESPONSE("no wlan:send jpeg images while back pressed",
    00608   if(thePackageCognitionMotionSender.robotState.getAnyBackButtonPressed() )
    00609   {OUTPUT(idJPEGImage,bin,JPEGImage(theImageReceiver) << cameraMatrix); } );
    00610
    00611   MODIFY("cognition:walkParams", thePackageCognitionMotionSender.motionRequest.walkRequest.walkParams);
    00612   NDECLARE_DEBUGDRAWING( "cognition:motionRequest", "drawingOnField", "shows the current motion request for the next second as choosen by the bahavior");
    00613   NCOMPLEX_DRAWING("cognition:motionRequest",
    00614     if(true || thePackageCognitionMotionSender.motionRequest.motionType == MotionRequest::walk) {
    00615       int numSteps = 10;
    00616       Vector2<double> from = thePackageCognitionMotionSender.robotPose.translation;
    00617       Vector2<double> to = from;
    00618       Pose2D step = thePackageCognitionMotionSender.motionRequest.walkRequest.walkParams;
    00619       step.translation /= numSteps;
    00620       step.rotation = thePackageCognitionMotionSender.robotPose.rotation;
    00621       double rotationStep = thePackageCognitionMotionSender.motionRequest.walkRequest.walkParams.rotation / numSteps;
    00622       step.rotation += rotationStep / 2;
    00623       for(int i = 0; i < numSteps; i++) {
    00624         to += Vector2<double>(step.getCos() * step.translation.x - step.getSin() * step.translation.y,step.getSin() * step.translation.x + step.getCos() * step.translation.y);
    00625         NCOLORED_LINE(
    00626           "cognition:motionRequest",
    00627           from.x,
    00628           from.y,
    00629           to.x,
    00630           to.y,
    00631           20,
    00632           Drawings::ps_solid,
    00633           0,
    00634           0,
    00635           255);
    00636         from = to;
    00637         step.rotation += rotationStep;
    00638       }
    00639       step.rotation -= rotationStep / 2;
    00640       to += Vector2<double>(step.getCos() * 100, step.getSin() * 100);
    00641       NARROW(
    00642           "cognition:motionRequest",
    00643           from.x,
    00644           from.y,
    00645           to.x,
    00646           to.y,
    00647           20,
    00648           Drawings::ps_solid,
    00649           Drawings::blue);
    00650     }
    00651   );
    00652
    00653 /*
    00654   DEBUG_RESPONSE("no wlan:head button saves collected",
    00655     if(theSensorDataBufferReceiver.frame[0].data[SensorData::head] > 10)
    00656     {
    00657       OutTextFile fout("/MS/Logfile.txt");
    00658       fout << "use" << "the" << "following" << "line" << "as" << "new" << "name" << "for" << "LOGFILE.LOG\n";
    00659       fout << getRobotConfiguration().getMacAddressString() << this->getCameraParameters() << "field:XXX" << "2005-07-<XX>" << "<DESCRIPTION>-<INDEX>.log";
    00660     } );
    00661 */
    00662   DEBUG_RESPONSE("no wlan:back rear saves collected",
    00663     if(theSensorDataBufferReceiver.frame[0].data[SensorData::backM] > 10) // backM is correct here !
    00664     {
    00665       OutTextFile fout("/MS/Logfile.txt");
    00666       fout << "use" << "the" << "following" << "line" << "as" << "new" << "name" << "for" << "LOGFILE.LOG\n";
    00667       fout << getRobotConfiguration().getMacAddressString() << this->getCameraParameters() << "field:XXX" << "2005-07-<XX>" << "<DESCRIPTION>-<INDEX>.log";
    00668     } );
    00669
    00670
    00671   /** test drawings */
    00672   NDECLARE_DEBUGDRAWING( "test:image1", "drawingOnImage", "shows the frame of the image coordinate system:\n green - bottom \n yellow - left \n red - right \n blue - top \n ");
    00673   NDECLARE_DEBUGDRAWING( "test:image2", "drawingOnImage", "shows a red / yellow bar of length 100 pixel that rotates depending on the frameNumber");
    00674
    00675   NCOLORED_LINE( "test:image1",   3,   3, 204,   3, 3, Drawings::ps_solid,   0,   0, 255);
    00676   NCOLORED_LINE( "test:image1", 204,   3, 204, 156, 3, Drawings::ps_solid, 255,   0,   0);
    00677   NCOLORED_LINE( "test:image1", 204, 156,   3, 156, 3, Drawings::ps_solid,   0, 255,   0);
    00678   NCOLORED_LINE( "test:image1",   3, 156,   3,   3, 3, Drawings::ps_solid, 255, 255,   0);
    00679
    00680   NCOLORED_LINE( "test:image2", 104, 80, 104 + 50.0 * sin(linesPercept.frameNumber / 40.0), 80 + 50.0 * cos(linesPercept.frameNumber / 40.0), 3, Drawings::ps_solid, 255, 255, 0);
    00681   NCOLORED_LINE( "test:image2", 104, 80, 104 - 50.0 * sin(linesPercept.frameNumber / 40.0), 80 - 50.0 * cos(linesPercept.frameNumber / 40.0), 3, Drawings::ps_solid, 255,   0, 0);
    00682
    00683
    00684   NDECLARE_DEBUGDRAWING( "test:field1", "drawingOnField", "shows the axes of the field coordinate system:\n green - positive x \n black - negative x \n red - positive y \n blue - negative y \n ");
    00685   NDECLARE_DEBUGDRAWING( "test:field2", "drawingOnField", "shows a red / yellow bar of length 2000 mm that rotates depending on the frameNumber");
    00686
    00687   NCOLORED_LINE( "test:field1", 0, -2000, 0, 0, 100, Drawings::ps_solid, 0, 0, 255);
    00688   NCOLORED_LINE( "test:field1", 0, 0, 0, 2000, 100, Drawings::ps_solid, 255, 0, 0);
    00689   NCOLORED_LINE( "test:field1", 0, 0, 3000, 0, 100, Drawings::ps_solid, 0, 255, 0);
    00690   NCOLORED_LINE( "test:field1", -3000, 0, 0, 0, 100, Drawings::ps_solid, 0, 0, 0);
    00691
    00692   NCOLORED_LINE( "test:field2", 0, 0, +1000.0 * sin(linesPercept.frameNumber / 40.0), +1000.0 * cos(linesPercept.frameNumber / 40.0), 100, Drawings::ps_solid, 255, 255, 0);
    00693   NCOLORED_LINE( "test:field2", 0, 0, -1000.0 * sin(linesPercept.frameNumber / 40.0), -1000.0 * cos(linesPercept.frameNumber / 40.0), 100, Drawings::ps_solid, 255,   0, 0);
    00694
    00695   DEBUG_RESPONSE("LED, tail, mouth:tail shows frameloss",
    00696     if ((frameNumber - lastFrameNumber) > 10)
    00697       thePackageCognitionMotionSender.motionRequest.tailRequest.tailRequestID = TailRequest::wagVerticalFast;
    00698     else if ((frameNumber - lastFrameNumber) > 5 )
    00699       thePackageCognitionMotionSender.motionRequest.tailRequest.tailRequestID = TailRequest::wagVertical;
    00700     else if ((frameNumber - lastFrameNumber) > 1 )
    00701       thePackageCognitionMotionSender.motionRequest.tailRequest.tailRequestID = TailRequest::wagHorizontalFast;
    00702   );
    00703   thePackageCognitionMotionSender.ledRequest.showCognitionFrameLostWarning = ( (frameNumber - lastFrameNumber) > 5 );
    00704
    00705   lastFrameNumber = frameNumber;
    00706
    00707   thePackageCognitionMotionSender.teamColor = getPlayer().getTeamColor();
    00708
    00709   thePackageCognitionMotionSender.wLanStatus = 0;
    00710
    00711 #ifndef NO_PROCESS_FRAMEWORK
    00712   if (theTeamMessage1Receiver.isActual()) thePackageCognitionMotionSender.wLanStatus += 1;
    00713   if (theTeamMessage2Receiver.isActual()) thePackageCognitionMotionSender.wLanStatus += 2;
    00714   if (theTeamMessage3Receiver.isActual()) thePackageCognitionMotionSender.wLanStatus += 4;
    00715
    00716 #ifdef FIVEDOGS
    00717   if (theTeamMessage4Receiver.isActual()) thePackageCognitionMotionSender.wLanStatus += 8;
    00718 #endif
    00719
    00720   thePackageCognitionMotionSender.timeStamp = SystemCall::getCurrentSystemTime();
    00721
    00722   thePackageCognitionMotionSender.send();
    00723 #endif
    00724
    00725   INFO(sendGameControlData,idText,text,"GameControlData: "
    00726     << "state: " << theGameControlDataReceiver.getStateName((GameControlData::State)(theGameControlDataReceiver.data.state))
    00727     << ", kickoff: " << theGameControlDataReceiver.getKickoffTeamName(theGameControlDataReceiver.getKickoffTeam())
    00728     << ", team color: " << ( (Player::teamColor)theGameControlDataReceiver.getOwnTeam().teamColour == Player::red ? "red" : "blue" )
    00729     << ", own score: " << theGameControlDataReceiver.getOwnTeam().score
    00730     << ", opponent score: " << theGameControlDataReceiver.getOpponentTeam().score
    00731     << ", penalty: "<< theGameControlDataReceiver.getPenaltyName((GameControlData::Penalties)(theGameControlDataReceiver.getRobot().penalty))
    00732     );
    00733   DEBUG_RESPONSE("send representation:game:gameControlData", OUTPUT(idText,text,"GameControlData: "
    00734     //  << "state: " << theGameControlDataReceiver.getStateName((GameControlData::state)(theGameControlDataReceiver.data.state))
    00735     << ", kickoff: " << theGameControlDataReceiver.getKickoffTeamName(theGameControlDataReceiver.getKickoffTeam())
    00736     //  << ", team color: " << theGameControlDataReceiver.getTeamColorName(theGameControlDataReceiver.getOwnTeam().teamColor)
    00737     << ", own score: " << theGameControlDataReceiver.getOwnTeam().score
    00738     << ", opponent score: " << theGameControlDataReceiver.getOpponentTeam().score
    00739     << ", penalty: "<< theGameControlDataReceiver.getPenaltyName((GameControlData::Penalties)(theGameControlDataReceiver.getRobot().penalty))));
    00740
    00741   DEBUG_RESPONSE("automated requests:cognition main finished", OUTPUT(idProcessFinished,bin,"c"));
    00742 #ifndef NO_PROCESS_FRAMEWORK
    00743   theDebugSender.send();
    00744
    00745   teamMessageCollection.send (thePackageCognitionMotionSender.robotPose);
    00746   teamMessageCollection.send (thePackageCognitionMotionSender.ballModel.seen);
    00747   teamMessageCollection.send (outgoingBehaviorTeamMessage);
    00748   teamMessageCollection.send (playersPercept);
    00749   if ( (teamMessageCollection.processOutMessages())) {
    00750     theTeamMessage1Sender.send();
    00751 #ifndef APERIOS1_3_2
    00752     theTeamMessage2Sender.send();
    00753     theTeamMessage3Sender.send();
    00754 #ifdef FIVEDOGS
    00755     theTeamMessage4Sender.send();
    00756 #endif
    00757 #endif
    00758   }
    00759
    00760 #endif
    00761
    00762   GT_TRACE
    00763
    00764   processImage = false;
    00765   processPercepts = false;
    00766   processSensorData = true;
    00767
    00768   thePackageCognitionMotionSender.gameControlData.data = theGameControlDataReceiver.data;
    00769   thePackageCognitionMotionSender.gameControlData.timeStamp = theGameControlDataReceiver.timeStamp;
    00770
    00771 #ifndef NSTATUSBROADCASTHANDLER
    00772 #ifdef APERIOS1_3_2
    00773
    00774   // Send status of dog every 30 frames
    00775   if ((frameNumber % 30) == 0) {
    00776
    00777     // Send position in release mode also
    00778     (((DebugDataControlerWorldStateMessage*)theBroadCastStatusHandler.thePackage[0])->_data).robotPoseX = (int)thePackageCognitionMotionSender.robotPose.translation.x;
    00779     (((DebugDataControlerWorldStateMessage*)theBroadCastStatusHandler.thePackage[0])->_data).robotPoseY = (int)thePackageCognitionMotionSender.robotPose.translation.y;
    00780     (((DebugDataControlerWorldStateMessage*)theBroadCastStatusHandler.thePackage[0])->_data).robotPoseRotation = (int)thePackageCognitionMotionSender.robotPose.rotation;
    00781     (((DebugDataControlerWorldStateMessage*)theBroadCastStatusHandler.thePackage[0])->_data).robotPoseColor = (thePackageCognitionMotionSender.teamColor == Player::red);
    00782
    00783 #ifndef NDEBUG
    00784     // rest only in debug
    00785     EtherDriverGetWLANSettingsMsg myEtherMsg;
    00786     ERA201D1_GetWLANSettings(&myEtherMsg);
    00787
    00788     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).seenBallStateX = (int)thePackageCognitionMotionSender.ballModel.seen.positionField.x;
    00789     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).seenBallStateY = (int)thePackageCognitionMotionSender.ballModel.seen.positionField.y;
    00790     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).propagatedBallStateX = (int)thePackageCognitionMotionSender.ballModel.propagated.positionField.x;
    00791     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).propagatedBallStateY = (int)thePackageCognitionMotionSender.ballModel.propagated.positionField.y;
    00792     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).communicatedBallStateX = (int)thePackageCognitionMotionSender.ballModel.communicated.positionField.x;
    00793     (((DebugDataControlerBallStateMessage*)theBroadCastStatusHandler.thePackage[1])->_data).communicatedBallStateY = (int)thePackageCognitionMotionSender.ballModel.communicated.positionField.y;
    00794
    00795     (((DebugDataControlerStatusStateMessage*)theBroadCastStatusHandler.thePackage[2])->_data).batteryStatus = SystemCall::getRemainingPower();
    00796     (((DebugDataControlerStatusStateMessage*)theBroadCastStatusHandler.thePackage[2])->_data).Systemtime = SystemCall::getCurrentSystemTime();
    00797     (((DebugDataControlerStatusStateMessage*)theBroadCastStatusHandler.thePackage[2])->_data).freeMem = SystemCall::getFreeMem();
    00798     (((DebugDataControlerStatusStateMessage*)theBroadCastStatusHandler.thePackage[2])->_data).MAC = 0;
    00799     (((DebugDataControlerStatusStateMessage*)theBroadCastStatusHandler.thePackage[2])->_data).WLANSignalStrengh = myEtherMsg.status;
    00800
    00801     (((DebugDataControlerDebugStateMessage*)theBroadCastStatusHandler.thePackage[3])->_data).DebugPort = DEBUGPORT;
    00802
    00803     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).CognitionAverage              = this->averageRunTime;
    00804     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).GTCamAverage                  = pGTCamSelector->averageRunTime;
    00805 //    (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).SpecialVisionAverage          = pSpecialVision->averageRunTime;
    00806     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).SpecialVisionAverage          = 0;
    00807     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).SensorBehaviorControlAverage  = pSensorBehaviorControl->averageRunTime;
    00808     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).BehaviorControlAverage        = pBehaviorControl->averageRunTime;
    00809     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).SelfLocatorAverage            = pSelfLocator->averageRunTime;
    00810     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).ObstaclesLocatorAverage       = pObstaclesLocator->averageRunTime;
    00811     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).PlayersLocatorAverage         = pPlayersLocator->averageRunTime;
    00812     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).TeamBallLocatorAverage        = pTeamBallLocator->averageRunTime;
    00813     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).BallLocatorAverage            = pBallLocator->averageRunTime;
    00814     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).RobotStateDetectorAverage     = pRobotStateDetector->averageRunTime;
    00815     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).CollisionDetectorAverage      = pCollisionDetector->averageRunTime;
    00816     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).SensorDataProcessorAverage    = pSensorDataProcessor->averageRunTime;
    00817     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).ImageProcessorAverage         = pImageProcessor->averageRunTime;
    00818     (((DebugDataControlerRunTimeMessage*)theBroadCastStatusHandler.thePackage[4])->_data).ColorTableModAverage          = pColorTableMod->averageRunTime;
    00819
    00820     this->frameCounter = 0;
    00821     pGTCamSelector->frameCounter = 0;
    00822 //    pSpecialVision->frameCounter = 0;
    00823     pSensorBehaviorControl->frameCounter = 0;
    00824     pBehaviorControl->frameCounter = 0;
    00825     pSelfLocator->frameCounter = 0;
    00826     pObstaclesLocator->frameCounter = 0;
    00827     pPlayersLocator->frameCounter = 0;
    00828     pTeamBallLocator->frameCounter = 0;
    00829     pBallLocator->frameCounter = 0;
    00830     pRobotStateDetector->frameCounter = 0;
    00831     pCollisionDetector->frameCounter = 0;
    00832     pSensorDataProcessor->frameCounter = 0;
    00833     pImageProcessor->frameCounter = 0;
    00834     pColorTableMod->frameCounter = 0;
    00835 #endif  // NDEBUG
    00836
    00837     // send the package
    00838     theBroadCastStatusHandler.sendStatusPackage();
    00839   }
    00840 #endif // APERIOS1_3_2
    00841 #endif // NSTATUSBROADCASTHANDLER
    00842
    00843
    00844
    00845
    00846
    00847
    00848
    00849 #ifndef NSTATUSBROADCASTHANDLER
    00850 #ifdef APERIOS1_3_2
    00851 #ifndef NDEBUG
    00852   MODIFY("cognition:averageRunTime",this->averageRunTime);
    00853   MODIFY("cognition:frameCounter",this->frameCounter);
    00854   MODIFY("cognition:maxRunTime",this->maxRunTime);
    00855 #endif
    00856 #endif
    00857 #endif
    00858
    00859   //runTimeCounter %= 100;
    00860   //cognitionAvarageRunTime = SystemCall::getTimeSince(cognitinStartTime)
    00861   //                        + cognitionAvarageRunTime*runTimeCounter;
    00862   //cognitionAvarageRunTime /= ++runTimeCounter;
    00863   //MODIFY("cognition:averageruntime",cognitionAvarageRunTime);
    00864   //MODIFY("cognition:runTimeCounter",runTimeCounter);
    00865   //MODIFY("cognition:runTimeCounter",runTimeCounter);
    00866   //MODIFY("RUNTIME:pImageProcessor:averageRunTime",pImageProcessor->averageRunTime);
    00867   //MODIFY("RUNTIME:pImageProcessor:frameCounter",pImageProcessor->frameCounter);
    00868   //MODIFY("RUNTIME:pImageProcessor:maxRunTime",pImageProcessor->maxRunTime);
    00869   //MODIFY("RUNTIME:pImageProcessor:minRunTime",pImageProcessor->minRunTime);
    00870   return 0;
    00871 }
    00872
    00873
    00874 void Cognition::init()
    00875 {
    00876
    00877 #ifndef NDEBUG
    00878   INIT_GTCAM_TIMESYNC_HANDLER(gtCamWorldStateIN,theImageReceiver);
    00879   INIT_GTCAM_HANDLER(gtCamWorldStateIN,theGTCamTimeSyncHandler);
    00880 #endif
    00881
    00882 #ifdef APERIOS1_3_2
    00883 #ifndef NSTATUSBROADCASTHANDLER
    00884   INIT_STATUSBROADCAST_HANDLER;
    00885 #endif
    00886 #endif
    00887
    00888   INIT_UDP_HANDLER(TeamMessage1);
    00889   INIT_UDP_HANDLER(TeamMessage2);
    00890   INIT_UDP_HANDLER(TeamMessage3);
    00891 #ifdef FIVEDOGS
    00892   INIT_UDP_HANDLER(TeamMessage4);
    00893 #endif
    00894   START_UDP_HANDLER;
    00895
    00896   START_UDP_BROADCAST_HANDLER(GameControlData,3838);
    00897
    00898   // !!! don't remove that lines, it is needed to trigger the Debug process to send
    00899   // debug requests back.
    00900   OUTPUT(idText,text,"player: " << Player::getTeamColorName(getPlayer().getTeamColor())
    00901     << " " << Player::getPlayerNumberName(getPlayer().getPlayerNumber())
    00902     << ", MAC Address: " << getRobotConfiguration().getMacAddressString());
    00903 #ifndef _WIN32
    00904   theTraceSender.send();
    00905 #endif
    00906 }
    00907
    00908
    00909 bool Cognition::handleMessage(InMessage& message)
    00910 {
    00911   switch (message.getMessageID())
    00912   {
    00913   case idDebugDataChangeRequest:
    00914     processImage = true;
    00915     return debugDataTable.processChangeRequest( message.bin);
    00916   case idSensorData:
    00917     message.bin >> theSensorDataBufferReceiver;
    00918     processSensorData = true;
    00919     return true;
    00920   case idImage:
    00921     message.bin >> RECEIVE_IMAGE(theImageReceiver,cameraMatrix);
    00922     cameraMatrix.frameNumber = theImageReceiver.frameNumber;
    00923     frameNumber = theImageReceiver.frameNumber;
    00924     processImage = true;
    00925     processSensorData = false;
    00926     return true;
    00927   case idJPEGImage:
    00928     {
    00929       JPEGImage jpe;
    00930       message.bin >> jpe >> cameraMatrix;
    00931       jpe.toImage(theImageReceiver);
    00932       cameraMatrix.frameNumber = theImageReceiver.frameNumber;
    00933       frameNumber = theImageReceiver.frameNumber;
    00934       processImage = true;
    00935       processSensorData = false;
    00936       return true;
    00937     }
    00938   case idLowResImage:
    00939     {
    00940       LowResImage lrImage(theImageReceiver);
    00941       message.bin >> lrImage >> cameraMatrix;
    00942       // cameraMatrix.frameNumber = lrImage.frameNumber;
    00943       return true;
    00944     }
    00945   case idPercepts:
    00946     {
    00947
    00948       Player pl;
    00949       message.bin >> RECEIVE_PERCEPTS( thePackageCognitionMotionSender.robotPose, cameraMatrix, theImageReceiver.cameraInfo, ballPercept, thePackageCognitionMotionSender.landmarksPercept,
    00950         linesPercept, edgesPercept,playersPercept, obstaclesPercept, psdPercept, collisionPercept);
    00951       processPercepts = true;
    00952       return true;
    00953     }
    00954   case idRemoteCamWorldState:
    00955   case idWorldState:
    00956     {
    00957       Player pl;
    00958       message.bin >> RECEIVE_WORLDSTATE(thePackageCognitionMotionSender.robotPose,
    00959         thePackageCognitionMotionSender.ballModel,
    00960         thePackageCognitionMotionSender.playerPoseCollection,
    00961         thePackageCognitionMotionSender.obstaclesModel,
    00962         thePackageCognitionMotionSender.robotState,
    00963         cameraMatrix,
    00964         theImageReceiver.cameraInfo);
    00965       cameraMatrix.frameNumber = theImageReceiver.frameNumber;
    00966       return true;
    00967     }
    00968   case idGTCamWorldState:
    00969     {
    00970       message.bin >> gtCamWorldStateIN;
    00971       return true;
    00972     }
    00973   case idHeadControlMode:
    00974     message.bin >> thePackageCognitionMotionSender.headControlMode;
    00975     return true;
    00976   case idMotionRequest:
    00977     message.bin >> thePackageCognitionMotionSender.motionRequest;
    00978     return true;
    00979   case idLEDRequest:
    00980     message.bin >> thePackageCognitionMotionSender.ledRequest;
    00981     return true;
    00982   case idGameControlData:
    00983     message.bin >> theGameControlDataReceiver;
    00984     return true;
    00985   case idSoundRequest:
    00986     message.bin >> thePackageCognitionMotionSender.soundRequest;
    00987     return true;
    00988   case idSpecialPercept:
    00989     {
    00990       Player pl;
    00991       CameraMatrix cm;
    00992       message.bin >> pl >> specialPercept >> cm;
    00993       // ???? cameraMatrix.frameNumber = theImageReceiver.frameNumber; ????
    00994       processPercepts = true;
    00995       return true;
    00996     }
    00997   case idOracledWorldState:
    00998     {
    00999       // send an oracled world state immediately back for paint synchronization
    01000       message >> debugOut;
    01001       return true;
    01002     }
    01003   case idCameraParameters:
    01004     {
    01005       message.bin >> cameraParameters;
    01006       this->setCameraParameters(cameraParameters);
    01007       return true;
    01008     }
    01009   case idColorTable64:
    01010     {
    01011       ColorTable64 colorTable64;
    01012       message.bin >> colorTable64;
    01013       memcpy(colorTableBuffer,&colorTable64,sizeof(ColorTable64));
    01014       processImage = true;
    01015       pImageProcessor->handleMessage(message);
    01016       //OUTPUT(idText,text,"Using ColorTable64");
    01017       return true;
    01018     }
    01019   case idColorTableTSL:
    01020     {
    01021       ColorTableTSL colorTableTSL;
    01022       message.bin >> colorTableTSL;
    01023       colorTableTSL.calculateLUT();
    01024       memcpy(colorTableBuffer,&colorTableTSL,sizeof(ColorTableTSL));
    01025       processImage = true;
    01026       //OUTPUT(idText,text,"Using ColorTableTSL");
    01027       return true;
    01028     }
    01029   case idImageProcessorParameters:
    01030     {
    01031       pImageProcessor->handleMessage(message);
    01032       return true;
    01033     }
    01034   case idLinesSelfLocatorParameters:
    01035   case idDDDSelfLocatorParameters:
    01036     {
    01037       pSelfLocator->handleMessage(message);
    01038       return true;
    01039     }
    01040   case idXabsl2DebugRequest:
    01041   case idKickSelectionTable:
    01042   case idGT2004EvolutionRequest:
    01043   case idGT2004Parameters:
    01044   case idBehaviorEvolutionPopulation:
    01045   case idBehaviorEvolutionParameters:
    01046     pBehaviorControl->handleMessage(message);
    01047     return true;
    01048   case idJoystickData:
    01049     message.bin >> joystickData;
    01050     return true;
    01051   case idBodyOffsets:
    01052     getRobotConfiguration().handleMessage(message);
    01053     return true;
    01054   case idGenericDebugData:
    01055     {
    01056       GenericDebugData d;
    01057       message.bin >> d;
    01058       if(d.id == GenericDebugData::teamMessageSendDelay)
    01059       {
    01060         OUTPUT(idText,text,"generic debug message (teamMessageSendDeley) - set Delay to " << d.data[0]);
    01061         teamMessageCollection.setDelay( (int)floor(d.data[0]));
    01062       }
    01063     }
    01064     return true;
    01065   case idDebugRequest:
    01066     {
    01067       processImage = true;
    01068       processSensorData = true;
    01069       return Process::handleMessage(message);
    01070     }
    01071
    01072   default:
    01073     {
    01074       return Process::handleMessage(message);
    01075     }
    01076   }
    01077 }
    01078
    01079 MAKE_PROCESS(Cognition);