worldmodelupdate.cpp

robocup 3d, a 3d base team similar to UvA 2d

/*
*********************************************************************************
*                            In the name of Almighty                            *
*                                                                               *
*        WorldModelUpdate.cpp : Robocup 3D Soccer Simulation Team Zigorat       *
*                     (This team was previously named Gcyrus)                   *
*                                                                               *
*  Date: 03/20/2007                                                             *
*  Author: Mahdi Hamdarsi                                                       *
*  Comments: This is the agents memory updater of outside world                 *
*                                                                               *
*********************************************************************************
*/

/*! \file WorldModelUpdate.cpp
<pre>
<b>File:</b>          WorldModelUpdate.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: Zigorat
<b>Authors:</b>       Mahdi Hamdarsi
<b>Created:</b>       03/20/2007
<b>Last Revision:</b> $ID$
<b>Contents:</b>      This is the agents memory updater of outside world
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
03/20/2007       Mahdi           Initial version created
</pre>
*/

#include "WorldModel.h"
#include "Parse.h"
#include "string.h"
#include "Logger.h"
#include "SoccerTypes.h"

#include <iostream>
#include <cstdlib>

using namespace std;


bool wasteMessage( char **msg );

/*****************************************************************************/
/********************** CLASS WORLDMODEL: UPDATE ROUTINES ********************/
/*****************************************************************************/


/*! This method updates time and simulator step from server
    message every cycle.
    \param strMsg time message to update from
    \return bool indicating weather update was successful
*/
bool WorldModel::updateTime( char **strMsg ) // (time (now 4.5) (step 0.02))
{
  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 't') // time
  {
    do
    {
      Parse::gotoFirstOccurenceOf( '(', strMsg );
      ++*strMsg;

      if(strlen(*strMsg) > 1)
        switch( **strMsg )
        {
        case 'n': // now
          if( strlen(*strMsg) > 4)
          {
            *strMsg += 4;
            m_SimulatorTime = Parse::parseFirstDouble( strMsg );
            break;
          }
          else
          {
            cerr << "WorldModel::updateTime: Time message truncated by predicate 'now'" << endl;
            return false;
          }

        case 's': // step
          if( strlen(*strMsg) > 5)
          {
            *strMsg += 5;
            m_SimulatorStep = Parse::parseFirstDouble( strMsg );
            break;
          }
          else
          {
            cerr << "WorldModel::updateTime: Time message truncated by predicate 'step'" << endl;
            return false;
          }

        default:
          {
            cerr << "WorldModel::updateTime: Unknown predicate found" << endl;
            return false;
          }
        }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;
      Parse::gotoFirstNonSpace(strMsg);

    }
    while ( **strMsg && **strMsg != ')' );
  }
  else
  {
    cerr << "WorldModel::updateTime: This is not a time message" << endl;
    return false;
  }

  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateTime: Server message truncated at the end of time message" << endl;
    return false;
  }

  ++*strMsg;
  return true;
}

/*! This method updates the sensros values from the supplied server message
    A sensor message looks like this: "(TCH (n lf) (val 1))"
    \param strMsg Server message to parse for update
    \return bool Indicating update was successfull */
bool WorldModel::updateSensors( char **strMsg )
{
  char token_name[128] = "";
  SensorT sense = SENSE_ILLEGAL;
  int i;
  double m_Value = 0;
  Parse::gotoFirstOccurenceOf( '(', strMsg );

  if( strncmp(*strMsg, "(TCH", 4) == 0 )
  {
    *strMsg += 4;

    do
    {
      Parse::gotoFirstNonSpace( strMsg );

      switch( *(*strMsg + 1) )
      {
      case 'n':  // (name)
        *strMsg += 2;
        Parse::gotoFirstNonSpace( strMsg );
        i = 0;
        while( **strMsg && **strMsg != ' ' && (isalnum(**strMsg) || **strMsg == '_') )
        {
          token_name[i++] = **strMsg;
          ++*strMsg;
        }

        sense = SoccerTypes::getSensorFromString( token_name );
        break;

      case 'v': // val
        *strMsg += 4;
        m_Value = Parse::parseFirstDouble( strMsg );
        break;

      default:
        break;
      }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;

    }
    while ( **strMsg && **strMsg != ')' );

  }
  else
  {
    wasteMessage( strMsg );
    cerr << "WorldModel::updateSensors: This is not a sensor message" << endl;
    return false;
  }

  Parse::gotoFirstNonSpace( strMsg );
  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateSensors: Server message truncated at the end of sensor message" << endl;
    return false;
  }

  ++*strMsg;
  m_Sensors[ (int)sense ].setName( SoccerTypes::getSensorString( sense ) );
  m_Sensors[ (int)sense ].setValue( m_Value );

  return true;
}

/*! This method updates the gyroscope parameters from the supplied server message
    A gyroscope message looks like this: "(GYR (n torso) (rt -0.00 -0.00 0.06))"
    \param strMsg string message recieved from the simulator
    \return bool Indicating wheather update was successfull */
bool WorldModel::updateGyroscope( char **strMsg )
{
  char token_name[128] = "";
  int i;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  if( strncmp(*strMsg, "(GYR", 4) == 0 )
  {
    *strMsg += 4;

    Parse::gotoFirstNonSpace( strMsg );

    if( *(*strMsg + 1) == 'n' )
    {
      *strMsg += 3;
      Parse::gotoFirstNonSpace( strMsg );
      i = 0;
      while( **strMsg && **strMsg != ' ' && (isalnum(**strMsg) || **strMsg == '_') )
      {
        token_name[i++] = **strMsg;
        ++*strMsg;
      }
      if( strcmp( token_name, "torso" ) != 0 )
      {
        cerr << "WorldModel::updateGyroscope: The supplied name is not torsogyro" << endl;
        return false;
      }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;
    }
    else
    {
      cerr << "WorldModel::updateGyroscope: Unknown predicate" << endl;
      return false;
    }

    Parse::gotoFirstNonSpace( strMsg );
    if( **strMsg != '(' )
    {
      cerr << "WorldModel::updateGyroscope: Gyroscope does not contain any data" << endl;
      return false;
    }

    ++*strMsg;
    Parse::gotoFirstNonSpace( strMsg );
    if( strncmp(*strMsg, "rt", 2) != 0 )
    {
      cerr << "WorldModel::updateGyroscope: No rate information found" << endl;
      return false;
    }
    *strMsg += 3;

    if( !**strMsg || **strMsg == ')' )
    {
      cerr << "WorldModel::updateGyroscope: Parse failed for x predicate" << endl;
      return false;
    }
    m_Gyroscope.setX( Rad2Deg( Parse::parseFirstDouble( strMsg ) ) );
    Parse::gotoFirstNonSpace( strMsg );
    if( !**strMsg || **strMsg == ')' )
    {
      cerr << "WorldModel::updateGyroscope: Parse failed for y predicate" << endl;
      return false;
    }
    m_Gyroscope.setY( Rad2Deg( Parse::parseFirstDouble( strMsg ) ) );

    Parse::gotoFirstNonSpace( strMsg );
    if( !**strMsg || **strMsg == ')' )
    {
      cerr << "WorldModel::updateGyroscope: Parse failed for z predicate" << endl;
      return false;
    }
    m_Gyroscope.setZ( Rad2Deg( Parse::parseFirstDouble( strMsg ) ) );

    Parse::gotoFirstNonSpace( strMsg );
    if( **strMsg != ')' )
    {
      cerr << "WorldModel::updateGyroscope: Unterminated gyroscope date" << endl;
      return false;
    }
    ++*strMsg;
  }
  else
  {
    cerr << "WorldModel::updateGyroscope: This is not a gyroscope sensor message" << endl;
    return false;
  }

  Parse::gotoFirstNonSpace( strMsg );
  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateGyroscope: Server message terminated at the end of gyro data" << endl;
    return false;
  }
  ++*strMsg;

  return true;
}

/*! This method updates a Universal joint (A joint which can
    turn in both directions) based on the  sent message from
    server, a universal message looks like this:
    "(UJ (n llj2_3) (ax1 0.00) (ax2 0.00))"
    \param strMsg Server message to update from
    \return bool indicating weather update was successful */
bool WorldModel::updateUniversalJoint( char **strMsg )
{
  char token_name[128] = "";
  int i = 0;
  Joint jnt;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 'U') // Universal Joint
  {
    do
    {
      Parse::gotoFirstOccurenceOf( '(', strMsg );
      ++*strMsg;

      if(strlen(*strMsg) > 1)
        switch( **strMsg )
        {
        case 'n': // name
          if(strlen(*strMsg) > 2) // name
          {
            *strMsg += 2;
            while( **strMsg && **strMsg != ' ' && (isalnum(**strMsg) || **strMsg == '_') )
            {
              token_name[i++] = **strMsg;
              ++*strMsg;
            }

            jnt.setID( SoccerTypes::getJointFromStr( token_name ) );
            if(jnt.getID() == JID_ILLEGAL)
            {
              cerr << "WorldModel::updateUniversalJoint: Joint is not a registered joint: '" << token_name << "'" << endl;
              return false;
            }
          }
          else
          {
            cerr << "WorldModel::updateUniversalJoint: Server message truncated near name predicate" << endl;
            return false;
          }
          break;

        case 'a': // axis
          if(strlen(*strMsg) > 3)
          {
            *strMsg += 2;
            if( **strMsg == '1' )
            {
              *strMsg += 1;
              jnt.setAxis1( Parse::parseFirstDouble( strMsg ) );
            }
            else
            {
              *strMsg += 1;
              jnt.setAxis2( Parse::parseFirstDouble( strMsg ) );
            }
          }
          else
          {
            cerr << "WorldModel::updateUniversalJoint: Server message truncated near axis predicate" << endl;
            return false;
          }
          break;

        case 'r' : //rate
          if(strlen(*strMsg) > 6)
          {
            *strMsg += 4;
            if( **strMsg == '1' )
            {