locwm.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "LOCWM.h"
#include "KF.h"
#include "../Globals.h"
#include "../Common/VisionObject.h"
#include "../Common/WorldObject.h"
#include "../Common/Common.h"

#include <iostream>
#include <math.h>

//#define NO_KF_BALL

using namespace std;

#define MINXCOORD -135
#define MAXXCOORD 135
#define MINYCOORD -245
#define MAXYCOORD 245

KF M1,M2;
int currentModel;
int error;//double cumError,cumErrorAlt;
int postSeen[4];//0=bl,1=br,2=yl,3=yr
int postIndex[4];
int beaconSeen[6];//0=yp,1=gp,2=bp,3=py,4=pg,5=pb
int beaconIndex[6];
int visionType[MAX_FIELD_OBJECTS];
int noFramesBallSeen;


LOCWM::LOCWM()
{
  worldObjectsBVR_ = new WorldObject[MAX_FIELD_OBJECTS];
  worldObjects_ = new WorldObject[MAX_FIELD_OBJECTS];
  worldObjectsDebug_ = new WorldObject[MAX_FIELD_OBJECTS];

  for (int i = 0; i < MAX_FIELD_OBJECTS; i++)
  {
	//data used for wireless (sent to self as well)
    worldObjects_[i].object_ = WorldObject::WM_OT_INVALID;
    worldObjects_[i].x_ = 0;
    worldObjects_[i].y_ = 0;
    worldObjects_[i].heading_ = 0;
    worldObjects_[i].uncertX_ = 0;
    worldObjects_[i].uncertY_ = 0;
    worldObjects_[i].uncertHeading_ = 0;
    worldObjects_[i].objSeenThisFrame_=false;

	//data sent to behaviour
    worldObjectsBVR_[i].object_ = WorldObject::WM_OT_INVALID;
    worldObjectsBVR_[i].x_ = 0;
    worldObjectsBVR_[i].y_ = 0;
    worldObjectsBVR_[i].heading_ = 0;
    worldObjectsBVR_[i].uncertX_ = 0;
    worldObjectsBVR_[i].uncertY_ = 0;
    worldObjectsBVR_[i].uncertHeading_ = 0;
    worldObjectsBVR_[i].objSeenThisFrame_=false;

	//used for WorldModel debugging program
    worldObjectsDebug_[i].object_ = WorldObject::WM_OT_INVALID;
	worldObjectsDebug_[i].x_ = 0;
	worldObjectsDebug_[i].y_ = 0;
	worldObjectsDebug_[i].heading_ = 0;
	worldObjectsDebug_[i].uncertX_ = 0;
	worldObjectsDebug_[i].uncertY_ = 0;
	worldObjectsDebug_[i].uncertHeading_ = 0;
    worldObjectsDebug_[i].objSeenThisFrame_=false;
  }

  //initialise self to be worldObjects_[0]
  numWorldObjects_ = 1;
  worldObjects_[0].object_ = WorldObject::WM_OT_SELF;
  worldObjects_[0].x_ = 0;
  worldObjects_[0].y_ = 0;
  worldObjects_[0].heading_ = 0;

  //initialise ball to be worldObjectsDebug_[1]
  worldObjectsDebug_[5].object_ = WorldObject::WM_OT_BALL_IND;
  worldObjectsDebug_[5].x_ = 0;
  worldObjectsDebug_[5].y_ = 0;
  worldObjectsDebug_[5].heading_ = 0;

  numWorldObjectsBVR_ = 6;
  numWorldObjectsDebug_ = 6;
  currentModel=1;
  noFramesBallSeen=0;
}

void LOCWM::IntegrateVisionData()
{
	numWorldObjects_ = 1;
	M1.TimeUpdate(distErrorPerFrameStill,angleErrorPerFrameStill,distErrorPerFrameMoving,angleErrorPerFrameMoving,ballDistErrorPerFrameStill,0);
//M2.TimeUpdate(distErrorPerFrameStill,angleErrorPerFrameStill,distErrorPerFrameMoving,angleErrorPerFrameMoving,ballDistErrorPerFrame,0);
	lcq_.ClearLocomotionData();
	KFilter(&M1,1); //Filter Self and Ball Model 1
//KFilter(&M2,2); //Filter Self and Ball Model 2
	BallPosition(); //Trig ball for wireless
	M1.CheckIfLost();
//M2.CheckIfLost();
	for (int i = 0; i < numVisionObjects_; i++)
	{
		if(visionObjects_[i].type_==VisionObject::OT_BALL)
		{
			noFramesBallSeen++;
		}
	}
	if ((frame_%25==0)&&(noFramesBallSeen>5))
	{
		SwapModels();
	}
	IntegrateFourRobotData(worldObjects_, numWorldObjects_, BOTID_);//Send vision data to behaviour
}


void LOCWM::SwapModels()
{
/*
	double diff;
	diff=delta*noFramesBallSeen/25;
	if (M1.cumError>(M2.cumError+diff/2.0))
	{
		if (M1.cumError>(M2.cumError+diff))
		{
			cout<<frame_<<" Overwriting Models - Moving"<<endl<<flush;
			M1.X = M2.X;
			M1.P = M2.P;
			currentModel=2;
		}
		else if(currentModel==1)
		{
			cout<<frame_<<" Model Swap - Moving"<<endl<<flush;
			currentModel=2;
		}
	}
	if (M2.cumError> (M1.cumError - diff/5.0))
	{
		if (M2.cumError>(M1.cumError + diff))
		{
			cout<<frame_<<" Overwriting Models - Still"<<endl<<flush;
			M2.X = M1.X;
			M2.P = M1.P;
			currentModel=1;
		}
		else if (currentModel==2)
		{
			currentModel=1;
			cout<<frame_<<" Model Swap - Still"<<endl<<flush;
		}
	}
	M1.cumError=0;
	M2.cumError=0;
	noFramesBallSeen=0;*/
}

//Takes data from vision and wireless and sends to behaviour
void LOCWM::IntegrateFourRobotData(const WorldObject* robotData, int numObjects, int robotID)
{

	//for WorldObjectBVR_
	//Robot 1 in [0]
	//Robot 2 in [1]
	//Robot 3 in [2]
	//Robot 4 in [3]
	//Ball co-op in [4] (currently not used)
	//Ball indiv in [5]
	//Other team robots after this

	//numWorldObjectsBVR_ = 6;
	numWorldObjectsDebug_ = 6;
	//determine object type for robot
	if (robotID==BOTID_)
	{
		//worldObjectsBVR_[BOTID_-1].object_ = WorldObject::WM_OT_SELF;
		worldObjectsDebug_[BOTID_-1].object_ = WorldObject::WM_OT_SELF;
	}
	else
	{
    	if (robotState_.GetTeam()==RobotState::RT_RED)
    	{
      		worldObjectsDebug_[robotID-1].object_=WorldObject::WM_OT_ROBOT_RED;
    	}
    	else
    	{
			 worldObjectsDebug_[robotID-1].object_=WorldObject::WM_OT_ROBOT_BLUE;
    	}

		static double ballx=0,ballxuncert=0,bally=0,ballyuncert=0;
		for (int i = 0; i < numObjects; i++)
		{
			//co-operative ball
		    if (robotData[i].object_==WorldObject::WM_OT_BALL_IND)
		    {
		     	ballx=robotData[i].x_;
		     	ballxuncert=robotData[i].uncertX_;
		     	bally=robotData[i].y_;
		     	ballyuncert=robotData[i].uncertY_;
#ifndef NO_KF_BALL
         		M1.KFBallCoop(ballx, bally, ballxuncert,  ballyuncert, ballDistErrorPerFrame);
#endif
//       		M2.KFBallCoop(ballx, bally, ballxuncert,  ballyuncert, ballDistErrorPerFrame);
	      	}
	  	}
  	}


  worldObjectsDebug_[5].object_ = WorldObject::WM_OT_BALL_IND;
  if(currentModel==1)
  {
#ifdef NO_KF_BALL
    // using trig ball only atm ...
    for (int i = 0; i < numWorldObjects_; i++) {
      if (worldObjects_[i].object_ == WorldObject::WM_OT_BALL_IND) {
        worldObjectsDebug_[5].x_ = worldObjects_[i].x_;
        worldObjectsDebug_[5].y_ = worldObjects_[i].y_;
        worldObjectsDebug_[5].uncertX_ = worldObjects_[i].uncertX_;
        worldObjectsDebug_[5].uncertY_ = worldObjects_[i].uncertY_;

      }
    }
#else
  	worldObjectsDebug_[5].x_ = M1.X[3][0];
  	worldObjectsDebug_[5].y_ = M1.X[4][0];
  	worldObjectsDebug_[5].uncertX_ = sqrt(M1.P[3][3]);
  	worldObjectsDebug_[5].uncertY_ = sqrt(M1.P[4][4]);
#endif
  }
  else
  {
/*  worldObjectsDebug_[5].x_ = M2.X[3][0];
	  worldObjectsDebug_[5].y_ = M2.X[4][0];
	  worldObjectsDebug_[5].uncertX_ = sqrt(M2.P[3][3]);
    worldObjectsDebug_[5].uncertY_ = sqrt(M2.P[4][4]);*/
  }

  worldObjectsDebug_[robotID-1].x_ = robotData[0].x_;
  worldObjectsDebug_[robotID-1].y_ = robotData[0].y_;
  worldObjectsDebug_[robotID-1].heading_ = robotData[0].heading_;
  worldObjectsDebug_[robotID-1].uncertX_ = robotData[0].uncertX_;
  worldObjectsDebug_[robotID-1].uncertY_ = robotData[0].uncertY_;
  worldObjectsDebug_[robotID-1].uncertHeading_ = robotData[0].uncertHeading_;

  numWorldObjectsBVR_ = numWorldObjectsDebug_;
  memcpy(worldObjectsBVR_, worldObjectsDebug_, sizeof(WorldObject)*numWorldObjectsDebug_);

  SetDataForTeam(worldObjectsBVR_, numWorldObjectsBVR_);//Changes co-ordinate system to that used by behaviour

}



void LOCWM::KFilter(KF* M, int modelNo)
{
  int i = 0;
	for (i =0; i<4; i++)
	{
		postSeen[i]=0;
		postIndex[i]=0;
	}

	for (i =0; i<6; i++)
	{
		beaconSeen[i]=0;
		beaconIndex[i]=0;
	}

	for (i = 0; i < numVisionObjects_; i++)
	{
		visionType[i]=visionObjects_[i].type_;
		if ((visionObjects_[i].type_>=VisionObject::OT_GOAL_B_L)&&(visionObjects_[i].type_<=VisionObject::OT_GOAL_Y_R))
		{
			postSeen[visionObjects_[i].type_-8]=1;
			postIndex[visionObjects_[i].type_-8]=i;
		}
		else if ((visionObjects_[i].type_>=VisionObject::OT_BEACON_YP)&&(visionObjects_[i].type_<=VisionObject::OT_BEACON_PB))
		{
			beaconSeen[visionObjects_[i].type_-2]=1;
			beaconIndex[visionObjects_[i].type_-2]=i;
		}
	}
	BeaconGoalPost(M);
	static double outsideCorners=0;
	outsideCorners*=0.95;
	if ((outsideCorners>4)&&goalKeeper)
	{
		cout<<"Outside box"<<endl<<flush;
	}

	for (i = 0; i < numVisionObjects_; i++)
	{
		//beacons
		if ((visionType[i]>=VisionObject::OT_BEACON_YP)&&(visionType[i]<=VisionObject::OT_BEACON_PB))
		{