basicworldmodel.cpp

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

    //-* <ObjectName>+<Id> -> VisionObject
	//-* fixed objects
    _visionObjectMap["Flag1_l"] = VO_FLAG1L;
    _visionObjectMap["Flag1_r"] = VO_FLAG1R;
    _visionObjectMap["Flag2_l"] = VO_FLAG2L;
    _visionObjectMap["Flag2_r"] = VO_FLAG2R;
    _visionObjectMap["Goal1_l"] = VO_GOAL1L;
    _visionObjectMap["Goal1_r"] = VO_GOAL1R;
    _visionObjectMap["Goal2_l"] = VO_GOAL2L;
    _visionObjectMap["Goal2_r"] = VO_GOAL2R;
	
	//-* ball
    _visionObjectMap["Ball"]    = VO_BALL;
	
    //-* opponents
	string oppTeamName = _oppTeamName;
    _visionObjectMap["Player"+oppTeamName+"1"]  =  VO_OPPONENT_1;
    _visionObjectMap["Player"+oppTeamName+"2"]  =  VO_OPPONENT_2;
    _visionObjectMap["Player"+oppTeamName+"3"]  =  VO_OPPONENT_3;
    _visionObjectMap["Player"+oppTeamName+"4"]  =  VO_OPPONENT_4;
    _visionObjectMap["Player"+oppTeamName+"5"]  =  VO_OPPONENT_5;
    _visionObjectMap["Player"+oppTeamName+"6"]  =  VO_OPPONENT_6;
    _visionObjectMap["Player"+oppTeamName+"7"]  =  VO_OPPONENT_7;
    _visionObjectMap["Player"+oppTeamName+"8"]  =  VO_OPPONENT_8;
    _visionObjectMap["Player"+oppTeamName+"9"]  =  VO_OPPONENT_9;
    _visionObjectMap["Player"+oppTeamName+"10"] =  VO_OPPONENT_10;
    _visionObjectMap["Player"+oppTeamName+"11"] =  VO_OPPONENT_11;

	//-* teammates
	string teamName = getMyTeamName();
    _visionObjectMap["Player"+teamName+"1"]  =  VO_TEAMMATE_1;
    _visionObjectMap["Player"+teamName+"2"]  =  VO_TEAMMATE_2;
    _visionObjectMap["Player"+teamName+"3"]  =  VO_TEAMMATE_3;
    _visionObjectMap["Player"+teamName+"4"]  =  VO_TEAMMATE_4;
    _visionObjectMap["Player"+teamName+"5"]  =  VO_TEAMMATE_5;
    _visionObjectMap["Player"+teamName+"6"]  =  VO_TEAMMATE_6;
    _visionObjectMap["Player"+teamName+"7"]  =  VO_TEAMMATE_7;
    _visionObjectMap["Player"+teamName+"8"]  =  VO_TEAMMATE_8;
    _visionObjectMap["Player"+teamName+"9"]  =  VO_TEAMMATE_9;
    _visionObjectMap["Player"+teamName+"10"] =  VO_TEAMMATE_10;
    _visionObjectMap["Player"+teamName+"11"] =  VO_TEAMMATE_11;

}

VisionSense BasicWorldModel::getVisionSense( VisionObject vo )
{
    TVisionMap::iterator iter = _visionMap.find( vo );

    if (iter == _visionMap.end())
        {
            //LOGERR("[ERROR](BasicWorldMode::getVisionSeen) unknown VisionObject: %i",vo);

            return VisionSense();
        }

    return (*iter).second;
		
}

void BasicWorldModel::setMyTeamName( string teamName ) 
{
	//LOG( 1,"Set teamName to :"+teamName );
	_myself.setTeamName( teamName ); 
	
	setupVisionObjectMap();
}

void BasicWorldModel::setMyGlobalPosWhileBeam( Vector3f posGlobal )
{
	
	LOG( 23,"setMyGlobalPosWhileBeam %f,%f,%f",posGlobal[0],posGlobal[1],posGlobal[2]);
	Time time = getRealTime();
	_myself.updateDynamic( time+action_latecy, posGlobal, posGlobal, Vector3f(0,0,0), Vector3f(0,0,0) );
	_myself.setDriveForce( Vector3f(0,0,0) );
	
	Vector3f pos = _myself.getGlobalPos();
	LOG( 23,"setMyGlobalPosWhileBeam after %f,%f,%f",pos[0],pos[1],pos[2]);
	
}

void BasicWorldModel::setBallBeforeKickOff()
{
	_ball.updateDynamic( getRealTime(), Vector3f(0,0,0), _myself.getGlobalPos(), Vector3f(0,0,0), Vector3f(0,0,0) );
}

void BasicWorldModel::setOppTeamName( const string &teamName )
{
	cout<<" opp team name is : "<<teamName<<'\n';
	_oppTeamName = teamName;
	
	setupVisionObjectMap();
}
/*
void BasicWorldModel::parseAgentState()
{
	LOG( 2,"seu parse agent state");
	while ( !PA->isBracketClose() )
	{
		//-* read the state name
        string StateName;
		//cout<<"StateName: "<<StateName<<endl;
        if ( !PA->getNextParamName(StateName) )
        {
            continue;
        }
		//cout<<"StateName: "<<StateName<<endl;
		//-* update the agent state
		if ( StateName == "battery")
		{
			float battery = 0;
			PA->getNextValue(battery);
			_myself.setBattery( battery );
		}
		else if ( StateName == "temp" )
		{
			float temp = 0;
			PA->getNextValue(temp);
			_myself.setTemperature( temp );
		}
		else
		{
			LOGERR("[Warning](BasicWorldModel::parseAgentState) unknown agent state!");
		}
		PA->gotoNextLeftBracket();		
	}		
}
*/
void BasicWorldModel::update()
{
	//-* 1.parse
	//parse( message );

	//-* 2.updateAll
	updateAll();
	LOG( 2,"complete updateAll");

	//-* 3.predict
	predict();
	LOG( 2,"complete predict");
	LOG( 2,"update ok");
}

void BasicWorldModel::updateAll()
{
	//-* 1. update the information of the flags and goals
	//-- for agent localization need these infromation
	updateFixedObjects();
	
	//-* 2. update agent:
	//-- localization
	updateAgent();
	
	//-* 3. update ball's information
	newupdateBall();
	
	//-* 4. update other Players
	updatePlayers();
	
	/** 5. update offside lines */
	updateOffSideLine();
}

void BasicWorldModel::updateFixedObjects()
{
	_flag_1_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG1L ) );
	_flag_1_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG1R ) );
	_flag_2_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG2L ) );
	_flag_2_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_FLAG2R ) );

	_goal_1_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL1L ) );
	_goal_1_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL1R ) );
	_goal_2_l.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL2L ) );
	_goal_2_r.updateLastSeen( _timeLastSeen, getVisionSense( VO_GOAL2R ) );

	/*VisionSense bv = _flag_1_l.getVisionSeen( 0 );
	LOG( 11,"flag_1_l updated: %f:%f,%f,%f", _flag_1_l.getTimeSeen( 0 ),bv.distance,bv.theta,bv.phi);

	Vector3f pos = _flag_1_l.getGlobalPos();
	LOG( 11,"flag_1_l global pos: %f,%f,%f",pos[0],pos[1],pos[2] );
	*/
}

void BasicWorldModel::updateAgent()
{
	localization();
	//Vector3f posMe = _myself.getGlobalPos();
	//LOG(11,"mypos: %f,%f,%f",posMe[0],posMe[1],posMe[2]);
	
	//Vector3f driveForce = _myself.getDriveForce();
	//LOG(11,"driveForce: %f,%f,%f",driveForce[0],driveForce[1],driveForce[2]);
}

void BasicWorldModel::newupdateBall()
{
	//-* update the sense about the ball
	_ball.updateLastSeen( _timeLastSeen, getVisionSense( VO_BALL ) );

	//-* -----update the global position and velocity of the ball---------
	Time timeDelta = _ball.getTimeSeen() - _ball.getTime();
	if ( timeDelta <= 0 )
		return;
	Step iStep = Time2Step(timeDelta);
	//LOG( 21," timeDelta: %f", timeDelta );
	//-* calculate ball global pos and global velocity
	Vector3f globalPos, globalVelocity;
	//-* in playon mode, we must calculate both ball's position and velocity
	//-- but in unplayon mode, we only need to set the vaule of some var
	if ( getPlayMode() == PM_PlayOn 
		 || getPlayMode() == PM_GOAL_KICK_LEFT
		 || getPlayMode() == PM_GOAL_KICK_RIGHT)
	{
		globalPos = _ball.getGlobalPos();
		Vector3f globalPos1 = _ball.getGlobalPos(1);
		Vector3f globalPos2 = _ball.getGlobalPos(2);
		globalVelocity = _ball.getGlobalVelocity();
		Vector3f globalVelocity1 = _ball.getGlobalVelocity(Step(1));
		Vector3f globalVelocity2 = _ball.getGlobalVelocity(Step(2));

		calculateBallState( globalPos, globalPos1, globalPos2,
							globalVelocity, globalVelocity1, globalVelocity2,
							iStep);
	}
	else
	{
		globalPos = getBallPositionUnPlayOn();
		globalVelocity.Zero();
	}
	if ( globalPos.z() < _ball.getRadius() )
		globalPos.z() = _ball.getRadius();
	_ball.updateDynamic( _ball.getTimeSeen(), globalPos, _myself.getGlobalPos(), globalVelocity, Vector3f(0,0,0) );
}

Vector3f BasicWorldModel::getBallPositionUnPlayOn()
{
	Vector3f posBall = _ball.calculateGlobalPosFromVision( _myself.getGlobalPos());
	Vector3f posOld = _ball.getGlobalPos();
	Vector3f velOld = _ball.getGlobalVelocity();
	switch ( getPlayMode())
	{
		case PM_BeforeKickOff :
		case PM_KickOff_Left :
        case PM_KickOff_Right :
			posBall.Zero();
            break;

        case PM_KickIn_Left :
        case PM_KickIn_Right :
		{
			posBall.y() = sign( posBall.y())*getFieldWidth()/2;
			posBall.z() = _ball.getRadius();
            break;
		}
		
		case PM_CORNER_KICK_LEFT:
		case PM_CORNER_KICK_RIGHT:
		{
			posBall.x() = sign( posBall.x())*getFieldLength()/2;
			posBall.y() = sign( posBall.y())*getFieldWidth()/2;
			posBall.z() = _ball.getRadius();
			break;
		}

        default:
            break;
	}

	//if pre cycle have been unPlayOn
	if ( velOld == Vector3f(0,0,0))
	{
		posBall = ( posBall + posOld)/2;
	}
	
	return posBall;
}

void BasicWorldModel::calculateBallState( Vector3f &posBall, Vector3f &prePosBall, Vector3f &preprePosBall,
										Vector3f &velBall, Vector3f &preVelBall, Vector3f &prepreVelBall,
										const Step iStep)
{
	//LOG( 25,"deltaTime: %f s",Step2Second(iStep));
	const float max_dist_gap = 0.5;
	Vector3f posSee, velSee, posSim, velSim;
	posSee = _ball.calculateGlobalPosFromVision( _myself.getGlobalPos());
	//posSim = _ball.calculateGloabalPosFromMemory( deltaTime);
	posSim = posBall;
	velSim = velBall;
	Vector3f posSim1 = prePosBall;
	Vector3f posSim2 = preprePosBall;
	Vector3f velSim1 = preVelBall;
	Vector3f velSim2 = prepreVelBall;
	_ball.updateKalman(iStep);
	Vector3f visionError = _ball.getVisionError();
	Vector3f velPol = _ball.calculateGlobalVelFromVision( _myself.getGlobalVelocity());
	Vector3f velPolErr = _myself.getKalmanP11() + visionError;
	//-* if collide happened, the see information only used
	//-- if no collide, use both information form see and simulate
	if ( _ball.predictStateAfterNrSteps( posSim, posSim1, posSim2, velSim, velSim1, velSim2, iStep) 
		&& (posSee -posSim).Length() < max_dist_gap	)
	{
		//LOG( 25,"posSim: %f, %f, %f",posSim[0],posSim[1],posSim[2]);
		//LOG( 25,"posSee: %f, %f, %f",posSee[0],posSee[1],posSee[2]);
		Vector3f simulatePosError = _ball.getKalmanP22();
		Vector3f simulateVelError = _ball.getKalmanP12();
		LOG( 25,"visionError: %f, %f, %f",visionError[0],visionError[1],visionError[2]);
		LOG( 25,"simulatePosError: %f, %f, %f",simulatePosError[0],simulatePosError[1],simulatePosError[2]);
		Vector3f weightedPos = posSee;
		float k = 0.0;
		for ( int i=0; i<3; i++)
		{
			k = visionError[i] / ( simulatePosError[i] +visionError[i]);
			weightedPos[i] += k*( posSim[i] - weightedPos[i]);
			simulatePosError[i] = visionError[i]*(1-k);
			LOG( 25,"K[%d]=%f",i,k);
			velSee[i] = ( weightedPos[i] - posBall[i])/Step2Second(iStep);
			k = visionError[i] / ( simulateVelError[i] +visionError[i]);
			velBall[i] = velSee[i] + k*( velSim[i] - velSee[i]);
			visionError[i] -= k*visionError[i];
			k = visionError[i]/ ( velPolErr[i] + visionError[i] );
			velBall[i] += k*( velPol[i] - velBall[i] );
		}
		_ball.setKalmanP22(simulatePosError);
		LOG( 25,"weightedPos: %f, %f, %f",weightedPos[0],weightedPos[1],weightedPos[2]);
		//LOG( 25,"velSee: %f, %f, %f",velSee[0],velSee[1],velSee[2]);
		//LOG( 25,"velSim: %f, %f, %f",velSim[0],velSim[1],velSim[2]);
		posBall = weightedPos;
	}
	else
	{
		LOG( 25,"more than max gap!");
		_ball.setKalmanP22(visionError);
		Vector3f posOld = posBall;
		posBall = posSee;
		//posBall = lowFreqSignalFilter( posSee, posBall);
		velSee = (posBall - posOld)/Step2Second(iStep); 
		velBall = ( velSee + velPol*0.5)/1.5;
		//velBall = lowFreqSignalFilter( velSee, velBall);
	}
	
	//
	//Vector3f velD = velBall -  velPol;
	//LOG( 25,"*");
	//LOG( 25,"velPol : %f,%f,%f",velPol[0],velPol[1],velPol[2]);
	//LOG( 25,"velBall: %f,%f,%f",velBall[0],velBall[1],velBall[2]);
	//LOG( 25,"velD : %f,%f,%f",velD[0],velD[1],velD[2]);
	//

}

void BasicWorldModel::updateBall()
{
	//-* update the sense about the ball
	_ball.updateLastSeen( _timeLastSeen, getVisionSense( VO_BALL ) );
	//VisionSense bv = _ball.getVisionSeen( 0 );
	//LOG( 11,"ball updated: %f:%f,%f,%f", _ball.getTimeSeen( 0 ),bv.distance,bv.theta,bv.phi);

	//-* -----update the global position and velocity of the ball---------
	Time timeDelta = ( _ball.getTimeSeen() - _ball.getTime() ) * 0.01;
	if ( timeDelta <= 0 )
		return;
	//LOG( 21," timeDelta: %f", timeDelta );
	//-* calculate ball global pos
	Vector3f tmp1, tmp2, globalPos, globalVelocity;
	tmp1 = _myself.getGlobalPos() + _ball.getPosRelativeSeen();
	tmp2 = _ball.getGlobalPos() + _ball.getGlobalVelocity()*timeDelta;
	globalPos = ( tmp1 + tmp2*0.5 )/1.5;
	//LOG( 21,"_ball.getPosRelativeSeen: %f,%f,%f",_ball.getPosRelativeSeen()[0],_ball.getPosRelativeSeen()[1],_ball.getPosRelativeSeen()[2]);
	//LOG( 21," tmp1: %f,%f,%f", tmp1[0],tmp1[1],tmp1[2]);
	//LOG( 21," tmp2: %f,%f,%f", tmp2[0],tmp2[1],tmp2[2]);
	//LOG( 21," poaBall 1: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);
	globalPos = lowFreqSignalFilter( globalPos, _ball.getGlobalPos() );
	//LOG( 21," poaBall 2: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);
	globalPos[2] = tmp1[2];
	globalPos[2] = setMaxNMin( globalPos[2], getFieldHeight(), _ball.getRadius() );
	//LOG( 21," poaBall 3: %f,%f,%f", globalPos[0],globalPos[1],globalPos[2]);
	//-* calculate ball velocity 
	tmp1 = ( globalPos - _ball.getGlobalPos() )/timeDelta;
	if ( globalPos[2] > _ball.getRadius() 
		&& _ball.getGlobalVelocity()[2] > 1 )//-* air ball
	{
		tmp2 = _ball.getGlobalVelocity()*( 1-timeDelta*air_friction_factor/_ball.getMass() );
		tmp2[2] = tmp1[2];
	}
	else//-* ground ball
	{
		tmp2 = _ball.getGlobalVelocity()*( 1-timeDelta*ground_friction_factor/_ball.getMass() );
		tmp2[2] = tmp1[2];
	}
	//LOG( 21," tmp1: %f,%f,%f", tmp1[0],tmp1[1],tmp1[2]);
	//LOG( 21," tmp2 = v*( 1-t*u/m ): v: %f,%f,%f m: %f", 
	//			_ball.getGlobalVelocity()[0],_ball.getGlobalVelocity()[1],_ball.getGlobalVelocity()[2],_ball.getMass());
	//LOG( 21," tmp2: %f,%f,%f", tmp2[0],tmp2[1],tmp2[2]);
	globalVelocity = (tmp1 + tmp2)/2;
	globalVelocity[0] = lowFreqSignalFilter( globalVelocity[0], _ball.getGlobalVelocity()[0] );
	globalVelocity[1] = lowFreqSignalFilter( globalVelocity[1], _ball.getGlobalVelocity()[1] );
	
	_ball.updateDynamic( _ball.getTimeSeen(), globalPos, _myself.getGlobalPos(), globalVelocity, Vector3f(0,0,0) );
}

void BasicWorldModel::updatePlayers()
{
	updateMyselfInTeam();//-* this step munst before update our team!!
	updateTeam( _teammate, VO_TEAMMATE_1 );
	updateTeam( _opponent, VO_OPPONENT_1 );	
}

/*! update the Player's info, who is myself in team
*/
void BasicWorldModel::updateMyselfInTeam()
{
	//-* if can not make sure my number, only in the beginning
	//-- do nothing
	if ( getMyNum() > 0 && getMyNum() <= teammates_num )
	{
		vector< PlayerObject >::iterator iter = _teammate.begin()+getMyNum()-1;
		iter->updateDynamic( getTimeLastSeen(),
									getMyGlobalPos(), getMyGlobalPos(), getMyGlobalVel(), Vector3f(0,0,0) );
	}
}

void BasicWorldModel::updateTeam( vector<PlayerObject> &team ,VisionObject num_1 )
{
	Vector3f posMe = _myself.getGlobalPos();
	//LOG( 21,"posMe: %f,%f,%f",posMe[0],posMe[1],posMe[2]);
	//-* Team
	unsigned int num = num_1;
	for ( vector< PlayerObject >::iterator iter = team.begin();
			iter != team.end();
			iter++, ++num )