worldmodel.cpp

2003年RoboCup仿真组世界冠军源代码 足球机器人 仿真组 的源代码

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*! \file WorldModel.cpp
<pre>
<b>File:</b>          WorldModel.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: UvA Trilearn
<b>Authors:</b>       Jelle Kok
<b>Created:</b>       12/02/2001
<b>Last Revision:</b> $ID$
<b>Contents:</b>      class definitions of WorldModel. This class contains
               the methods of the WorldModel that process the incoming
               information and analyze the current state of the world.
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
12/02/2001       Jelle Kok       Initial version created
</pre>
*/

#include<stdio.h>    // needed for printf
#include<errno.h>    // needed for ETIMEDOUT
#ifdef WIN32
  #include<windows.h>
#else
  #include<strings.h>  // needed for strcpy
  #include<pthread.h>  // needed for pthread_mutex_init
#endif
#include<string.h>   // needed for strcpy
#include<math.h>     // needed for erf
#include<map>        // needed for map
#include"WorldModel.h"

/*****************************************************************************/
/********************** CLASS WORLDMODEL *************************************/
/*****************************************************************************/

/*! This constructor creates the worldmodel, all variables are initialized by
    default values
    \param ss reference to class in which all server parameters are stored
    \param ps reference to class in which all client parameters are stored
    \param fs reference to class in which all formation information is stored*/
WorldModel::WorldModel( ServerSettings *ss, PlayerSettings *ps, 
                        Formations *fs):agentObject(  )
{
  dTotalVarVel   = 0.0;
  dTotalVarPos   = 0.0;
  SS             = ss;
  PS             = ps;
  formations     = fs;
  bNewInfo       = false;

  setSide         ( SIDE_ILLEGAL ); // is set by init message
  strTeamName[0] = '\0';
  setPlayMode     ( PM_BEFORE_KICK_OFF );
  iGoalDiff      = 0;
  m_sidePenalty = SIDE_ILLEGAL;

  int i;
  for( i = 0; i < MAX_TEAMMATES ; i ++ )
    Teammates[i].setType( SoccerTypes::getTeammateObjectFromIndex( i ) );
  for( i = 0; i < MAX_OPPONENTS ; i ++ )
    Opponents[i].setType( SoccerTypes::getOpponentObjectFromIndex( i ) );
  for( i = 0; i < MAX_OPPONENTS + MAX_TEAMMATES ; i ++ )
    UnknownPlayers[i].setType( OBJECT_ILLEGAL );
  for( i = 0; i < MAX_FLAGS; i ++ )
    Flags[i].setType( OBJECT_ILLEGAL );
  for( i = 0; i < MAX_LINES; i ++ )
    Lines[i].setType( OBJECT_ILLEGAL );

  iNrUnknownPlayers     = 0;

  Ball.setType             ( OBJECT_BALL    );
  agentObject.setType      ( OBJECT_ILLEGAL );
  agentObject.setStamina   ( Stamina(SS->getStaminaMax(),1.0,1.0) );

  for( i = 0 ; i < CMD_MAX_COMMANDS ; i ++ )
  {
    queuedCommands[i].commandType = (CommandT)i;
    performedCommands[i]          = false;
    iCommandCounters[i]           = 0;
  }

  iNrHoles              = 0;
  iNrOpponentsSeen      = 0;
  iNrTeammatesSeen      = 0;
  bsCheckBall           = BS_ILLEGAL;

  // initialize the mutex for bNewInfo
#ifdef WIN32
  InitializeCriticalSection( &mutex_newInfo );
  event_newInfo = CreateEvent( NULL, TRUE, FALSE, NULL );
#else
  pthread_mutex_init( &mutex_newInfo, NULL );
  pthread_cond_init ( &cond_newInfo,  NULL );
#endif
  m_bRecvThink          = false;
  timeLastSenseMessage  = Time( 0, 1 );

  strcpy( m_colorPlayers[0], "ffb0b0" );   // pink
  strcpy( m_colorPlayers[1], "ffb000" );   // orange
  strcpy( m_colorPlayers[2], "00b000" );   // darkgreen
  strcpy( m_colorPlayers[3], "00ffff" );   // lightblue
  strcpy( m_colorPlayers[4], "ff00ff" );   // purple
  strcpy( m_colorPlayers[5], "ffff00" );   // yellow
  strcpy( m_colorPlayers[6], "ffffff" );   // white
  strcpy( m_colorPlayers[7], "0000ff" );   // blue
  strcpy( m_colorPlayers[8], "00ff00" );   // green
  strcpy( m_colorPlayers[9], "ff0000" );   // red
  strcpy( m_colorPlayers[10],"aaaaaa" );   // gray

  for( i = 0 ; i < MAX_FEATURES ; i ++ )
  {
    m_features[i].setTimeSee( Time( UnknownTime, 0 ) );
    m_features[i].setTimeSense( Time( UnknownTime, 0 ) );
  }
}

/*! Destructor */
WorldModel::~WorldModel()
{
#ifdef WIN32
  DeleteCriticalSection( &mutex_newInfo );
#endif
}

/*! This method returns a pointer to the Object information of the object
    type that is passed as the first argument.
    \param o ObjectType of which information should be returned
    \return pointer to object information of supplied ObjectT argument */
Object* WorldModel::getObjectPtrFromType( ObjectT o )
{
  Object *object = NULL;
  if( o == OBJECT_ILLEGAL )
    return NULL;

  if( SoccerTypes::isKnownPlayer( o ) )
  {
    if( o == agentObject.getType() )
      object = &agentObject;
    else if( SoccerTypes::isTeammate( o ) )
      object = &Teammates[SoccerTypes::getIndex(o)];
    else
      object = &Opponents[SoccerTypes::getIndex(o)];
  }
  else if( SoccerTypes::isFlag( o ) )
    object = &Flags[SoccerTypes::getIndex(o)];
  else if( SoccerTypes::isLine( o ) )
    object = &Lines[SoccerTypes::getIndex(o)];
  else if( SoccerTypes::isBall( o ) )
    object = &Ball;
  else if( o == OBJECT_OPPONENT_GOALIE )
    return getObjectPtrFromType( getOppGoalieType() );
  else if( o == OBJECT_TEAMMATE_GOALIE )
    return getObjectPtrFromType( getOwnGoalieType() );
 return object;
}

/*! This method sets the time of the last catch cycle. This information is
    received by the SenseHandler when the referee has sent this message.
    After a catch, the goalie is not allowed to catch the ball for
    catch_ban_cycles (defined in ServerSettings).
    \param iTime time the ball was catched. */
void WorldModel::setTimeLastCatch( Time time )
{
  timeLastCatch = time;
}

/*! This method returns the number of cycles since the last catch.
    \return cycles since last catch. */
int WorldModel::getTimeSinceLastCatch()
{
  if( timeLastCatch.getTime() == -1 )
    return 1000;
  return timeLastSenseMessage - timeLastCatch;
}

/*! This method sets the time of the last received referee message. This
    information is received by the SenseHandler.
    \param iTime time the referee sent the last message. */
bool WorldModel::setTimeLastRefereeMessage( Time time )
{
  timeLastRefMessage = time;
  return true;
}

/*! This method returns the time of the last received referee message.
    \return time of last received referee message. */
Time WorldModel::getTimeLastRefereeMessage( )
{
  return timeLastRefMessage;
}

/*! This method returns the current time. In case of a player this is the
    time of the last sense message, in case of the coach this is the time of
    the last see_global message.
    \return actual time */
Time WorldModel::getCurrentTime()
{
  if( getPlayerNumber() == 0 )
    return getTimeLastSeeGlobalMessage();
  else
    return getTimeLastRecvSenseMessage();
}

/*! This method returns the current cycle number. In case of a player this is
    the cycle of the last sense message, in case of the coach this is the cycle
    of the last see_global message.
    \return actual time */
int WorldModel::getCurrentCycle()
{
  return getCurrentTime().getTime();
}


/*! This method returns whether the time of the server stands
    still. This occurs during non play-on modes (kick_in, kick_off,
    etc.).

    \return bool indicating whether time of the server stands still. */
bool WorldModel::isTimeStopped()
{
  return getCurrentTime().isStopped();
}

/*! This method returns whether the last received message was a see or not.
    \return bool indicating whether the last received message was a see.*/
bool WorldModel::isLastMessageSee() const
{
  return getTimeLastSeeMessage() == getTimeLastSenseMessage() ;
}

/*! This method returns the time of the last see global message.
    This message can only be received by the coach.
    \return time of last see_global message */
Time WorldModel::getTimeLastSeeGlobalMessage( ) const
{
  return getTimeLastRecvSeeMessage();
}

/*! This method sets the time of the last see_global message.
    \param time see message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSeeGlobalMessage( Time time )
{
  updateRelativeFromGlobal( );
  return setTimeLastSeeMessage( time ); // set see message
}

/*! This method returns the time of the last see message
    \return time of last see message */
Time WorldModel::getTimeLastSeeMessage( ) const
{
  return timeLastSeeMessage;
}

/*! This method returns the time of the last received see message.
    The difference with getTimeLastSeeMessage is that that method returns
    the last see message that has been updated in the world model. In most
    cases these are equal.
    \return time of last received sense message */
Time WorldModel::getTimeLastRecvSeeMessage( ) const
{
  return timeLastRecvSeeMessage ;
}
/*! This method sets the time of the last see message. It also sends a
    condition signal to indicate that variable bNewInfo has
    changed. When main thread is stopped in waitForNewInformation, it
    is unblocked.

    \param time see message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSeeMessage( Time time )
{
  timeLastRecvSeeMessage  = time;
  if( SS->getSynchMode() == false )
  {
#ifdef WIN32
    //EnterCriticalSection( &mutex_newInfo );
    bNewInfo            = true;
    SetEvent            (  event_newInfo );
    //LeaveCriticalSection( &mutex_newInfo );
#else
    pthread_mutex_lock  ( &mutex_newInfo );
    bNewInfo            = true;
    pthread_cond_signal ( &cond_newInfo );
    pthread_mutex_unlock( &mutex_newInfo );
#endif
  }

  return true;
}

/*! This method returns the time of the last sense message
    \return time of last sense message */
Time WorldModel::getTimeLastSenseMessage( ) const
{
  return timeLastSenseMessage ;
}

/*! This method returns the time of the last received sense message.
    The difference with getTimeLastSenseMessage is that that method returns
    the last sense message that has been updated in the world model. In most
    cases these are equal.
    \return time of last received sense message */
Time WorldModel::getTimeLastRecvSenseMessage( ) const
{
  return timeLastRecvSenseMessage ;
}

  
/*! This method sets the time of the last sense message. It also send a
    condition signal to indicate that variable bNewInfo has changed.
    When main thread is stopped in waitForNewInformation, it is
    unblocked.
    \param time sense message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSenseMessage( Time time )
{
  timeLastRecvSenseMessage = time;
  if( SS->getSynchMode() == false )
  {
#ifdef WIN32
    //EnterCriticalSection( &mutex_newInfo );
    bNewInfo = true;
    SetEvent            (  event_newInfo );
  //LeaveCriticalSection( &mutex_newInfo );
#else
    pthread_mutex_lock  ( &mutex_newInfo );
    bNewInfo = true;
    pthread_cond_signal ( &cond_newInfo );
    pthread_mutex_unlock( &mutex_newInfo );
#endif
  }

  return true;
}

/*! This method returns the time of the last hear message
    \return time of last hear message */
Time WorldModel::getTimeLastHearMessage( ) const
{