worldmodelupdate.cpp

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

  if( m_FieldLength == 0 )
    m_FieldLength = length;
  else
    m_FieldLength = (m_FieldLength + length) / 2;

  if( m_FieldWidth == 0 )
    m_FieldWidth = width;
  else
    m_FieldWidth = (m_FieldWidth + width) / 2;

  return 
    (isConfidenceGood( objTop ) && isConfidenceGood( objBottom ) && isConfidenceGood( objLeft ) && isConfidenceGood( objRight ) );
}

/*! This method updates positions, to a global one, after all see messages
    are parsed, and makes position global, because every see msg which is 
    parsed, stores the position as a relative one, so a globalization is
    definitely needed.
    \return bool indicating wheather update was successful */
bool WorldModel::updateGlobalFromRelative()
{
  if( m_FieldLength < MIN_FIELD_LENGTH || m_FieldWidth < MIN_FIELD_WIDTH )
    return false;

  double dist_min = 1000, dist_min2 = 1000;
  ObjectT objFlagMin = OBJECT_ILLEGAL, objFlagMin2 = OBJECT_ILLEGAL, objFlag;
  VecPosition p;

  for( objFlag = OBJECT_CORNER_L_T; objFlag <= OBJECT_CORNER_R_B; objFlag = (ObjectT)(objFlag + 1) )
  {
    if( !isConfidenceGood( objFlag ) )
      continue;

    p = m_Flags[SoccerTypes::getIndex( objFlag )].getPosition();
    if(p.getMagnitude() < dist_min)
    {
      dist_min2 = dist_min;
      objFlagMin2 = objFlagMin;

      dist_min = p.getMagnitude();
      objFlagMin = objFlag;
    }
    else if(p.getMagnitude() < dist_min2)
    {
      dist_min2 = p.getMagnitude();
      objFlagMin2 = objFlag;
    }
  }

  for( objFlag = OBJECT_GOAL_L_T; objFlag <= OBJECT_GOAL_R_B; objFlag = (ObjectT)(objFlag + 1) )
  {
    if( !isConfidenceGood( objFlag ) )
      continue;

    p = m_Goals[SoccerTypes::getIndex( objFlag )].getPosition();
    if(p.getMagnitude() < dist_min)
    {
      dist_min2 = dist_min;
      objFlagMin2 = objFlagMin;

      dist_min = p.getMagnitude();
      objFlagMin = objFlag;
    }
    else if(p.getMagnitude() < dist_min2)
    {
      dist_min2 = p.getMagnitude();
      objFlagMin2 = objFlag;
    }
  }

  if( objFlagMin == OBJECT_ILLEGAL || objFlagMin2 == OBJECT_ILLEGAL)
  {
    WMLogger << "ERROR: WorldModel::updateGlobalFromRelative: couldn't have enough data to update self position" << endl;
    return false;
  }

  Circle c1( getPosition( objFlagMin ) , dist_min  );
  Circle c2( getPosition( objFlagMin2 ), dist_min2 );
  
  VecPosition posSol1, posSol2;
  int iNrSol = c1.getCircleIntersectionPoints( c2, &posSol1, &posSol2 );
  if( iNrSol == 0 )
  {
    WMLogger << "ERROR: WorldModel::updateGlobalFromRelative: some mysterious thing happend!" << endl;
    return false;
  }

  if( iNrSol == 2 )
    if( getFieldRect().isInside( posSol2 ) )
      posSol1 = posSol2;

  if( !getFieldRect().isInside( posSol1 ) )
  {
    cerr << "WorldModel::updateGlobalFromRelative: calculated position lies out of field" << endl;
    return false;
  }

  posSol1.setZ( 2.73 );            /// Currently z-coordinate is not calculated`
  m_Agent.setPosition( posSol1 );
  m_Agent.setTimeLastSeen( getSimulatorTime() );

  AngDeg angRelFlag = SoccerTypes::isFlag( objFlagMin ) ? (m_Flags[SoccerTypes::getIndex( objFlagMin ) ].getPosition().getTheta()) :
                                                          (m_Goals[SoccerTypes::getIndex( objFlagMin ) ].getPosition().getTheta());
  AngDeg angAgent = (getPosition(objFlagMin) - m_Agent.getPosition()).getDirection() - angRelFlag;
  angAgent = VecPosition::normalizeAngle( angAgent );
  m_Agent.setDirection( angAgent );

  VecPosition posBall = m_Ball.getPosition();
  posBall.rotate( m_Agent.getDirection() );
  m_Ball.setPosition( m_Agent.getPosition() + posBall );

  cout << "pos" << m_Agent.getPosition() << endl;

  return true;
}

/*! This method updates the game state information from the recieved server message
    A GameState message looks like this: "(GS (unum 1) (team left) (t 0.00) (pm BeforeKickOff))"
    \param strMsg Server message to parse game state information from
    \return bool Indicates that Server message was parsed successfully */
bool WorldModel::updateGameState( char **strMsg )
{
  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(strncmp( *strMsg, "GS", 2 ) != 0) // GameState information
  {
    cerr << "WorldModel::updateGameState: This is not a GameState message" << endl;
    return false;
  }

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

    if( strncmp( "team", *strMsg, 4 ) == 0 )
    {
      *strMsg += 4;
      Parse::gotoFirstNonSpace( strMsg );
      m_Side = SoccerTypes::getSideFromStr( *strMsg );
    }
    else if( strncmp( "unum", *strMsg, 4 ) == 0 )
    {
      *strMsg += 4;
      Parse::gotoFirstNonSpace( strMsg );
      m_AgentNumber = Parse::parseFirstInt( strMsg );
    }
    else if( strncmp( "pm", *strMsg, 2 ) == 0 )
    {
      *strMsg += 3;
      Parse::gotoFirstNonSpace( strMsg );
      m_PlayMode = SoccerTypes::getPlayModeFormStr( *strMsg );
    }
    else if( strncmp( "t", *strMsg, 1 ) == 0 )
    {
      *strMsg += 2;
      m_GameTime = Parse::parseFirstDouble( strMsg );
    }

    Parse::gotoFirstOccurenceOf( ')', strMsg );
    ++*strMsg;
    Parse::gotoFirstNonSpace(strMsg);
  }
  while ( **strMsg && **strMsg != ')' );

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

  ++*strMsg;
  return true;
}

/**********************************************************************/
/**********************  Public Routines ******************************/
/**********************************************************************/

/*! This method updates internal information stored in world
    model  by the sent  message from  the server,  part like 
    current time, positions, and etc. 
    \param msg Message server to update from
    \return bool indicating weather update was successful
*/
bool WorldModel::updateFromServerMsg( char ** msg )
{
  bool bResult = true;
  for(int i = 0; i < 4; i++)
  {
    m_Flags[i].setPosition( VecPosition(UnknownDoubleValue, UnknownDoubleValue, UnknownDoubleValue) );
    m_Goals[i].setPosition( VecPosition(UnknownDoubleValue, UnknownDoubleValue, UnknownDoubleValue) );
  }

  while(**msg)
  {
    Parse::gotoFirstOccurenceOf( '(', msg );
    if( strlen(*msg) < 2)
      break;

    switch( *(*msg + 1) )
    {
    case 't':  // time
      bResult &= updateTime( msg );
      break;

    case 'H':  // HJ (Hinge Joint)
      bResult &= updateHingeJoint( msg );
      break;

    case 'U':  // HJ (Hinge Joint)
      bResult &= updateUniversalJoint( msg );
      break;

    case 'S':  // See
      bResult &= updatePositions( msg );
      break;

    case 'T':  // TCH (sensors)
      bResult &= updateSensors( msg );
      break;

    case 'G':  // GYR (gyroscope) - or - GS (GameState)
      if( *(*msg + 2 ) == 'Y' )
        bResult &= updateGyroscope( msg );
      else if( *(*msg + 2) == 'S' )
        bResult &= updateGameState( msg );
      break;

    default:   // Unknown parameter.
      bResult &= wasteMessage( msg );
      break;
    }

  }

  bResult &= updateAll();

  logWorldModel();
  return bResult;
}

/*! This method updates all the internal worldmodel information
    every cycle. First field width and height are calculated then
    agent position is calculated from given relative flag positions.
    \return bool indicating weather update was successful
*/
bool WorldModel::updateAll()
{
  bool bResult = true;

  bResult &= updateCoordinates();
  bResult &= updateGlobalFromRelative();
  m_CurrentCycle++;

  return bResult;
}

/*****************************************************************/
/************************  Other Routines  ***********************/
/*****************************************************************/

/*! This method wastes a part of message which is unfamiliar to
    agent so other parts could be read.
    \param msg Message to waste :)
    \return bool indicating weather wasting was successfull
*/
bool wasteMessage( char **msg )
{
  int iBracesCount = 0;

  Parse::gotoFirstOccurenceOf( '(', msg );
  if(strlen(*msg) > 1)
  {
    ++*msg;
    iBracesCount = 1;

    while( **msg && iBracesCount > 0 )
    {
      if(**msg == '(')
        iBracesCount++;
      else if(**msg == ')')
        iBracesCount--;

      ++*msg;
    }
  }

  return true;
}

/*********************************************************************************/
/**************************** Testing purposes ***********************************/
/*********************************************************************************/

// #define TEST_UPDATE  /*!< define this to test worldmodel updating */

#ifdef TEST_UPDATE
int main()
{
  char message[2048] = "", *str;
  string strMsg;

  strMsg += "(GS (unum 10) (team right) (t 0.00) (pm BeforeKickOff))";
  strMsg += "(time (now 2.24))";
  strMsg += "(GYR (n torso) (rt 0.00 -0.00 -0.00))";
  strMsg += "(See ";
  strMsg += " (F1L (pol 27.59 144.31 -6.29))";
  strMsg += " (F2L (pol 27.59 -144.32 -6.29))";
  strMsg += " (F1R (pol 32.15 29.99 -5.39))";
  strMsg += " (F2R (pol 32.15 -29.99 -5.39))";
  strMsg += " (G1L (pol 22.98 168.58 -8.50))";
  strMsg += " (G2L (pol 22.98 -168.58 -8.50))";
  strMsg += " (G1R (pol 28.29 9.22 -6.90))";
  strMsg += " (G2R (pol 28.29 -9.22 -6.90))";
  strMsg += " (B (pol 4.05 -0.00 -47.76))";
  strMsg += ")";
  strMsg += "(UJ (n laj1_2) (ax1 0.00) (ax2 0.00))";
  strMsg += "(UJ (n raj1_2) (ax1 -0.00) (ax2 0.00))";
  strMsg += "(HJ (n laj3) (ax -0.00))";
  strMsg += "(HJ (n raj3) (ax 0.00))";
  strMsg += "(HJ (n laj4) (ax 0.00))";
  strMsg += "(HJ (n raj4) (ax 0.00))";
  strMsg += "(HJ (n llj1) (ax 0.09))";
  strMsg += "(HJ (n rlj1) (ax 0.09))";
  strMsg += "(UJ (n llj2_3) (ax1 0.00) (ax2 3.64))";
  strMsg += "(UJ (n rlj2_3) (ax1 0.00) (ax2 -3.63))";
  strMsg += "(HJ (n llj4) (ax 0.03))";
  strMsg += "(HJ (n rlj4) (ax 0.03))";
  strMsg += "(TCH (n lf) (val 0))";
  strMsg += "(UJ (n llj5_6) (ax1 0.00) (ax2 0.12))";
  strMsg += "(TCH (n rf) (val 0))";
  strMsg += "(UJ (n rlj5_6) (ax1 0.00) (ax2 -0.12))";

  strcpy( message, strMsg.c_str() );
  str = message;

  WorldModel wm( 0 );
  if( wm.updateFromServerMsg( &str ) )
    cout << "WorldModel successfully updated" << endl;
  else
  {
    cerr << "WorldModel update failed" << endl;
    return 1;
  }

  cout << "simtime = " << wm.getSimulatorTime() << ", time = " << wm.getTime() << ", step = " << wm.getSimulatorStep() << endl;
  wm.getBallPosition().show();

  Joint j;
  for(JointT t = (JointT)0; t < MAX_JOINTS; t = (JointT)((int)t + 1))
  {
    j = wm.getJoint(t);
    cout << "(Joint (name " << SoccerTypes::getJointStr(j.getID()) << ") ";
    cout << "(axis1 " << j.getAxis1() << ") (rate1 " << j.getRate1() << ") ";
    cout << "(axis2 " << j.getAxis2() << ") (rate2 " << j.getRate2() << ") )\n";
  }

  cout << "Agent gyroscope:" << wm.getGyroscope() << endl;
  cout << "Sensors: Left = " << wm.getSensor( SENSE_FOOT_LEFT ) << ", right = " << wm.getSensor( SENSE_FOOT_RIGHT ) << endl;
  cout << "Agent Position: " << wm.getAgentPosition() << endl;
  cout << "Ball Position: " << wm.getBallPosition() << endl;
  cout << "Current PlayMode: " << SoccerTypes::getPlayModeStr( wm.getPlayMode() ) << endl;
  cout << "Agent number: " << wm.getAgentNumber() << endl; 
  cout << "Team Side: " << SoccerTypes::getSideStr( wm.getSide() ) << endl;
  cout << "Field Length = " << wm.getFieldLength() << " | Field Width = " << wm.getFieldWidth() << endl;

  return 0;
}
#endif // TEST_UPDATE