sensehandler.cpp

浙江大学中控杯仿真组机器人源码

/*
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 SenseHandler.cpp
<pre>
<b>File:</b>          SenseHandler.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: UvA Trilearn
<b>Authors:</b>       Jelle Kok
<b>Created:</b>       28/11/2000
<b>Last Revision:</b> $ID$
<b>Contents:</b>      This file contains the class SenseHandler that is used
               to process the information coming from the server.
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
28/11/2000       Jelle Kok       Initial version created
</pre>
*/

#include "SenseHandler.h"
#include "ActHandler.h"  // sigalarmHandler
#include "Parse.h"
#include "Offclient.h"

#include <signal.h>   // needed for SIGALARM
#include <string.h>   // needed for strlen
#include <stdio.h>    // needed for printf
#include <iostream>   // needed for cout

/*****************************************************************************/
/********************* CLASS SENSEHANDLER ************************************/
/*****************************************************************************/

/*! This function is needed to start the Sense Thread (thread that continually
    waits for input and parses the input). This function is needed since it is
    not possible to call a method from a class using a thread. So this function
    calls handleMessagesFromServer from the SenseHandler class.
    \param v pointer to a SenseHandler class.*/
#ifdef WIN32
DWORD WINAPI sense_callback( LPVOID v )
#else
void* sense_callback( void *v )
#endif
{
  Log.log( 1, "Starting to listen for server messages" );
  SenseHandler* s = (SenseHandler*)v;
  s->handleMessagesFromServer( );
#ifdef WIN32
  return 0;
#else
  return NULL;
#endif
}

// motheds of CViewAngle
// by liming 06.10.11
CViewAngle g_viewAngle;

void CViewAngle::setTimeAfterSenceArrived() {
	#ifdef WIN32
		time1 = GetTickCount();
	#else
		gettimeofday( &time1, NULL );
	#endif
}

void CViewAngle::setTimeAfterSeeArrived() {
	#ifdef WIN32
		time2 = GetTickCount();
	#else
		gettimeofday( &time2, NULL );
	#endif
}



void CViewAngle::setViewAngleType() {
	double timeDiff;
	#ifdef WIN32
  		timeDiff = ((double)(time2 - time1) / 1000.0) ;
	#else
		timeDiff = ((double)time2.tv_sec + (double)time2.tv_usec/1000000 ) -
			((double)time1.tv_sec + (double)time1.tv_usec/1000000 ) ;
	#endif
//	cout << timeDiff << endl;
//	while ( timeDiff > 0.1)
//		timeDiff -= 0.1;
//	while ( timeDiff < -0.1 )
//		timeDiff += 0.1;
	if( timeDiff > 0.035 )
		m_viewAngleT = VA_NARROW;
	else
		m_viewAngleT = VA_NORMAL;
}

SoccerCommand CViewAngle::getCommandChangeViewType() {
	if ( m_viewAngleT == VA_NARROW )
		 return SoccerCommand( CMD_CHANGEVIEW, VA_NARROW, VQ_HIGH );
	else
		 return SoccerCommand( CMD_CHANGEVIEW, VA_NORMAL, VQ_HIGH );
}

//~motheds of CViewAngle


/*! Constructor for the SenseHandler. It needs a reference to a connection and
    a reference to a worldmodel.
    \param c Connection from which input is received
    \param wm WorldModel to which new information will be sent for processing
    \param ss ServerSettings that contain the parameters used by the server
    \param ps PlayerSettings that determine how to interact with messages. */
SenseHandler::SenseHandler( Connection *c, WorldModel *wm, ServerSettings *ss,
                            PlayerSettings *ps )
{
  connection             = c;
  SS                     = ss;
  PS                     = ps;
  WM                     = wm;
  iSimStep               = SS->getSimulatorStep()*1000;
  iTimeSignal            = (int)(iSimStep*0.85);

#ifdef WIN32
  TIMECAPS tc;
  UINT     resolution = 1;   // timer resolution for the application (ms)
  iTimer = 0;

  // set the minimum timer resolution for an application
  if (TIMERR_NOERROR == timeGetDevCaps( &tc, sizeof(TIMECAPS) ))
  {
    timerRes = min( max( tc.wPeriodMin, resolution ), tc.wPeriodMax );
    timeBeginPeriod( timerRes );
  }
#else
  struct sigaction sigact;

  sigact.sa_flags = SA_RESTART; // do not unblock primitives (like recvfrom)
  sigact.sa_handler = (void (*)(int))sigalarmHandler;
  sigaction( SIGALRM, &sigact, NULL );

  // set timer signal to indicate when ActHandler should sent commands to the
  // server, this structure will later be filled with exact timing values
  itv.it_interval.tv_sec  = 0;
  itv.it_interval.tv_usec = 0;
  itv.it_value.tv_sec     = 0;
  itv.it_value.tv_usec    = 0;
#endif
}

/*! This is the main routine of this class. It loops forever (till the thread
    is destroyed) and receives and parses the incoming messages.   */
void SenseHandler::handleMessagesFromServer( )
{
  char strBuf[MAX_MSG];
  int i=0;

  while( 1 )
  {
    strBuf[0]='\0';
    if( i != -1 )                                         // if no error
      i = connection->receiveMessage( strBuf, MAX_MSG );  // get message
    if( strBuf[0] != '\0' ) {                              // if not empty
      // offclient support: record all received messages
      offclient.SaveRecv(strBuf);

      analyzeMessage( strBuf );                           // parse message
    }
  }
}


/*! This method sets the time signal. This is the time that should be
    waited before the next action should be sent to the server. As
    soon as a sense message arrives this method is called. Using the
    information from the member variable 'iTriCounter' which denotes
    when the see message will arrive in this cycle (0=first half,
    1=2nd half, 2=no see, all for the default view frequency) the
    timer is set. The values that denote the fraction of the
    simulation step that is waited are all defined in PlayerSettings,
    such that they can be easily changed. */
void SenseHandler::setTimeSignal( )
{
  if( WM->getAgentViewFrequency() == 1.0 ) // VA_NORMAL AND VQ_HIGH (default)
  {
    if( iTriCounter % 3 == 0 )             // see will arrive first half cycle
    {
      iTimeSignal = (int)(iSimStep * PS->getFractionWaitSeeBegin() );
      iTriCounter = 0;
    }
    else if( iTriCounter % 3 == 1 )        // see will arrive 2nd half of cycle
    {
      iTimeSignal = (int)(iSimStep * PS->getFractionWaitSeeEnd() );
    }
    else                                   // no see will arrive
      iTimeSignal = (int)(iSimStep * PS->getFractionWaitNoSee( ) );
  }
  else if( WM->getAgentViewFrequency() == 2.0 ) // VA_WIDE AND VQ_HIGH
  {
    if( iTriCounter % 3 == 0 )              // see will arrive
    {
      iTimeSignal = (int)(iSimStep * PS->getFractionWaitSeeEnd() );
      iTriCounter = 0;
    }
    else                                   // no see will arrive
      iTimeSignal = (int)(iSimStep * PS->getFractionWaitNoSee() );
  }
  else                                     // VA_NARROW AND VQ_HIGH
    iTimeSignal = (int)(iSimStep * PS->getFractionWaitSeeEnd() );

  iTriCounter++;
#ifdef WIN32
  // kill the previous timer
  if (iTimer != 0) timeKillEvent( iTimer );
  // start a new one
  iTimer = timeSetEvent( iTimeSignal / 1000, timerRes,
                         sigalarmHandler, (DWORD)0, TIME_ONESHOT );
#else
  itv.it_value.tv_usec = iTimeSignal;
  setitimer( ITIMER_REAL, &itv, NULL );
#endif
}

/*! This method analyzes the type of the incoming message and calls the
    message that corresponds to this message.
    \param strMsg message that should be parsed.
    \return bool indicating whether the message was parsed or not */
bool SenseHandler::analyzeMessage( char *strMsg )
{
  Log.log( 1, strMsg );
  bool bReturn = false;
  
  switch( strMsg[1] )
  {
    case 'c':
        if( strMsg[2] == 'h' )
          return analyzeChangePlayerTypeMessage( strMsg );      // ( c hange_
        else
          ;                                                    // (clang
        break;
    case 'f':
          return analyzeFullStateMessage( strMsg );      // ( f ullstate_
    case 'o':                                                 // ( o k
        if( strlen(strMsg) > 14 && strMsg[4] == 'c' && strMsg[10] == 'b' )
          analyzeCheckBall( strMsg );                         // (ok check_ball
        return true;
    case 's':
    {
      switch( strMsg[3] )
      {
      case 'e':
        if( strMsg[5] == 'g')
          return analyzeSeeGlobalMessage  ( strMsg ); // (se e_g
        else if( WM->isFullStateOn( ) == false )
          return analyzeSeeMessage        ( strMsg ); // (se e
        break;                  
      case 'n': 
        bReturn = analyzeSenseMessage      ( strMsg ); // (se n se
        if( WM->isFullStateOn( ) == true  )
          WM->updateAfterSenseMessage( );
        return bReturn; 
        break;
      case 'r': return analyzeServerParamMessage( strMsg ); // (se r ver_param
      default : break;
      }
    }
        break;
    case 'i':     return analyzeInitMessage       ( strMsg ); // ( i nit
    case 'h':     return analyzeHearMessage       ( strMsg ); // ( h ear
    case 'p':     return ( strMsg[8] == 't')
                  ? analyzePlayerTypeMessage ( strMsg )  // (player_ t ype
                  : analyzePlayerParamMessage( strMsg ); // (player_ p aram
    case 'e':     printf("(%d,%d) %s\n", WM->getCurrentCycle(),
                           WM->getPlayerNumber(),strMsg);// ( error
        break;
    case 't':     Log.logWithTime( 2, " incoming think message" );
                  WM->processRecvThink( true );          // ( think
        break;
    default:      cerr << "(" << WM->getCurrentTime() << ", " <<
                        WM->getPlayerNumber()
                        << ") (SenseHandler::analyzeMessage) " <<
                     "ignored message: " << strMsg << "\n";
                  return false;
  }
  return false;
}


/*! This method analyzes a see Message. It gets the time from the message and
    tries to synchronize with the server. Then the message is stored in the
    world model, which processes it when it performs an update.
    \return bool indicating whether the message was parsed correctly. */
bool SenseHandler::analyzeSeeMessage( char *strMsg )
{

  g_viewAngle.setTimeAfterSeeArrived();
  g_viewAngle.setViewAngleType();


  strcpy( WM->strLastSeeMessage, strMsg );

  Log.logWithTime( 2, " %s",strMsg );

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

  Time    time = WM->getTimeLastRecvSenseMessage();
  int iTime = Parse::parseFirstInt( &strMsg );         // get the time
  if( time.getTime() != iTime )
  {
    cerr << "(SenseHandler:analyzeSeeMessage) see and different time as sense:"
         << time.getTime() << " vs. " << iTime << endl;
    return false;
  }