agent.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 <sstream>
#include <netinet/in.h>
#include "agent.h"
#include "worldmodel.h"
#include "wmbuilder.h"
#include "skill.h"
#include "decision.h"
#include "formation.h"

extern WorldModel wm;
extern WMBuilder wmb;
extern Skill skill;
extern Decision decision;
extern Formation formation;

extern AgentParam param;

using namespace std;
using namespace salt;

Agent::Agent()
{
    mSensationCount = 0;
    mLastDriveVec = Vector3f(0.0, 0.0, 0.0);
    mLastDriveTime = 0;
    mDriveVec[0] = Vector3f(0.0, 0.0, 0.0);  
  
    mReadFd = 3;
    mWriteFd = 4;
  
    mSensationCount = 0;
  
    PlayerInitialized = false; // set true when teamunum is received.
}

Agent::~Agent()
{
}

void Agent::InitAgent()
{
    stringstream ss;
    ss << "A(init (unum " << mTeamUnum << ") (teamname " << mTeamName << "))";
    PutOutput(ss.str());
}

void Agent::CreateAgent()
{
    PutOutput("A(create)");
}

void Agent::Beam(const salt::Vector3f& pos, bool send)
{
    if(send)
    {
        stringstream ss;
        ss << "A(beam " << pos[0] << " " << pos[1] << " " << pos[2] << ")";
        log.Log("%s\n", ss.str().c_str());
        PutOutput(ss.str());
    }else{
        execBeam = true;
        BeamParam = pos;
    }
}

void Agent::Kick(const float angle, const float power, bool send)
{
    if(send)
    {
        stringstream ss;
        ss << "A(kick " << angle << " " << power << ")";
        log.Log("%s\n", ss.str().c_str());
        PutOutput(ss.str());
    }else{
        execKick = true;
        kick_angle = angle;
        kick_power = power;
    }
}

void Agent::Drive(const salt::Vector3f& vec, bool send)
{
    if(send)
    {
        if(vec == mLastDriveVec) {} // nothing changed
        else if(isnan(vec.Length())) {} // ignore nan
        else
        {
            int time = wm.GetSimTime(); // TODO: consider action latency
            for(int i = mLastDriveTime + 1; i < time; i++)
                mDriveVec[i] = mLastDriveVec;
            mLastDriveVec = vec;
            mDriveVec[time] = vec;    
            mLastDriveTime = time;
    
            // execute and log
            stringstream ss;
            ss << "A(drive " << vec[0] << " " << vec[1] << " " << vec[2] << ")";
            log.Log("%s\n", ss.str().c_str());
            PutOutput(ss.str());    
        }
    }
    else
    {
        execDrive = true;
        DriveParam = vec;
    }
}

bool Agent::SelectInput()
{
    fd_set readfds;
    FD_ZERO(&readfds);
    FD_SET(mReadFd, &readfds);
    return select(mReadFd+1, &readfds, 0, 0, 0) > 0;
}

bool Agent::GetInput()
{
    if(param.offclient_do)
    {
        // reading from offclient_fp
        if(fgets(mBuffer + sizeof(long), buffer_size, offclient_fp) == NULL) return false;
        fprintf(stderr, "Message received: %s", GetMsg());
    }
    else
    {
        // read from commserver
        memset(mBuffer, 0, sizeof(mBuffer));
        if(!SelectInput()) return false;
  
        unsigned int bytesRead = read(mReadFd, mBuffer, sizeof(mBuffer)); // read count
        if(bytesRead < sizeof(unsigned int)) return false;
  
        unsigned int msgLen = ntohl(*(unsigned int*)mBuffer);
        unsigned int msgRead = bytesRead - sizeof(unsigned int);
        char* offset = mBuffer + bytesRead;
  
        while(msgRead < msgLen){
            if(!SelectInput()) return false;
            msgRead += read(mReadFd, offset, sizeof(mBuffer) - msgRead);
            offset += msgRead;
        }
        (*offset) = 0;

        // write to log: mBuffer[1]
        if(param.offclient_rec)
            printf("%s\n", mBuffer + sizeof(long));
    }
    return true;
}

void Agent::PutOutput(const char* out)
{
    if(!param.offclient_do)
    {
        strcpy(mBuffer + sizeof(long), out);
        unsigned int len = strlen(out);
        unsigned int netlen = htonl(len);
        memcpy(mBuffer, &netlen, sizeof(netlen));
        write(mWriteFd, mBuffer, len + sizeof(netlen));
    }
    else
        fprintf(stderr, "Message sent: %s\n", out);
}

void Agent::PutOutput(const std::string& str)
{ 
    PutOutput(str.c_str());
}

//TODO: reply of C may be after a sensation. Then this function will produce error
// so it is not used. just an illustration of how to use RTT
int Agent::GetThinkingTime()
{
    string RequestThinkingTimeMessage = "C";
    PutOutput(RequestThinkingTimeMessage);
    if(!GetInput())
    {
        fprintf(stderr, "Reading from CommServer when waiting for response for thinking time failed.\n");
        return -1;
    }
    else
    {
        // GetMsg()'s format: C<time>, where <time> is the current thinking time (in SimTime)
        return atoi(GetMsg() + 1);
    }    
}

void Agent::RequestTimeNotify(int sim_time)
{
    char buf[1024];
    sprintf(buf, "R%d",  sim_time);
    PutOutput(buf);
}

void Agent::OpenLog()
{    
    if(param.action_log)
        log.Open("action", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.wmb_log)
        wmb.log.Open("wmb", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.wm_log)
        wm.log.Open("wmm", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.wm_logvision)
        wm.logvision.Open("wmv", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.wm_logcur)
        wm.logcur.Open("wmc", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.decision_log) 
        decision.log.Open("decision", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.skill_log) 
        skill.log.Open("skill", wm.GetTeamName().c_str(), wm.GetTeamUnum());
    if(param.formation_log) 
        formation.log.Open("formation", wm.GetTeamName().c_str(), wm.GetTeamUnum());
}

void Agent::LogTime()
{
    log.Log("Time = %.2f\n", wm.GetTime());
    decision.log.Log("Time = %.2f\n", wm.GetTime());
    skill.log.Log("Time = %.2f\n", wm.GetTime());
    formation.log.Log("Time = %.2f\n", wm.GetTime());
    wmb.log.Log("Time = %.2f\n", wm.GetTime());	    
    wm.log.Log("Time = %.2f\n", wm.GetTime());
}

bool Agent::IsTypeKnown()
{
    return (wm.GetTeamIndex() != TI_NONE && wm.GetTeamUnum() != 0);
}

int Agent::InitPlayer()
{
    PlayerInitialized = true;
    
    // log system
    fprintf(stderr, "Opening logs...\n");
    OpenLog();

    // formation
    fprintf(stderr, "Initializing formation.\n");
    if(formation.Init())
        fprintf(stderr, "Formation initialized succesfully.\n");
    else
    {
        fprintf(stderr, "Fatal Error: cannot read formation file *.fmt and *.sbp, aborting...\n"); 
        return 0; 
    }
    return 1;
}

int Agent::Behave()
{
    string DoneThinkingMessage = "D";
    string DoneInitMessage = "I";  
  
    // setup internal datastructures
    fprintf(stderr, "Initializing WorldModel.\n");  
    if(wmb.Init())
        fprintf(stderr, "WorldModel initialized successfully.\n");
    else
        { fprintf(stderr, "Fatal: Cannot initialize WorldModel, aborting...\n"); return 0; }
  
  while(true)
  {
      if(!GetInput()) break;      
      wmb.Parse(GetMsg());
      
      if(IsTypeKnown() && !PlayerInitialized)
          if(!InitPlayer()) return 0;

      // offclient start-point
      float cur_time = wm.GetTime();
      if(param.offclient_do && cur_time > param.offclient_time - 0.005)
          fprintf(stderr, "(Offclient) Time to debug.\n"); // set a breakpoint here to debug
     
      char s_type = GetMsg()[0];
      if(s_type == 'D')
          PutOutput(DoneInitMessage);
      else if(s_type == 'S' && wm.GetPlayMode() != PM_BeforeKickOff) // time notify request
      {
          for(int dt = CycleStep; dt < 20; dt += CycleStep)
              RequestTimeNotify(wm.GetSimTime(0) + dt);    
      }
      
      if(s_type == 'S' || s_type == 'T') 
      {
          if(mSensationCount == 0) CreateAgent();
          else if(mSensationCount == 1) InitAgent();
          else if(IsTypeKnown())
          {
              // Add time information before logging
              LogTime();

              // call decision component
              execBeam = execDrive = execKick = false;
              decision.Think();
              
              // execute action
              if(execBeam) Beam(BeamParam, true);
              if(execDrive) Drive(DriveParam, true);
              if(execKick) Kick(kick_angle, kick_power, true);
          }
          ++mSensationCount;
          PutOutput(DoneThinkingMessage);         	
      }    
  }
  
  return 1;
}