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

# Representations/WLan/TeamMessageCollection.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Representations/WLan/TeamMessageCollection.cpp
    00003 *
    00004 * Implementation of class TeamMessageCollection.
    00005 *
    00006 * @author <A href=mailto:sebastian.schmidt@udo.edu>Sebastian Schmidt</A>
    00007 * @author <A href=mailto:m_wachter@gmx.de>Michael Wachter</A>
    00008 */
    00009
    00010 #include "TeamMessageCollection.h"
    00011 #include "Platform/SystemCall.h"
    00012 #include "Tools/Player.h"
    00013 #include "Tools/Debugging/Debugging.h"
    00014
    00015 TeamMessageCollection::TeamMessageCollection()
    00016 {
    00017   lastSendingTimeStamp = 0;
    00018   for (int a=0; a< maxNumberOfTeamMessages; a++)
    00019   {
    00020     incomingTimeStamp[a] = teamMessageTimeStamp[a] = 0;
    00021   }
    00022
    00023   int i;
    00024
    00025   // all ringbuffer elements are set to zero.
    00026   for(int j=0;j< maxNumberOfTeamMessages ;j++)
    00027   {
    00028     for(int i=0;i<100;i++) offsets[j].add(0);
    00029     offsets[j].init();
    00030     timeOffsetSum[j]=0;
    00031     offsets[j].add(0);
    00032   }
    00033
    00034   messagesSent = false;
    00035   for(i=0;i< maxNumberOfTeamMessages ;i++)
    00036   {
    00037     teamMessages[i] = NULL;
    00038   }
    00039
    00040   for(i=0;i< maxNumberOfTeamMessages ;i++)
    00041   {
    00042     teamMessagesForActualTeamMessages[i] = 0;
    00043   }
    00044
    00045   numberOfInTeamMessages = 0;
    00046   numberOfOutTeamMessages = 0;
    00047   lastSendTime=0;
    00048   sendNow = false;
    00049   delay = 100;
    00050  }
    00051
    00052 void TeamMessageCollection::processTimeStamps()
    00053 {
    00054   signed long actualOffset;
    00055   for (int i=0; i < numberOfInTeamMessages; i++)
    00056   {
    00057   /*  Offset = (1/2) * ( T2 - T1 + T3 - T4 )  =  T5 - T4
    00058   *
    00059   *  T1 = LastReceivedTimeStamp
    00060   *  T2 = IncomimgTimeStamp
    00061   *  T3 = TimeStamp
    00062   *  T4 = System-Time
    00063   *  T5 = System-Time of the other Robot
    00064   *
    00065     */
    00066
    00067     actualOffset= ( (signed long)teamMessages[i]->incomingTimeStamp[teamMessages[i]->playerNumberOfSender] -
    00068       (signed long)teamMessages[i]->lastReceivedTimeStamp[(int)getPlayer().getPlayerNumber()] +
    00069       (signed long)teamMessages[i]->timeStamp -
    00070       (signed long)SystemCall::getCurrentSystemTime() ) / 2 ;
    00071
    00072     // calculating the average of the values in the ringBuffer
    00073     timeOffsetSum[i] += actualOffset;
    00074     timeOffsetSum[i] -= offsets[i].getEntry(-1);
    00075
    00076     (const_cast<TeamMessage*>(teamMessages[i]))->timeOffset =
    00077       //((TeamMessage*)teamMessages[i])->timeOffset =
    00078       timeOffsetSum[i] / offsets[i].getNumberOfEntries();
    00079     offsets[i].add(actualOffset);
    00080
    00081     // Setting timeStamps in teamMessages
    00082   }
    00083
    00084   for (int i=0; i<numberOfInTeamMessages; i++)
    00085   {
    00086     for (int j=0; j<numberOfOutTeamMessages; j++)
    00087     {
    00088        outTeamMessages[j]->incomingTimeStamp[(int)getPlayer().getPlayerNumber()] =
    00089        SystemCall::getCurrentSystemTime();
    00090        if (teamMessages[i]->playerNumberOfSender != Player::undefinedPlayerNumber)
    00091        {
    00092           outTeamMessages[j]->lastReceivedTimeStamp[(int)teamMessages[i]->playerNumberOfSender]
    00093           = teamMessages[i]->timeStamp;
    00094        }
    00095     }
    00096   }
    00097
    00098
    00099   INFO(sendTimeOffsets,idText,text,"OTM: ITS: " << outTeamMessages[0]->incomingTimeStamp[0] <<
    00100     " " << outTeamMessages[0]->incomingTimeStamp[1] <<
    00101     " " << outTeamMessages[0]->incomingTimeStamp[2] <<
    00102     " " << outTeamMessages[0]->incomingTimeStamp[3] <<
    00103     " LRTS: " << outTeamMessages[0]->lastReceivedTimeStamp[0] <<
    00104     " " << outTeamMessages[0]->lastReceivedTimeStamp[1] <<
    00105     " " << outTeamMessages[0]->lastReceivedTimeStamp[2] <<
    00106     " " << outTeamMessages[0]->lastReceivedTimeStamp[3]);
    00107
    00108   INFO(sendTimeOffsets,idText,text,"TM0: TS: " << teamMessages[0]->timeStamp <<
    00109     " POS: " << teamMessages[0]->playerNumberOfSender <<
    00110     " ITS: " << teamMessages[0]->incomingTimeStamp[0] <<
    00111     " " << teamMessages[0]->incomingTimeStamp[1] <<
    00112     " " << teamMessages[0]->incomingTimeStamp[2] <<
    00113     " " << teamMessages[0]->incomingTimeStamp[3] <<
    00114     " LRTS: " << teamMessages[0]->lastReceivedTimeStamp[0] <<
    00115     " " << teamMessages[0]->lastReceivedTimeStamp[1] <<
    00116     " " << teamMessages[0]->lastReceivedTimeStamp[2] <<
    00117     " " << teamMessages[0]->lastReceivedTimeStamp[3]);
    00118
    00119   INFO(sendTimeOffsets,idText,text,"TM1: TS: " << teamMessages[1]->timeStamp <<
    00120     " POS: " << teamMessages[1]->playerNumberOfSender <<
    00121     " ITS: " << teamMessages[1]->incomingTimeStamp[0] <<
    00122     " " << teamMessages[1]->incomingTimeStamp[1] <<
    00123     " " << teamMessages[1]->incomingTimeStamp[2] <<
    00124     " " << teamMessages[1]->incomingTimeStamp[3] <<
    00125     " LRTS: " << teamMessages[1]->lastReceivedTimeStamp[0] <<
    00126     " " << teamMessages[1]->lastReceivedTimeStamp[1] <<
    00127     " " << teamMessages[1]->lastReceivedTimeStamp[2] <<
    00128     " " << teamMessages[1]->lastReceivedTimeStamp[3]);
    00129
    00130   INFO(sendTimeOffsets,idText,text,"TM2: TS: " << teamMessages[2]->timeStamp <<
    00131     " POS: " << teamMessages[2]->playerNumberOfSender <<
    00132     " ITS: " << teamMessages[2]->incomingTimeStamp[0] <<
    00133     " " << teamMessages[2]->incomingTimeStamp[1] <<
    00134     " " << teamMessages[2]->incomingTimeStamp[2] <<
    00135     " " << teamMessages[2]->incomingTimeStamp[3] <<
    00136     " LRTS: " << teamMessages[2]->lastReceivedTimeStamp[0] <<
    00137     " " << teamMessages[2]->lastReceivedTimeStamp[1] <<
    00138     " " << teamMessages[2]->lastReceivedTimeStamp[2] <<
    00139     " " << teamMessages[2]->lastReceivedTimeStamp[3]);
    00140
    00141   INFO(sendTimeOffsets,idText,text,"TimeOffsets : "
    00142     << teamMessages[0]->timeOffset << " " << teamMessages[0]->playerNumberOfSender <<" "
    00143     << teamMessages[1]->timeOffset << " " << teamMessages[1]->playerNumberOfSender <<" "
    00144     << teamMessages[2]->timeOffset << " " << teamMessages[2]->playerNumberOfSender);
    00145 }
    00146
    00147
    00148 void TeamMessageCollection::processMessages()
    00149 {
    00150
    00151   numberOfTeamMessages = 0;
    00152   for(int a=0;a< numberOfInTeamMessages;a++)
    00153   {
    00154     // Set the teamMessagesForActualTeamMessages so that operator[] only returns actual
    00155     // teamMessages.
    00156
    00157     if (teamMessages[a]->isActual())
    00158     {
    00159       teamMessagesForActualTeamMessages[numberOfTeamMessages++] = a;
    00160     }
    00161   }
    00162
    00163   for(int i=0; i< numberOfOutTeamMessages; i++)
    00164   {
    00165     outTeamMessages[i]->resetSendMarkers();
    00166   }
    00167
    00168   processTimeStamps();
    00169 }
    00170
    00171 void TeamMessageCollection::setOutTeamMessages(TeamMessage& tm)
    00172 {
    00173   outTeamMessages[numberOfOutTeamMessages] = &tm;
    00174   numberOfOutTeamMessages++;
    00175
    00176 }
    00177
    00178 void TeamMessageCollection::setInTeamMessages(TeamMessage& tm)
    00179 {
    00180   teamMessages[numberOfInTeamMessages] = &tm;
    00181   numberOfInTeamMessages++;
    00182 }
    00183
    00184
    00185 void TeamMessageCollection::send(const RobotPose& robotPose,bool sendNow) const
    00186 {
    00187   for (int i=0;i< numberOfOutTeamMessages;i++)
    00188   {
    00189      outTeamMessages[i]->robotPose = robotPose ;
    00190      outTeamMessages[i]->sendRobotPose = true;
    00191   }
    00192   this->sendNow |= sendNow;
    00193 }
    00194
    00195 void TeamMessageCollection::send(const SeenBallState& seenBallState, bool sendNow) const
    00196 {
    00197   for (int i=0;i< numberOfOutTeamMessages;i++)
    00198   {
    00199       outTeamMessages[i]->seenBallState = seenBallState;
    00200       outTeamMessages[i]->sendSeenBallState = true;
    00201   }
    00202   this->sendNow |= sendNow;
    00203 }
    00204
    00205 void TeamMessageCollection::send(const BehaviorTeamMessage& behaviorTeamMessage,bool sendNow) const
    00206 {
    00207   for (int i=0;i< numberOfOutTeamMessages;i++)
    00208   {
    00209       outTeamMessages[i]->behaviorTeamMessage = behaviorTeamMessage;
    00210       outTeamMessages[i]->sendBehaviorTeamMessage = true;
    00211   }
    00212   this->sendNow |= sendNow;
    00213 }
    00214
    00215 void TeamMessageCollection::send(const PlayersPercept& playersPercept,bool sendNow) const
    00216 {
    00217   for (int i=0;i< numberOfOutTeamMessages;i++)
    00218   {
    00219       outTeamMessages[i]->playersPercept = playersPercept;
    00220       outTeamMessages[i]->sendPlayersPercept = true;
    00221   }
    00222   this->sendNow |= sendNow;
    00223 }
    00224
    00225 void TeamMessageCollection::send(const SensorBehaviorTeamMessage& sensorBehaviorTeamMessage, bool sendNow /* = false */) const
    00226 {
    00227   for (int i=0; i< numberOfOutTeamMessages; i++)
    00228   {
    00229     outTeamMessages[i]->sensorBehaviorTeamMessage = sensorBehaviorTeamMessage;
    00230     outTeamMessages[i]->sendSensorBehaviorTeamMessage = true;
    00231   }
    00232   this->sendNow |= sendNow;
    00233 }
    00234
    00235 void TeamMessageCollection::send(const MultipleBallPerceptList& multipleBallPerceptList, bool sendNow /* = false */) const
    00236 {
    00237   for (int i=0; i< numberOfOutTeamMessages; i++)
    00238   {
    00239     outTeamMessages[i]->multipleBallPerceptList = multipleBallPerceptList;
    00240     outTeamMessages[i]->sendMultipleBallPerceptList = true;
    00241   }
    00242   this->sendNow |= sendNow;
    00243 }
    00244
    00245 void TeamMessageCollection::send(const OdometryData& odometryData, bool sendNow /* = false */) const
    00246 {
    00247   for (int i=0; i< numberOfOutTeamMessages; i++)
    00248   {
    00249     outTeamMessages[i]->odometryData = odometryData;
    00250     outTeamMessages[i]->sendOdometryData = true;
    00251   }
    00252   this->sendNow |= sendNow;
    00253 }
    00254
    00255 void TeamMessageCollection::send(const GT2005ParticleContainerSend& gt2005ParticleContainerSend, bool sendNow /* = false */) const
    00256 {
    00257   for (int i=0; i< numberOfOutTeamMessages; i++)
    00258   {
    00259     outTeamMessages[i]->gt2005ParticleContainerSend = gt2005ParticleContainerSend;
    00260     outTeamMessages[i]->sendGT2005ParticleContainerSend = true;
    00261   }
    00262   this->sendNow |= sendNow;
    00263 }
    00264
    00265 void TeamMessageCollection::send(const RobotPoseCollection& robotPoseCollection, bool sendNow /* = false */) const
    00266 {
    00267   for (int i=0; i< numberOfOutTeamMessages; i++)
    00268   {
    00269     outTeamMessages[i]->robotPoseCollection = robotPoseCollection;
    00270     outTeamMessages[i]->sendRobotPoseCollection = true;
    00271
    00272   }
    00273   this->sendNow |= sendNow;
    00274 }
    00275
    00276 void TeamMessageCollection::send(const ObstaclesModel& obstaclesModel,bool sendNow) const
    00277 {
    00278   for (int i=0;i< numberOfOutTeamMessages;i++)
    00279   {
    00280     outTeamMessages[i]->obstaclesModel = obstaclesModel;
    00281     outTeamMessages[i]->sendObstaclesModel = true;
    00282   }
    00283   this->sendNow |= sendNow;
    00284 }
    00285
    00286 bool TeamMessageCollection::processOutMessages() {
    00287
    00288   // is te delay over ?
    00289   sendNow |= ((signed)(SystemCall::getCurrentSystemTime() - (signed) lastSendTime) > delay);
    00290   if (sendNow)
    00291   {
    00292     // set timestamps for all outgoing TeamMessages
    00293     for(int a=0;a< numberOfOutTeamMessages;a++) {
    00294       outTeamMessages[a]->timeStamp = SystemCall::getCurrentSystemTime();
    00295     }
    00296
    00297     messagesSent = true;
    00298     lastSendTime = SystemCall::getCurrentSystemTime();
    00299     sendNow = false;
    00300     return (true);
    00301   }
    00302   else return (false);
    00303 }
    00304
    00305
    00306 void TeamMessageCollection::setDelay(int delay)
    00307 {
    00308   this->delay = delay;
    00309 }
    00310
    00311 int TeamMessageCollection::getDelay() const
    00312 {
    00313   return (delay);
    00314 }
    00315
    00316
    00317
    00318
    00319