agent.cpp

东南大学最新的校内比赛的源代码

/*****************************************************************************************
 *                                      SEU-3D
 *                     -------------------------------------------------
 * Copyright (c) 2005, Yuan XU<[e-mail]>,Chang'e SHI<[e-mail]>
 * Copyright (c) 2006, Yuan XU<[e-mail]>,Chunlu JIANG<[e-mail]>
 * Southeast University ,China
 * All rights reserved.
 *
 * Additionally,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 Library 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.
 ****************************************************************************************/
/**
 * \file            Agent.cpp
 *
 * \brief agent基类/the basic agent class.
 *
 *
 * 实现agent的基本动作,以及初始化,感知-思考-动作的主循环
 *
 * LastModify:
 * $Id: Agent.cpp,v 1.1.1.1 2006/09/15 02:03:05 Administrator Exp $
 */
 
#include "Agent.h"
#include "Logger.h"
#include "Geometry.h"
#include "Formation.h"
#include "Parser.h"


using namespace std;
using namespace salt;

#ifdef ENABLE_LOG
extern FileLogger msgLog;
extern FileLogger localizationLog;
extern FileLogger ballPosLog;
#endif

Agent::Agent()
{
	_teamName = "SEU-3D";
    _sensationCount = 0;
    _driveVec = Vector3f(0,0,0);
	_minLatecy = sense_latecy;
	_maxLatecy = max_request_time_notify;
	_panTilted = false;
}

Agent::~Agent()
{
}

void Agent::processInitMessage()
{
    // Ddata

    // After startup, this initialization data message is sent to the
    // agent. On a normal startup, data will be empty. On an agent
    // migration (see Section 5.3), the data returned by the old agent
    // processes will be given to the new agent. An initialization done
    // message should be sent once the initialization data has been
    // processed and all other startup is complete.

    
    LOG( 1,"(Agent) received init message.\n");
   	COMM->PutOutput(doneInitMessage());
}

void Agent::processThinkTimeMessage()
{
    // Ktime

    // This is a think time message that notifies the agent how much
    // thinking time was used for the last thinking cycle. These are
    // only sent if send agent think times is on.

    
    LOG( 1, "(Agent) think Time Message.\n" );
	LOG( 2, " Average think time : %f", WM->getAverageThinkTime() );
    // still unsure what the correct behavior is...

}

void Agent::think()
{
    // Stime time data

    // This is a sensation to be given to the agent. It begins a
    // thinking cycle. The first time is the simulation time the
    // sensation was generated (also known as the send time) and the
    // second is the time that the sensation is delivered to the agent
    // (also known as the arrive time). If the parameter send agent send
    // time is off, -1 is always sent as the send time, and if the
    // parameter send agent arrive time is off, -1 is always sent as the
    // arrive time. The data is an arbitrary data string generated by
    // the world model. The agent can reply with act messages, and must
    // finish with a done thinking message.
	LOGBEGIN;
	LOG( 1, "received sensation %i" , _sensationCount );
	
	//if ( WM->getGameTime() > 150 ) exit(0);
	
	WM->update();
	
	_panTilted = false;

    switch ( _sensationCount )
        {
        case 0:
            createAgent();
            break;

        case 1:
            initAgent();
            break;

        default:;
            playMatch();
        }
	
    ++_sensationCount;
	//if ( _sensationCount > 1000 ) exit(1);//profile the program
	//--------------for DEBUG-----------------
	//-- init FileLogger and open file
	if ( _sensationCount == 10 )
	{
		init();
		WM->setSameTypeTeammateNum(); //-* can only update after set the num
#ifdef ENABLE_LOG		
		msgLog.openFile("msgLog"); //-* logging all msg for communcation with the server
		localizationLog.openFile("myPos");//-* for debuging localization
		ballPosLog.openFile("ballPos"); //-* for debuging localize ball's position
#endif
	}
	//-------------end DEBUG----------------------

/*  ** From 'NEWS' in SPADES 1.10 **
 *   
 *  Agents can now, in the middle of a thinking cycle, send a "request current think time" message. The commserver responds with the
 *  current think time for this thinking cycle. This message is processed by the commserver (no network message to the engine) and
 *  the commserver responds as quickly as it can. The format:
 *  Agent->CS: "C" is a request current think time message
 *  CS->Agent: "C<time>" is the response, where <time> is the current thinking time
 *  CS->Agent: "req_curr_think_time_when_not_thinking" is a new error code if the agent sends this message when it's not thinking
 */	
	//-* "C" message
	//-- send a "C" to commserver
	//-- commserver reply current think time
	//-- the reply formation is "C<time>"
//	Time timeBeforeThink = WM->getRealTime();
	COMM->PutOutput( requestThinkTimeMessage() );
	LOG( 2, "---------after send C-------");
	if ( COMM->GetInput() )
	{
		LOG( 2, COMM->GetMsg() );
#ifdef ENABLE_LOG
		msgLog.log( COMM->GetMsg() );
		msgLog.log( "\n");
#endif
		PA->parseAll(COMM->GetMsg());
		LOG( 2, "C time: %f",WM->getCurrentThinkTime() );
	}

	sendActionNotifys();
}

void Agent::createAgent()
{
    // use the create effector to setup the Sensors and Effectors
    LOG( 1, "creating agent\n");

    COMM->PutOutput("A(create)");
}

void Agent::initAgent()
{
    LOG( 1,"sending init command\n");

    stringstream ss;
    ss << "A(init (unum " << _teamUnum << ") (teamname " << _teamName << "))";

    COMM->PutOutput(ss.str().c_str());
}

void Agent::done()
{
	COMM->PutOutput(doneThinkingMessage());
}

void Agent::kick( float angle, float power)
{
    LOG( 1,"kicking Ball angle=%f power=%f", angle, power );
	GL_PRINT("test","kick0[%f, %f]",angle,power);
	angle = clamp(angle,min_kick_angle,max_kick_angle);
	
	_actSS << "(kick "
	<< angle
	<< " " << power
	<< ")";
	GL_PRINT("test","kick[%f, %f]",angle,power);
}

void Agent::beam(const salt::Vector3f& position)
{
    LOG( 1,"beaming [ %f, %f, %f ]", position[0], position[1], position[2] );

	_actSS << "(beam "
       << position[0]
       << " " << position[1]
       << " " << position[2]
       << ")";

	//-* reset my gloabl position
	WM->setMyGlobalPosWhileBeam( position );
}

void Agent::drive(const salt::Vector3f& driveVec)
{
    //LOG( 1,"driving [ %f, %f, %f ]", driveVec[0], driveVec[1], driveVec[2] );

    //GL_LINE("drive",WM->getMyGlobalPos(),WM->getMyGlobalPos()+driveVec*0.1f,colorWhite);

	//-* when use drive(0,0,0), the agent seems keep the drive force,
	//-- not change to (0,0,0). So, when try to stop, we use drive(1,0,0) instead it.
	//-- perhaps it is a server's bug
	//Vector3f driveForce = driveVec;
	//if ( driveForce == Vector3f(0,0,0) )
	//	driveForce = Vector3f(1,0,0);
	
    /*_actSS << "(drive "
       << driveForce[0]
       << " " << driveForce[1]
       << " " << driveForce[2]
       << ")";*/
	_actSS << "(drive "
       << driveVec[0]
       << " " << driveVec[1]
       << " " << driveVec[2]
       << ")";

	WM->setMyDriveForce( driveVec );
	GL_PRINT("test","drive[%f, %f, %f]",driveVec[0],driveVec[1],driveVec[2]);
}

/**
	say action
@param[in] msg the string message which will be said
*/
void Agent::say(const string &msg)
{
	_actSS<<"(say "
	   << msg
       << ")";
}


void Agent::goalieCatch()
{
	_actSS<<"(catch)";
}

void Agent::requestTimeNotify( int t )
{
	if ( _requestedTime.find(t) == _requestedTime.end() )
	{
		stringstream ss;
	 	ss <<"R"
        	<<t;
		COMM->PutOutput(ss.str());
		_requestedTime.insert(t);
	}
}

void Agent::sendAction( const Action& a)
{	
	std::map<BasicActionType, BasicActionData>::const_iterator iter(a.begin()), end(a.end());
	
	for(;iter!=end;iter++)
	{
		sendBasicAction( iter->first,iter->second );
	}
}

void Agent::sendAction( const Time t )
{
	LOG( 3,"sendAction");
	Action action;
	if ( _actionQueue.popAction( t, action) )
	{
		_actSS.str("");
		_actSS<<'A';
		sendAction( action );
		COMM->PutOutput(_actSS.str());
	}
}

void Agent::sendThinkTimeNotifys()
{
	WM->clearThinkTimeSet();
	for( Step dt = think_steps; dt < step_num_per_cycle; dt += think_steps )
	{
		int time = int( WM->getRealTime() ) + dt;
		WM->pushThinkTime( time );
	    requestTimeNotify( time );
	}
}

void Agent::sendActionNotifys()
{
	_actionQueue.removeNoNeedAction(WM->isKickable());
	set<Time> actionTimeSet = _actionQueue.getActionDequeTime();
	stringstream ss;
	for ( set<Time>::iterator iter = actionTimeSet.begin(); iter!=actionTimeSet.end(); iter++)
	{
		requestTimeNotify( (int)(*iter));
	}
}

void Agent::endThink()
{
#ifdef MULITPLY_THREAD
	int	rc = pthread_mutex_lock( &actionSendedMutex);
	if ( rc)//-* thread lock error
	{
		LOGERR("thread lock error");
		exit(1);
	}
#endif
	//-* delete the old data of _requestedTime
	_requestedTime.erase( _requestedTime.begin(), _requestedTime.upper_bound( WM->getRealTime()) );
	//-* send 'D' to server, inform that end think
	COMM->PutOutput(doneThinkingMessage());
#ifdef MULITPLY_THREAD
	rc = pthread_mutex_unlock( &actionSendedMutex);
	if ( rc)//-* thread unlock error
	{
		LOGERR("thread unlock error");
		exit(1);
	}
#endif
}

void Agent::sendBasicAction(const BasicActionType type, const BasicActionData& data )
{
	switch ( type )
	{
		case BAT_DRIVE :
			drive( data._vec );
			break;
		
		case BAT_KICK :
			kick( data._vec.x(), data._vec.y() );
			break;
		
		case BAT_BEAM :
		{
			beam( data._vec );
			break;
		}
		
		case BAT_CATCH :
		{
			goalieCatch();
			break;
		}
		
		case BAT_SAY :
		{
			//say(  ... );
			break;
		}
		
		case BAT_PANTILT :
		{
			//panTilt( data._vec.x(), data._vec.y() );
			break;
		}
		
		case BAT_ILLEGAL :
		{
			LOGERR("I can not send an ILLEGAL action!");
			break;
		}
		
		default:
			LOGERR("I can not recognize this action!");
			break;
	}
}

/** 处理'O'信息 */
void Agent::processOMessage(const std::string& msg)
{
	Time oTime = static_cast<Time>(atof(&msg[1]));
	_actionQueue -= ( oTime );
	LOGERR("receive O at %f",oTime);
}