GT2005: Processes/CMD/Cognition.cpp Source 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);