worldmodel.cpp

浙江大学 RoboCup3D 2006 源代码

                {



                    name += strId;



                }







			//cerr << "object name " << name << endl;



			



            // try to lookup the VisionObject



			TVisionObjectMap::iterator iter = mVisionObjectMap.find(name);



            if (iter == mVisionObjectMap.end())



                {



			//cerr << "aoao2" << endl;

                    continue;



                }





              VisionObject vo = (*iter).second; 







            // find  to the 'pol' entry in the object's section



            Vector3 pol;



            if (pred.GetValue("pol", pol) == false) {



                continue;



            }







            // read the position vector



            VisionSense sense;



			sense.distance = pol[0]; sense.theta = pol[1]; sense.phi = pol[2];



			if (isnan(sense.phi) || isinf(sense.phi)) {



				cerr << "sense.phi " << sense.phi << " fixed to " << 90. << endl;



				sense.phi = 90.;



			}



			



 //            cout<<vo<<"sense( "<<sense.distance<<" "<<sense.theta<<" "<<sense.phi<<")"<<endl;



            // update the vision map



            mVisionMap[vo] = sense;



			//cerr << "mVisionMap[" << vo << "] = (" << sense.distance << "," << sense.theta << "," << sense.phi << ")" << endl;



        }



}







void



WorldModel::ParseVision(Predicate& predicate)



{



    ParseObjectVision(predicate);







    //



    // get our position. The current implementation of the worldmodel



    // relies on the VisionPerceptor that deliverd our position. In



    // future releases this should be replaced by a self localization







    // algorithm using the relative positions of some flags.



    //







    // find the PerfectVision data about the object







    // advance to the section about object 'name'



    if (predicate.GetValue("mypos", mMyPos) == false) {



		return;



	}







    cout << "mypos: (" << mMyPos[0] << "," << mMyPos[1] << "," << mMyPos[2] << ")" << endl;



}







bool WorldModel::Parse(const string& message)



{
//	cout << "WorldModel::Parse() begins" << endl;

	vector <Predicate> predList;

	// Initialize for parseHear()
	global.wm.hearMsg.isBallSeen = 0;
	global.wm.hearMsg.ballPos = Vector3(0, 0, 0), global.wm.hearMsg.ballVel= Vector3(0, 0, 0);
	for (int i=0; i<11; i++) {
		for (int j = 0; j < 3; ++j) {
			global.wm.hearMsg.our[i][j] = 0;
			global.wm.hearMsg.opp[i][j] = 0;
		}
	}
	memset(global.wm.hearMsg.action, 0, sizeof(global.wm.hearMsg.action));

	if (message.find('(') == -1) {



//		cout << "WorldModel::Parse() false" << endl;



		return false;



	}



	if (Parser::Parse(message.substr(message.find('(')), predList) == false) {



//		cout << "WorldModel::Parse() false" << endl;



		return false;



	}



	



    if (predList.size() < 1) {



//		cout << "WorldModel::Parse() false" << endl;



        return false;



    }







	for (int i = 0; i < predList.size(); ++ i) {







		if (predList[i].name == "GameState") {
			// check for the game state perceptor
			ParseGameState(predList[i]);
			if (global.wm.myNumber == 0 && mTeamUnum != 0) {
				cout << "my number is set to " << mTeamUnum << endl;
				global.wm.myNumber = mTeamUnum;
			}
		} else if (predList[i].name == "Vision") {	
			// check for the vision perceptor
			ParseVision(predList[i]);
		} else if(predList[i].name=="AgentState"){
		      ParseAgentState(predList[i]);
			//cerr << "(WorldModel) skipping unknown predicate " << predList[i].name << endl;
		} else if(predList[i].name == "hear"){
			ParseHear(predList[i]);
		}
	}

    // update our position
	CalcPlayerPosition();

//	cout << "WorldModel::Parse() ends" << endl;
	return true;
}







WorldModel::VisionSense WorldModel::GetVisionSense(VisionObject obj)



{



    TVisionMap::iterator iter = mVisionMap.find(obj);







    if (iter == mVisionMap.end())



        {



     //       cerr << "ERROR: (WorldMode::GetVisionSense) unknown VisionObject " << obj << endl;



            return VisionSense();



        }







    return (*iter).second;



}







Vector3 WorldModel::GetPosition(VisionSense sense)



{



    return mMyPos + GetDriveVec(sense)/* * sense.distance*/;//this is a bug



}







Vector3 WorldModel::GetObjectPosition(VisionObject obj)



{



    return GetPosition(GetVisionSense(obj));



}











TPlayMode WorldModel::GetPlayMode()



{



    return mPlayMode;



}







Vector3 WorldModel::GetDriveVec(const WorldModel::VisionSense& vision)



{



    return Vector3



        (



         vision.distance * cosDeg(vision.theta) *



         sinDeg(90.0 - vision.phi),







         vision.distance * sinDeg(vision.theta) *



         sinDeg(90.0 - vision.phi),







         vision.distance * cosDeg(90.0 - vision.phi)



         );



}







void WorldModel::CalcPlayerPosition()



{



}







Vector3 WorldModel::GetMyPosition()



{



    return mMyPos;



}







TTeamIndex WorldModel::GetMyTeam()



{
    return mTeamIndex;
}

double WorldModel::GetMinimalKickDistance()
{
    return mAgentRadius + mBallRadius + 0.04;
}

int WorldModel::GetTeamUnum()
{
    return mTeamUnum;
}

void WorldModel::SetTeamName(std::string name){
    mTeamName =   name;
}

string WorldModel::GetTeamName(){
    return mTeamName;
}

void WorldModel:: SetOppTeamName(std::string name)
{
    mOppTeamName = name;
}

string WorldModel::GetOppTeamName()
{
       return mOppTeamName;    
}

void WorldModel::ParseHear(Predicate& predicate)
{
  	vector <string> hearMsg = predicate.attr;
	vector <string>::iterator sIter = hearMsg.begin();

	int i = 0;

	string msg[3];
	for (; i<3; i++) {
		msg[i] = hearMsg[i];
	}
	if (msg[1] != "self") {
//		cerr << msg[2] << endl;
		string tempNum = msg[2].substr(0,msg[2].find('_'));
		msg[2] = msg[2].substr(msg[2].find('_')+1);
//		cerr <<"T "<<global.wm.currentTime<<endl;
		string tempTeamName = msg[2].substr(0, msg[2].find('_'));
		if (tempTeamName == sayMsgSign && msg[2].find("zzz") != 0) {
			//cout << tempNum << " say" << endl;
			string hearData="";

			msg[2] = msg[2].substr(msg[2].find('_')+1);
			if ((msg[2].substr(0, msg[2].find('_'))) == "zzb") {
				global.wm.hearMsg.isBallSeen = 1;
				msg[2] = msg[2].substr(msg[2].find('_') + 1);

				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballPos.x = parseFromHear(hearData) / 1000 -110;
				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballPos.y = parseFromHear(hearData) / 1000 -68;
				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballPos.z = parseFromHear(hearData) / 1000 -10;
				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballVel.x = parseFromHear(hearData) / 1000 -110;
				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballVel.y = parseFromHear(hearData) / 1000 -68;
				hearData = msg[2].substr(0, msg[2].find("_"));
				msg[2] = msg[2].substr(msg[2].find("_")+1);
				global.wm.hearMsg.ballVel.z = parseFromHear(hearData) / 1000 -10;
			}
			
			int k = 0;
			if ((msg[2].substr(0, msg[2].find('_'))) == "zzp") {
				msg[2] = msg[2].substr(msg[2].find('_') + 1);
				for (i=0; i < 22*3+1; i++) {
					hearData = msg[2].substr(0, msg[2].find("_"));
					msg[2] = msg[2].substr(msg[2].find("_")+1);
					
//				cerr << i/3 << " " << hearData << endl;
					if (hearData == "zzz")
						break;
					if (i%3 == 0) {
						k = atoi(hearData.c_str());
						if (k<12)
							global.wm.hearMsg.our[k][i%3] = 1;
						else
							global.wm.hearMsg.opp[k-11][i%3] = 1;					
					}
					else if (i%3 == 1) {
						if (k<12)
		 					global.wm.hearMsg.our[k][i%3] = parseFromHear(hearData) / 1000 -110;
						else
	 						global.wm.hearMsg.opp[k-11][i%3] = parseFromHear(hearData) / 1000 -110;
					}
					else if (i%3 == 2) {
						if (k<12)
							global.wm.hearMsg.our[k][i%3] = parseFromHear(hearData) / 1000 -68;
						else
							global.wm.hearMsg.opp[k-11][i%3] = parseFromHear(hearData) / 1000 -68;
					}
				}
			}
/*
			cerr << "is ball seen: " << global.wm.hearMsg.isBallSeen << "\nball position: " << global.wm.hearMsg.ballPos.x << "," << global.wm.hearMsg.ballPos.y << "," << global.wm.hearMsg.ballPos.z <<"\t" <<
				"\nball velocity: " << global.wm.hearMsg.ballVel.x << "," << global.wm.hearMsg.ballVel.y << "," << global.wm.hearMsg.ballVel.z << endl;
			cerr << "T: "<<global.wm.currentTime << endl;
			for (i=0; i<11; i++)
			{
				cerr << "our player " << i << ": " << global.wm.hearMsg.our[i][0] << "," << global.wm.hearMsg.our[i][1] << "," <<global.wm.hearMsg.our[i][2] << endl;
			}
			for (i=0; i<11; i++)
			{
				cerr << "opp player " << i << ": " << global.wm.hearMsg.opp[i][0] << "," << global.wm.hearMsg.opp[i][1] << "," <<global.wm.hearMsg.opp[i][2] << endl;
			}
*/
		}
	}
}

double WorldModel::parseFromHear(const string& str)
{
	double N = 0;
	char msg[4];
	strcpy(msg, str.c_str());
	int size = strlen(msg);
	for (int s=0; s<size; s++)
	{
		int i = msg[s];
		if (i < 34)
			N += pow((float)88, s) * (i-32);
		else
		{
			if (i < 39)
				N += pow((float)88, s) * (i-34);
			else if (i < 92)
				N += pow((float)88, s) * (i-37);
			else if (i < 95)
				N += pow((float)88, s) * (i-38);
			else
				N += pow((float)88, s) * (i-39);
		}
	}
	return N;
}

#endif