wmbuilder.cpp

RoboCup 3D 仿真组清华大学2005的源代码

/***************************************************************************
 *   Copyright (C) 2005 by Rujia Liu                                       *
 *   [e-mail]                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <zeitgeist/leaf.h>
#include <oxygen/oxygen.h>
#include <boost/shared_ptr.hpp>
#include "wmbuilder.h"
#include "worldmodel.h"

using namespace std;
using namespace boost;
using namespace oxygen;
using namespace salt;
using namespace zeitgeist;

extern Zeitgeist zg;
extern WorldModel wm;

WMBuilder::WMBuilder()
{
}

WMBuilder::~WMBuilder()
{
}

bool WMBuilder::Init()
{
    // initialize sexpparser (s-expression parser)
    mParser = shared_dynamic_cast<BaseParser>(zg.GetCore()->New("SexpParser"));
    if(!mParser.get()) return false; // no sexpparser
  
    // Setup GameStateMap
    mPlayModeMap.clear();
  
    // 18 playmodes
    mPlayModeMap[STR_PM_BeforeKickOff]     = PM_BeforeKickOff;
    mPlayModeMap[STR_PM_KickOff_Left]      = PM_KickOff_Left;
    mPlayModeMap[STR_PM_KickOff_Right]     = PM_KickOff_Right;
    mPlayModeMap[STR_PM_PlayOn]            = PM_PlayOn;
    mPlayModeMap[STR_PM_KickIn_Left]       = PM_KickIn_Left;
    mPlayModeMap[STR_PM_KickIn_Right]      = PM_KickIn_Right;
    mPlayModeMap[STR_PM_CORNER_KICK_LEFT]  = PM_CORNER_KICK_LEFT ;
    mPlayModeMap[STR_PM_CORNER_KICK_RIGHT] = PM_CORNER_KICK_RIGHT;   
    mPlayModeMap[STR_PM_GOAL_KICK_LEFT]    = PM_GOAL_KICK_LEFT;
    mPlayModeMap[STR_PM_GOAL_KICK_RIGHT]   = PM_GOAL_KICK_RIGHT;    
    mPlayModeMap[STR_PM_OFFSIDE_LEFT]      = PM_OFFSIDE_LEFT;
    mPlayModeMap[STR_PM_OFFSIDE_RIGHT]     = PM_OFFSIDE_RIGHT;  
    mPlayModeMap[STR_PM_GameOver]          = PM_GameOver;
    mPlayModeMap[STR_PM_Goal_Left]         = PM_Goal_Left;
    mPlayModeMap[STR_PM_Goal_Right]        = PM_Goal_Right;
    mPlayModeMap[STR_PM_FREE_KICK_LEFT]    = PM_FREE_KICK_LEFT;
    mPlayModeMap[STR_PM_FREE_KICK_RIGHT]   = PM_FREE_KICK_RIGHT;
    mPlayModeMap[STR_PM_Unknown]           = PM_NONE;
    
    // SetupVisionObjectMap
    mVisionObjectMap.clear();
    mVisionObjectMap["Flag1_l"] = VO_FLAG1L;
    mVisionObjectMap["Flag1_r"] = VO_FLAG1R;
    mVisionObjectMap["Flag2_l"] = VO_FLAG2L;
    mVisionObjectMap["Flag2_r"] = VO_FLAG2R;
    mVisionObjectMap["Goal1_l"] = VO_GOAL1L;
    mVisionObjectMap["Goal1_r"] = VO_GOAL1R;
    mVisionObjectMap["Goal2_l"] = VO_GOAL2L;
    mVisionObjectMap["Goal2_r"] = VO_GOAL2R;
    mVisionObjectMap["Ball"]    = VO_BALL;
    return true;
}

// fetch the sense of an object: look up in the sense table
VisionSense WMBuilder::GetVisionSense(VisionObject obj)
{
    TVisionMap::iterator iter = mVisionMap.find(obj);
    if (iter == mVisionMap.end()) return VisionSense(); // error: unknown visionobject
      else return (*iter).second;
}
  
/*
  use the closest object to do self localization
  The distance error at 100m is approx. 0.1m, 0.01m at 10m
  So when near the goal, this position is ``proved'' to be accurate using ``nearest mark'' heuristic
*/
salt::Vector3f WMBuilder::LocalizeWithOneFlag(VisionObject vo)
{
    // get absolute position of vo, vop
    Vector3f vop;
    switch(vo){
        case VO_FLAG1L:
          vop = Vector3f(-wm.mFieldLength/2.0, -wm.mFieldWidth/2.0, 0); break;
        case VO_FLAG2L:
          vop = Vector3f(-wm.mFieldLength/2.0, +wm.mFieldWidth/2.0, 0); break;
        case VO_FLAG1R:
          vop = Vector3f(+wm.mFieldLength/2.0, -wm.mFieldWidth/2.0, 0); break;
        case VO_FLAG2R:
          vop = Vector3f(+wm.mFieldLength/2.0, +wm.mFieldWidth/2.0, 0); break;
        case VO_GOAL1L:
          vop = Vector3f(-wm.mFieldLength/2.0, -wm.mGoalWidth/2.0, 0); break;
        case VO_GOAL2L:
          vop = Vector3f(-wm.mFieldLength/2.0, +wm.mGoalWidth/2.0, 0); break;
        case VO_GOAL1R:
          vop = Vector3f(+wm.mFieldLength/2.0, -wm.mGoalWidth/2.0, 0); break;
        case VO_GOAL2R:
          vop = Vector3f(+wm.mFieldLength/2.0, +wm.mGoalWidth/2.0, 0); break;
      }      
    // translate to local view. need to be tested.
    if(wm.GetTeamIndex() == TI_RIGHT) vop = -vop;
    
    // get relative posiion rep, then calculate selflocation
    Vector3f rep = GetRelativePosition(GetVisionSense(vo));
    return vop - rep;      
}

// distance = 100: 0.0965
// distance = 10: 0.00965
float WMBuilder::GetVariance(VisionObject vo)
{
    float var;
    
    float dist = GetVisionSense(vo).distance;
    float phi = GetVisionSense(vo).phi;
    float theta = GetVisionSense(vo).theta;
    
    var = dist * 0.000965;    
    var = dist * 0.1480;

//#define GEOM_ESTIMATE    
#ifdef GEOM_ESTIMATE
    
    float var_dist =  dist * 0.000965;
    float var_theta = dist * 0.001225;
    float var_phi = dist * 0.001480;
    
    float AC = var_dist;
    float AB = dist * gSin(gDegToRad(var_phi / 2.0)) * 2.0;
    float BC = sqrt(sqr(AC) + sqr(AB));
    
    float OC = dist;
    float OB = dist + var_dist;
    float CD = OC * gSin(gDegToRad(phi));
    float OD = OC * gCos(gDegToRad(phi));
    float OE = OB * gCos(gDegToRad(phi) - var_phi);
    float DE = OE - OD;
    
    float EF = OE * gSin(gDegToRad(var_theta / 2.0)) * 2.0;
    float DF = sqrt(sqr(DE) + sqr(EF));
    
    float CF = sqrt(sqr(CD) + sqr(DF));
    
    var = CF;
#endif    
    return var;
}


salt::Vector3f WMBuilder::Localize()
{   
    VisionObject vo[8] = {VO_FLAG1L, VO_FLAG1R, VO_FLAG2L, VO_FLAG2R, 
                          VO_GOAL1L, VO_GOAL1R, VO_GOAL2L, VO_GOAL2R};
    Vector3f pos, p;
    float var, totalvar, k;
    
    // find the nearest
    pos = LocalizeWithOneFlag(VO_FLAG1L);

#define KALMAN_FILTER
    
#ifdef KALMAN_FILTER        
    totalvar = var = GetVariance(VO_FLAG1L);
    
    // kalman filter, mean error = 0.045
    for(int i = 1; i < 8; i++)
    {
         var = GetVariance(vo[i]);
         p = LocalizeWithOneFlag(vo[i]);

         k = totalvar / ( var + totalvar );
         pos[0] += k*( p[0] - pos[0] );
         pos[1] += k*( p[1] - pos[1] );
         pos[2] += k*( p[2] - pos[2] );
         totalvar -= k*totalvar;
    }
#endif    
    return pos;
    
}

void WMBuilder::ParseGameState(const Predicate& predicate)
{
    // parse team name, then convert to string
    string team;
    if (predicate.GetValue(predicate.begin(), "team", team)){
        if (team == "left") wm.mTeamIndex = TI_LEFT;
        else if (team == "right") wm.mTeamIndex = TI_RIGHT;
        else{
           wm.mTeamIndex = TI_NONE;
           cout << "(WorldModel) received TeamIndex TI_NONE\n"; // error
        }
    }
  
    // parse play mode, then convert to string
    string mode;
    if (predicate.GetValue(predicate.begin(), "playmode", mode)){
        TPlayModeMap::iterator iter = mPlayModeMap.find(mode);
        if (iter != mPlayModeMap.end())
            wm.mPlayMode = (TPlayMode)(*iter).second;
        else
          cout << "(WorldModel) could not parse playmode " << mode << "\n"; // error, maybe goal_kick_left
    }
  
    // get values
    predicate.GetValue(predicate.begin(),"FieldLength",   wm.mFieldLength);
    predicate.GetValue(predicate.begin(),"FieldWidth",    wm.mFieldWidth);
    predicate.GetValue(predicate.begin(),"FieldHeight",   wm.mFieldHeight);
    predicate.GetValue(predicate.begin(),"GoalWidth",     wm.mGoalWidth);
    predicate.GetValue(predicate.begin(),"GoalDepth",     wm.mGoalDepth);
    predicate.GetValue(predicate.begin(),"GoalHeight",    wm.mGoalHeight);
    predicate.GetValue(predicate.begin(),"BorderSize",    wm.mBorderSize);
    predicate.GetValue(predicate.begin(),"time",          wm.mTime);
    predicate.GetValue(predicate.begin(),"unum",          wm.mTeamUnum);
    predicate.GetValue(predicate.begin(),"AgentRadius",   wm.physics.mAgentRadius);
    predicate.GetValue(predicate.begin(),"AgentMass",     wm.physics.mAgentMass);
    predicate.GetValue(predicate.begin(),"AgentMaxSpeed", wm.physics.mAgentMaxSpeed);
    predicate.GetValue(predicate.begin(),"BallRadius",    wm.physics.mBallRadius);
    predicate.GetValue(predicate.begin(),"BallMass",      wm.physics.mBallMass);
}

void WMBuilder::ParseAgentState(const Predicate& predicate)
{
    predicate.GetValue(predicate.begin(),"battery", wm.mBattery);     // in percentage
    predicate.GetValue(predicate.begin(),"temp",    wm.mTemperature); // in degree
}

void WMBuilder::ParseVision(const Predicate& predicate)
{
    for(Predicate::Iterator iter(predicate); iter != iter.end(); ++iter)
    {
        // extract the element as a parameter list
        Predicate::Iterator paramIter = iter;
        if (! predicate.DescentList(paramIter)) continue;

        // read the object name: `Flag', `Goal' or `Player'
        string name;
        if (! predicate.GetValue(paramIter, name)) continue;
    
        string strId;
        VisionObject vo;
        int teamId, id;
    
        if(name == "mypos")
        {
            ++paramIter;
            predicate.AdvanceValue(paramIter, wm.mMyDebugPos[0]);
            predicate.AdvanceValue(paramIter, wm.mMyDebugPos[1]);
            predicate.AdvanceValue(paramIter, wm.mMyDebugPos[2]);
            printf("mypos is %.3f %.3f %.3f", wm.mMyDebugPos[0], wm.mMyDebugPos[1], wm.mMyDebugPos[2]);
            continue;
        }
    
        if(name == "Player"){
            // try read the 'team' and 'id' section
            string teamName;
            predicate.GetValue(paramIter, "team", teamName);
            if(teamName == wm.mTeamName) teamId = 0; else teamId = 1;
            predicate.GetValue(paramIter, "id", id);
        }else{
            // try read the 'id' section
            if (predicate.GetValue(paramIter, "id", strId)) name += strId;
            // try to lookup the VisionObject
            TVisionObjectMap::iterator iter = mVisionObjectMap.find(name);
            if (iter == mVisionObjectMap.end()) continue;
            vo = (*iter).second;
        }

        // find  to the 'pol' entry in the object's section
        Predicate::Iterator polIter = paramIter;
       
        if (! predicate.FindParameter(polIter, "pol")) continue;

        // read the position vector
        VisionSense sense;
        if ( (! predicate.AdvanceValue(polIter, sense.distance)) || 
           (! predicate.AdvanceValue(polIter, sense.theta)) || 
           (! predicate.AdvanceValue(polIter, sense.phi)) ) continue;

        // update the player position or vision map
        if(name == "Player")
            mPlayerPositionSense[teamId][id] = sense;
        else
            mVisionMap[vo] = sense;
    }
}

void WMBuilder::Parse(const string& message)
{
    log.Log("%s\n", message.c_str());

    int i, j;  
    shared_ptr<PredicateList> predList;
  
    // Notification. no vision infomation is received, so only parse time[0]
    int tmp;
    if(message[0] == 'T')
    {
        tmp = 0;
        for(i = 1; message[i] >= '0' && message[i] <= '9'; i++) // bug fix.
            tmp = tmp * 10 + message[i] - '0';
        wm.physics.time[0] = tmp;
      
        int action_latency = 0;
        wm.physics.time[0] += action_latency; 
        // action latency is 10. see lib/oxygen/gamecontrolserver, GetActionLatency
    }
  
    // `I' is old information(not starting a thinking cycle), `S' is regular sense
    if(message[0] == 'I' || message[0] == 'S')
    {
        wm.physics.TimeShift();
      
        // parse time[1]      
        tmp = 0;
        for(i = 1; message[i] != ' '; i++)
            tmp = tmp * 10 + message[i] - '0';
        wm.physics.time[1] = tmp;

        // parse time[0]
        tmp = 0;
        while(!(message[i] >= '0' && message[i] <= '9')) i++;
        for(; message[i] >= '0' && message[i] <= '9'; i++)
            tmp = tmp * 10 + message[i] - '0';
        wm.physics.time[0] = tmp;
        //wm.physics.time[0] += 10; 
        // action latency is 10. see lib/oxygen/gamecontrolserver, GetActionLatency
            
        // parse
        predList = mParser->Parse(message);
        if (!predList.get()) return;

        // parse information
        PredicateList& list = *predList;   
        for(PredicateList::TList::const_iterator iter = list.begin(); iter != list.end(); ++iter)
        {
            const Predicate& predicate = (*iter);
      
            if (predicate.name == "GameState") 
                ParseGameState(predicate);
            else if (predicate.name == "AgentState") 
                ParseAgentState(predicate);
            else if (predicate.name == "Vision") 
                ParseVision(predicate); 
            else
                cout << "(WorldModel) skipping unknown predicate " << predicate.name << "\n";		  
        }
  
        // raw data calculatable from vision sense
        wm.physics.mMyPos[1] = Localize();

        wm.physics.mBallRelPos[1] = GetRelativePosition(GetVisionSense(VO_BALL));
        for(i = 0; i < 2; i++)
            for(j = 1; j <= 11; j++)
                wm.physics.mPlayerRelPosition[i][j][1] = GetRelativePosition(mPlayerPositionSense[i][j]);
        wm.physics.mPlayerRelPosition[0][wm.GetTeamUnum()][1] = Vector3f(0.0, 0.0, 0.0);	  
#define LOG_VISION      
#ifdef LOG_VISION
        Vector3f v1 = wm.GetMyPosition(1);
        Vector3f v2 = wm.GetBallPosition(1);
        wm.logvision.Log("%.2f %.3f %.3f %.3f %.3f\n", wm.GetTime(1), v1[0], v1[1], v2[0], v2[1]);
#endif      
    }

    // Do update. this is important especially message[0] = 'T' 
    // (no actual sense is received, only a time notification)
    wm.physics.InitPrediction();
    wm.physics.GetCurrentPosition();
    wm.physics.GetCurrentSpeed();    
}