Ejemplo n.º 1
0
void GraphicalRobotElement::setCurrentWIM (WorldInfo nwim) {
	currentWIM.Clear();

	if (teamColor == 1) {
		currentWIM = nwim;
	} else {
		WorldInfo MyWorld;
		MyWorld.Clear();
		MyWorld.CopyFrom (nwim);
		float value = (-1) * MyWorld.myposition().x();
		MyWorld.mutable_myposition()->set_x (value);
		value = (-1) * MyWorld.myposition().y();
		MyWorld.mutable_myposition()->set_y (value);
		value = MyWorld.myposition().phi() + Pi;
		MyWorld.mutable_myposition()->set_phi (value);
		currentWIM = MyWorld;
	}

	if (LWSTraceVisible) {
		setLWSTraceVisible (false);
		updateTraceRect();
		setLWSTraceVisible (true);
	} else {
		setLWSTraceVisible (false);
		updateTraceRect();
		setLWSTraceVisible (false);
	}
}
Ejemplo n.º 2
0
int Localization::LocalizationData_Load(parts & Particles, vector<KObservationModel> & Observation, KMotionModel & MotionModel) {
	bool addnewptrs = false;
	//Fill the world with data!
	WorldInfo *WI = DebugData.mutable_world();


	//Setting my position
	WI->mutable_myposition()->set_x(AgentPosition.x);
	WI->mutable_myposition()->set_y(AgentPosition.y);
	WI->mutable_myposition()->set_phi(AgentPosition.theta);
	WI->mutable_myposition()->set_confidence(AgentPosition.confidence);


	//Setting robotPositionField X = DX, Y = DY, phi = DF

	//	DebugData.mutable_robotposition()->set_x(MotionModel.Distance.val);
	//	DebugData.mutable_robotposition()->set_y(MotionModel.Direction.val);
	//	DebugData.mutable_robotposition()->set_phi(MotionModel.Rotation.val);
	DebugData.mutable_robotposition()->set_x(TrackPointRobotPosition.x);
	DebugData.mutable_robotposition()->set_y(TrackPointRobotPosition.y);
	DebugData.mutable_robotposition()->set_phi(TrackPointRobotPosition.phi);


	//	DebugData.mutable_myposition()->set_confidence(AgentPosition.confidence );

	RobotPose prtcl;

	if (DebugData.particles_size() < Particles.size)
		addnewptrs = true;
	for (unsigned int i = 0; i < Particles.size; i++) {
		if (addnewptrs)
			DebugData.add_particles();
		DebugData.mutable_particles(i)->set_x(Particles.x[i]);
		DebugData.mutable_particles(i)->set_y(Particles.y[i]);
		DebugData.mutable_particles(i)->set_phi(Particles.phi[i]);
		DebugData.mutable_particles(i)->set_confidence(Particles.Weight[i]);
	}


	//	if(osbm!=null){
	//		DebugData.Observations = obsm;
	//	}
	return 1;
}