worldmodel.c

uva trilearn的robocup源程序

  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_OFFSIDE_RIGHT  && getSide() == SIDE_RIGHT ) ||
         ( pm == PM_OFFSIDE_LEFT   && getSide() == SIDE_LEFT );
}

/*! This method checks whether the current play mode indicates that the other
    team stood offside. When the specified PlayModeT equals PM_ILLEGAL 
    (default), the current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team stood offside. */
bool WorldModel::isOffsideThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_OFFSIDE_LEFT  && getSide() == SIDE_RIGHT ) ||
         ( pm == PM_OFFSIDE_RIGHT && getSide() == SIDE_LEFT );
}

/*! This method checks whether the current play mode indicates that we have
    a kick in. When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether we have a kick in. */
bool WorldModel::isKickInUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_KICK_IN_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_IN_RIGHT && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that the other
    team has a kick in. When the specified PlayModeT equals PM_ILLEGAL 
    (default), the current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team has a kick in. */
bool WorldModel::isKickInThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_KICK_IN_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_IN_LEFT   && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that we have
    made a free kick fault. This happens when a player kicks the ball twice 
    after a free kick or a kick in. 
    When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether we have made a free kick fault. */
bool WorldModel::isFreeKickFaultUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_FREE_KICK_FAULT_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_FREE_KICK_FAULT_RIGHT && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that the other
    team has made a free kick fault. This happens when a player kicks the ball 
    twice after a free kick or a kick in. 
    When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team has made a free kick fault.*/
bool WorldModel::isFreeKickFaultThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_FREE_KICK_FAULT_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_FREE_KICK_FAULT_LEFT   && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that we have
    a kick off. When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether we have a kick off. */
bool WorldModel::isKickOffUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_KICK_OFF_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_OFF_RIGHT && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that the other
    team has a kick off. When the specified PlayModeT equals PM_ILLEGAL 
    (default), the current play mode is used. When the specified PlayModeT 
    equals PM_ILLEGAL (default), the current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team has a kick off. */
bool WorldModel::isKickOffThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_KICK_OFF_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_OFF_LEFT   && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that we have
    made a back pass (which is not allowed). This occurs when a teamamte has
    passed the ball back to the goalkeeper and he catched it.
    When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether we have made a back pass. */
bool WorldModel::isBackPassUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that the other
    team has made a back pass (which is not allowed). This occurs when an
    opponent has passed the ball back to the goalkeeper and he catched it.
    When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team has made a back pass. */
bool WorldModel::isBackPassThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that we have
    a goal kick. When the specified PlayModeT equals PM_ILLEGAL (default), the 
    current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether we have a goal kick. */
bool WorldModel::isGoalKickUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
}

/*! This method checks whether the current play mode indicates that the other
    team has a kick off. When the specified PlayModeT equals PM_ILLEGAL 
    (default), the current play mode is used.
    \param pm play mode to check. In default case (PM_ILLEGAL) the current play
           mode is used. 
    \return bool indicating whether the other team has a kick off. */
bool WorldModel::isGoalKickThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL ) 
    pm = getPlayMode();

  return ( pm == PM_GOAL_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_GOAL_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
}


/*! This method prints all the objects and information of the agent to the
    specified outputstream. Only the information of the objects that are seen
    recently are printed.
    \param os output stream to which output is written (default cout). */
void WorldModel::show( ostream & os )
{
  int i;
  os << "Worldmodel (" << getCurrentTime() << ")\n" <<
        "========================\n";
  os << "Teamname: " << getTeamName() << endl;
  if( Ball.getTimeLastSeen( ).getTime() != -1 )
    Ball.show();
  os << "Teammates: " << endl;
  for( i = 0; i < MAX_TEAMMATES ; i++ )
    if( isConfidenceGood( Teammates[i].getType() ) )
      Teammates[i].show( getTeamName() );
  os << "Opponents: " << endl;
  for( i = 0; i < MAX_OPPONENTS ; i++ )
    if( isConfidenceGood( Opponents[i].getType() ) )
      Opponents[i].show( DEFAULT_OPPONENT_NAME );
  os << "Agent: " << endl;
  agentObject.show( getTeamName() );

  os << "General Info: " << endl <<
          "side: "      << SoccerTypes::getSideStr( getSide() )    << endl <<
          "kicks: "     << getNrOfCommands( CMD_KICK )             << endl <<
          "turns: "     << getNrOfCommands( CMD_TURN )             << endl <<
          "dashes: "    << getNrOfCommands( CMD_DASH )             << endl <<
          "turnnecks: " << getNrOfCommands( CMD_TURNNECK )         << endl <<
          "says: "      << getNrOfCommands( CMD_SAY )              << endl <<
          "playmode: "  << SoccerTypes::getPlayModeStr( playMode ) << endl <<
          "===================================="                   << endl;

}

/*! This method prints all the objects and information contained in the object
    set 'set' to specified outputstream. 
    \param os output stream to which output is written (default cout). */
void WorldModel::show( ObjectSetT set, ostream & os )
{
  int iIndex;
  for( ObjectT o = iterateObjectStart( iIndex, set, -1.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext( iIndex, set, -1.0 ) )
    show( o, os );
  os << endl;
}

/*! This method prints the queued commands - commands that are sent by the
    ActHandler to the server - to the specified output stream.
    \param os output stream to which information is printed (default cout)*/
void WorldModel::showQueuedCommands( ostream & os )
{
  os << "Commands in queue:\n" ;
  for( int i = 0; i < MAX_COMMANDS; i++ )
    if( queuedCommands[i].commandType != CMD_ILLEGAL )
      queuedCommands[i].show( os );
}

/*! This method prints the information about the Object o to the output stream
    os.
    \param o object of which information should be printed
    \param os output stream to which information is printed. */
void WorldModel::show( ObjectT o, ostream &os )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
  {
    if( SoccerTypes::isPlayer( o ) )
    {
      PlayerObject *pobj = (PlayerObject*) object ;
      if( SoccerTypes::isTeammate( o ) )
        pobj->show( getTeamName(), os );
      else
        pobj->show( "Opponent", os );
    }
    else
      object->show( os );    
  }
}

/*! This method blocks till new information has arrived. Information is either
    a sense_body message or a see message. If there isn't received information
    from the server for longer than 3 seconds, server is assumed dead and
    false is returned.
    \return true when new info has arrived, false if server is dead */
bool WorldModel::waitForNewInformation( )
{
  bool bReturn = true;
  if( bNewInfo == false ) // there hasn't arrived any information yet
  {
    struct timeval now;
    struct timespec timeout;
    gettimeofday(&now, NULL);
    timeout.tv_sec = now.tv_sec + PS->getServerTimeOut();
    timeout.tv_nsec = now.tv_usec*1000;

    // lock mutex and wait till it is unlocked by Sense thread
    // this happens in setTimeLastSeeMessage, setTimeLastSenseMessage
    // or setTimeLastSeeGlobalMessage
    pthread_mutex_lock( &mutex_newInfo );
    int ret;
    Log.logWithTime( 2, "go into conditional wait" );
    while( (ret = pthread_cond_timedwait( &cond_newInfo,
                    &mutex_newInfo, &timeout) ) == EINTR )
     printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
    Log.logWithTime( 2, "go out of conditional wait" );     
    if( ret == ETIMEDOUT ) // if no information was received but timer timed out
      bReturn = false;
    pthread_mutex_unlock( &mutex_newInfo );
  }
  // reset the indication of new visual information
  bNewInfo = false;

  return bReturn;
}

/*! This method logs all object information that is currently stored in the
    World Model. The output is formatted as follows. First the current time
    (cycle_nr,cycle_stopped) is printed, followed by the object information
    of the specified object 'obj'. The global x and global y position are first
    printed, followed by the global x and global y velocity. Finally the body
    and the neck angle are printed. Then the information of the ball
    "pos_x pos_y vel_x vel_y conf" is printed, followed by the information of
    all eleven teammates "nr pos_x pos_y vel_x vel_y conf" and the same
    information about the eleven opponents. Values that are currently not known
    are replaced by the value -10.0000. This method is normally used by a
    player to log every cycle all its information contained in the world model
    to a file. If the coach (with perfect information) does the same, these two
    files can be analyzed to calculate the error for the different values.
    \param iLogLevel loglevel for which information should be printed
    \param obj object of which information should be printed at start line
               of this object also the body and neck angle are printed. */
void WorldModel::logObjectInformation( int iLogLevel, ObjectT obj )
{
  
  char   str[2048];
  double dConf     = PS->getPlayerConfThr();
  sprintf( str, "(%4d,%3d) ", getCurrentTime().getTime(),
                              getCurrentTime().getTimeStopped() );
  if( obj != OBJECT_ILLEGAL )
    sprintf( str, "%12.6f %12.6f %12.6f %12.6f %12.6f %12.6f",
                                  getGlobalPosition(obj).getX(),
                                  getGlobalPosition(obj).getY(),
                                  getGlobalVelocity(obj).getX(),
                                  getGlobalVelocity(obj).getY(),
                                  getGlobalBodyAngle(obj),
                                  getGlobalNeckAngle(obj) );

  if( getConfidence      ( OBJECT_BALL ) > dConf &&
      getRelativeDistance( OBJECT_BALL ) < 20.0 )
    sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                  getGlobalPosition(OBJECT_BALL).getX(),
                                  getGlobalPosition(OBJECT_BALL).getY() );
  else
    sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

  if( getTimeGlobalVelocity( OBJECT_BALL ) > getTimeFromConfidence( dConf ) &&
      getRelativeDistance  ( OBJECT_BALL ) < 20.0 )
    sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                  getGlobalVelocity(OBJECT_BALL).getX(),
                                  getGlobalVelocity(OBJECT_BALL).getY() );
  else
    sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
  sprintf( &str[strlen(str)], " %12.6f", getConfidence(OBJECT_BALL) );

  int    iIndex=-1;
  int    iIndexPlayer;
  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, 0.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext( iIndex, OBJECT_SET_PLAYERS, 0.0 ) )
  {
    bool bPrint = false;
    
    iIndexPlayer = (SoccerTypes::isTeammate(o))
                     ? SoccerTypes::getIndex(o) + 1
                     : SoccerTypes::getIndex(o) + 12;
    sprintf( &str[strlen(str)], " %d", iIndexPlayer );
    if( getConfidence      ( o ) > dConf && isKnownPlayer(o) &&
        getRelativeDistance( o ) < 20.0 )
    {
      sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                    getGlobalPosition(o).getX(),
                                    getGlobalPosition(o).getY() );
      bPrint = true;
    }
    else
       sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

    if( getTimeGlobalVelocity( o ) > getTimeFromConfidence( dConf ) &&
        getRelativeDistance  ( o ) < 20.0 && isKnownPlayer(o)  )
    {
      sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                    getGlobalVelocity(o).getX(),
                                    getGlobalVelocity(o).getY() );
      bPrint = true;
    }
    else
      sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

    if( bPrint )
      sprintf( &str[strlen(str)], " %12.6f", getConfidence(o) );
    else
      sprintf( &str[strlen(str)], " %12.6f", -10.0 );
  }
  if( getCurrentCycle() != 3000  )
      Log.log( iLogLevel, str );
}