sensehandler.cpp

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

    // get the object type at the current position in the string
    o = SoccerTypes::getObjectFromStr( &strMsg, &isGoalie, WM->getTeamName() );
    if( o == OBJECT_ILLEGAL )
    {
      Log.log( 4, "Illegal object" );
      Log.log( 4, "total messages: %s", WM->strLastSeeMessage );
      Log.log( 4, "rest of message: %s", strMsg );
    }

    dX = Parse::parseFirstDouble( &strMsg );        // parse first value
    dY = Parse::parseFirstDouble( &strMsg );        // parse second value
    if ( *strMsg != ')' )                           // if it was no flag
    {
      dVelX = Parse::parseFirstDouble( &strMsg );   // parse delta x
      dVelY = Parse::parseFirstDouble( &strMsg );   // parse delta y
      if( *strMsg != ')' )                          // stil not finished
      {                                             // get body and neck angle
        angBody = Parse::parseFirstDouble( &strMsg );
        angNeck = Parse::parseFirstDouble( &strMsg );
      }
    }
    // skip ending bracket of object information.
    Parse::gotoFirstOccurenceOf( ')', &strMsg );
    strMsg++;

    // process the parsed information (unread values are Unknown...)
    WM->processSeeGlobalInfo( o, time, VecPosition(dX,dY),
                    VecPosition(dVelX,dVelY), angBody, angNeck );
    }
  WM->setTimeLastSeeGlobalMessage( time );  // set time last see global message
  return true;
}

/*! This method parses a full state message. This message contains all
    information from the playing field without noise. It will not be used
    during real tournaments. */
bool SenseHandler::analyzeFullStateMessage( char *strMsg )  
{
  Log.restartTimer( );
  Log.logWithTime( 2, " incoming fullstate message" );
  Log.log( 4, " fullstate message: %s", strMsg );  
  strcpy( WM->strLastSeeMessage, strMsg );

  ObjectT o;
  bool    isGoalie;  
  double  dX, dY, dVelX, dVelY;
  int     iTime;
  AngDeg  angBody, angNeck;
  Time    time = WM->getCurrentTime();

  iTime = Parse::parseFirstInt( &strMsg );         // get the time
  time.updateTime( iTime );
  Log.log( 4, "fullstate time: %d", time.getTime() );
  
  strMsg++;                                      // skip space
  Parse::gotoFirstOccurenceOf( ' ', &strMsg );   // skip (pmode
  strMsg++;                                      // skip space

  Log.log( 4, "fullstate parse ref: %s", strMsg );
  RefereeMessageT rm = SoccerTypes::getRefereeMessageFromStr( strMsg ); 
  PlayModeT       pm = SoccerTypes::getPlayModeFromRefereeMessage( rm );
  if( pm != PM_ILLEGAL )                                
    WM->setPlayMode( pm );                              
      
  Parse::gotoFirstOccurenceOf( 'e', &strMsg );   // go to end of vmode
  strMsg++;                                      // skip 'e'
  strMsg++;                                      // skip space

  Log.log( 4, "fullstate parse qua: %s", strMsg );
  ViewQualityT vq = SoccerTypes::getViewQualityFromStr( strMsg ); 
  Parse::gotoFirstOccurenceOf( ' ', &strMsg );   // go to end of quality
  strMsg++;                                      
  Log.log( 4, "fullstate parse ang: %s", strMsg );  
  ViewAngleT   va = SoccerTypes::getViewAngleFromStr( strMsg );

  Log.log( 4, "fullstate parse count: %s", strMsg );  
  WM->setNrOfCommands( CMD_KICK       , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_DASH       , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_TURN       , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_CATCH      , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_MOVE       , Parse::parseFirstInt( &strMsg ) );  
  WM->setNrOfCommands( CMD_TURNNECK   , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_CHANGEVIEW , Parse::parseFirstInt( &strMsg ) );
  WM->setNrOfCommands( CMD_SAY        , Parse::parseFirstInt( &strMsg ) );

  int iArmMovable = Parse::parseFirstInt( &strMsg );
  int iArmExpires = Parse::parseFirstInt( &strMsg );
  Parse::parseFirstDouble( &strMsg ); // skip pointto info, comes later
  Parse::parseFirstDouble( &strMsg ); // skip pointto info, comes later
  WM->setNrOfCommands( CMD_POINTTO    , Parse::parseFirstInt( &strMsg ) );

  Parse::gotoFirstOccurenceOf( 'b', &strMsg );   // go to ball position  
  
  Log.log( 4, "fullstate parse ball: %s", strMsg );    
  dX    = Parse::parseFirstDouble( &strMsg );    // parse first value
  dY    = Parse::parseFirstDouble( &strMsg );    // parse second value  
  dVelX = Parse::parseFirstDouble( &strMsg );    // parse third value  
  dVelY = Parse::parseFirstDouble( &strMsg );    // parse fourth value  
  if( WM->isBeforeKickOff() ) 
    dX = dY = dVelX = dVelY = 0.0;
  if( WM->getSide() == SIDE_RIGHT )
  {
    dX    *= -1;
    dY    *= -1;    
    dVelX *= -1;    
    dVelY *= -1;          
  }  
  WM->processSeeGlobalInfo( OBJECT_BALL, time, VecPosition(dX,dY),
                    VecPosition(dVelX,dVelY), -1, -1 );
  strMsg++;
  Log.log( 4, "fullstate ball: %f %f %f %f", dX, dY, dVelX, dVelY );
    
  while( *strMsg != ')' )                          // " ((objname.." or ")"
  {
    dVelX = dVelY = UnknownDoubleValue;
    angBody = angNeck = UnknownAngleValue;
    strMsg += 2;          // go the start of the object name
    Log.log( 4, "fullstate parse object: %s", strMsg );    
    // get the object type at the current position in the string
    o = SoccerTypes::getObjectFromStr( &strMsg, &isGoalie, 
                          (WM->getSide() == SIDE_LEFT ) ? "l" : "r" );

    dX      = Parse::parseFirstDouble( &strMsg );   // parse x position
    dY      = Parse::parseFirstDouble( &strMsg );   // parse y position
    dVelX   = Parse::parseFirstDouble( &strMsg );   // parse x velocity
    dVelY   = Parse::parseFirstDouble( &strMsg );   // parse y velocity
    angBody = Parse::parseFirstDouble( &strMsg );   // parse body angle
    angNeck = Parse::parseFirstDouble( &strMsg );   // parse neck angle

    if( WM->getSide() == SIDE_RIGHT )
    {
      dX    *= -1;
      dY    *= -1;    
      dVelX *= -1;    
      dVelY *= -1;          
      angBody = VecPosition::normalizeAngle( angBody + 180 );
    }    

    double dStamina  = Parse::parseFirstDouble( &strMsg );  // get stamina
    double dEffort   = Parse::parseFirstDouble( &strMsg );  // get effort
                       Parse::parseFirstDouble( &strMsg );  // skip recovery

    // skip ending bracket of stamina and then of object information.
    Parse::gotoFirstOccurenceOf( ')', &strMsg );
    Parse::gotoFirstOccurenceOf( ')', &strMsg );
    
    strMsg++;
    strMsg++;    

    Log.log( 1, "fullstate obj %d: %f %f %f %f %f %f", o, dX, dY, dVelX, dVelY,
                 angBody, angNeck );
    // process the parsed information 
    if( o == WM->getAgentObjectType() )
      WM->processNewAgentInfo( vq, va, dStamina, dEffort, -1.0, -1.0, -angNeck,
                               -1,iArmMovable, iArmExpires, VecPosition(0,0));
      
    WM->processSeeGlobalInfo( o, time, VecPosition(dX,dY),
                              VecPosition(dVelX,dVelY), angBody, angNeck );
    
  }
  WM->setTimeLastSeeGlobalMessage( time );  // set time last see global message
  WM->setTimeLastSenseMessage( time );      // set time last see global message

  return true;
}

/*! This method analyzes a sense message. All information from the player is
    parsed and updated in the WorldModel.
    A sense message looks like (sense_body 0 (view_mode high normal)
    (stamina 2000 1) (speed 0 0) (head_angle 0) (kick 0) (dash 0)
    (turn 0) (say 0) (turn_neck 0) (catch 0) (move 0) (change_view 0))
    \param strMsg message that should be parsed
    \return bool indicating whether the message was parsed correctly. */
bool SenseHandler::analyzeSenseMessage( char *strMsg )
{
  Log.log( 999, "%s", strMsg );
  // cerr << strMsg << endl; 
  // set the synchronization counter, this is a value [0..2] indicating the
  // section of the pattern this cycle is in. It gives an indication when new
  // visual information will arrive.

  if( SS->getSynchMode() == false )
    setTimeSignal();                        // set signal when to send action
  strcpy( WM->strLastSenseMessage, strMsg );

  if( WM->getRelativeDistance( OBJECT_BALL ) < SS->getVisibleDistance() )
    Log.logWithTime( 560, "%s", WM->strLastSenseMessage );

  int iTime = Parse::parseFirstInt( &strMsg );// get time
  Time timeOld = WM->getCurrentTime();
  Time timeNew = timeOld;
  timeNew.updateTime( iTime );

  if( timeNew.getTimeDifference( timeOld ) > 1 )
    Log.log( 1, "Missed a sense!!" );

  Log.logWithTime ( 2, "\n\nSENSE (%d, %d)", timeNew.getTime(),
  timeNew.getTimeStopped() );
  Log.restartTimer( );
  iSimStep               = SS->getSimulatorStep()*1000;
  iTimeSignal            = (int)(iSimStep*0.85); 
  Log.logWithTime ( 2, " alarm after %d", iTimeSignal );

  WM->setTimeLastSenseMessage( timeNew ); // set the time

//  Log.logWithTime( 2, " end analyzing sense" );
  return true;
}

/*! This method analyzes an init message. All information from the
    initialization is parsed and updated in the WorldModel.
    An init message looks like (init [l|r] 10 before_kick_off)
    \param strMsg message that should be parsed
    \return bool indicating whether the message was parsed correctly. */
bool SenseHandler::analyzeInitMessage( char *strMsg )
{
  Log.log( 999, "%s", strMsg );
  strMsg += 6;                                            // go to Side
  WM->setSide( SoccerTypes::getSideFromStr( strMsg ) );   // get and set Side
  int nr = Parse::parseFirstInt( &strMsg );               // get and set number
  if( nr == 0 )                                           // coach
  {
     WM->setPlayerNumber( nr );
     cerr << strMsg  << endl;
     return true;
  }
  WM->setAgentObjectType( SoccerTypes::getTeammateObjectFromIndex( nr - 1 ) );
  WM->setPlayerNumber( nr );
  strMsg++;                                               // skip space to pm
  WM->setPlayMode( SoccerTypes::getPlayModeFromStr( strMsg ) ); // get playmode
  return true;
}

/*! This method analyzes a hear message. When the message is from the
    referee the message is parsed and the new play mode is set or the
    goal difference is adjusted. When the message comes from another
    player the method analyzePlayerMessage is called A hear message
    looks like (hear 0 self|referee|dir message)

    \param strMsg message that should be parsed
    \return bool indicating whether the message was parsed correctly. */
bool SenseHandler::analyzeHearMessage( char *strMsg )
{
  RefereeMessageT rm;
  PlayModeT       pm;
  strcpy( WM->strLastHearMessage, strMsg);

  int iTime = Parse::parseFirstInt( &strMsg );              // ignore time
  Time time( iTime );

  switch( Parse::gotoFirstNonSpace( &strMsg ) )
  {
    case 'r':                                               // referee
      Log.log( 999, "%s", WM->strLastHearMessage );
      WM->setTimeLastRefereeMessage( time );
      Parse::gotoFirstOccurenceOf( ' ', &strMsg );          // go to start
      Parse::gotoFirstNonSpace   ( &strMsg      );          // and first part
      rm = SoccerTypes::getRefereeMessageFromStr( strMsg ); // get the ref msg
      Log.logWithTime( 2, " referee message: %s %s",
      SoccerTypes::getRefereeMessageStr(rm), WM->strLastHearMessage);
      pm = SoccerTypes::getPlayModeFromRefereeMessage( rm );// get play mode
      if( pm != PM_ILLEGAL )                                // from ref msg
        WM->setPlayMode( pm );                              // if was pm, set 

      switch( rm )
      {
        case REFC_GOAL_LEFT:                            // goal left
          if( WM->getSide() == SIDE_LEFT )
            WM->addOneToGoalDiff();
          else
            WM->subtractOneFromGoalDiff();
          WM->processSeeGlobalInfo( OBJECT_BALL, time, VecPosition( 0, 0 ),
               VecPosition( 0, 0 ), 0, 0 );
          break;
        case REFC_GOAL_RIGHT:                      // goal right
          if( WM->getSide() == SIDE_RIGHT )
            WM->addOneToGoalDiff();
          else
            WM->subtractOneFromGoalDiff();
          WM->processSeeGlobalInfo( OBJECT_BALL, time, VecPosition( 0, 0 ),
               VecPosition( 0, 0 ), 0, 0 );
          break;
        case REFC_GOALIE_CATCH_BALL_LEFT:         // catch ball
        case REFC_GOALIE_CATCH_BALL_RIGHT:
          WM->processCatchedBall( rm, time );
          break;
        case REFC_PENALTY_ONFIELD_LEFT:
          WM->setSidePenalty( SIDE_LEFT );
          break;
        case REFC_PENALTY_ONFIELD_RIGHT:
          WM->setSidePenalty( SIDE_RIGHT );
          break;
        case REFC_PENALTY_MISS_LEFT:
        case REFC_PENALTY_SCORE_LEFT:
          WM->setPlayMode( PM_FROZEN );
          break;
        case REFC_PENALTY_MISS_RIGHT:
        case REFC_PENALTY_SCORE_RIGHT:
          WM->setPlayMode( PM_FROZEN );         
          break;
        case REFC_PENALTY_FOUL_LEFT:
        case REFC_PENALTY_FOUL_RIGHT:
        default:
          break;
      }
      break;
    case 'o':                                               // online_coach_
        analyzeCoachMessage( strMsg );
        break;
    case 's':                                               // self
      break;                                                // do nothing
    default:                                                // from direction
        Log.logWithTime( 600, "incoming hear: %s", WM->strLastHearMessage );
        analyzePlayerMessage( iTime, strMsg );    // from player
      break;
  }

  return true;
}

/*! This message analyzes an incoming communication message. Messages from
    opponents are discarded. First it is checked whether the message arrived
    from a teammate using a specific encoding string and then the contents
    are parsed and stored in the world model, which will process it when it
    updates the world model.
    \param iTime time from the message */
bool SenseHandler::analyzePlayerMessage( int iTime, char *strMsg )