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);
}
}
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;
}