void LocalizationModule::update()
{
// Modify based on control portal
if (lastReset != resetInput.message().timestamp())
{
lastReset = resetInput.message().timestamp();
particleFilter->resetLocTo(resetInput.message().x(),
resetInput.message().y(),
resetInput.message().h());
}
// Calculate the deltaX,Y,H (PF takes increments from robot frame)
lastOdometry.set_x(curOdometry.x());
lastOdometry.set_y(curOdometry.y());
lastOdometry.set_h(curOdometry.h());
// Want odometry to give information relative to current robot frame
// IE choose to have robot frame change as the robot moves
float sinH, cosH;
sincosf(motionInput.message().h(), &sinH, &cosH);
float rotatedX = cosH*motionInput.message().x()
+ sinH*motionInput.message().y();
float rotatedY = cosH*motionInput.message().y()
- sinH*motionInput.message().x();
curOdometry.set_x(rotatedX * ODOMETRY_X_FRICTION_FACTOR);
curOdometry.set_y(rotatedY * ODOMETRY_Y_FRICTION_FACTOR);
curOdometry.set_h(motionInput.message().h() * ODOMETRY_HEADING_FRICTION_FACTOR);
deltaOdometry.set_x(curOdometry.x() - lastOdometry.x());
deltaOdometry.set_y(curOdometry.y() - lastOdometry.y());
deltaOdometry.set_h(curOdometry.h() - lastOdometry.h());
// Ensure deltaOdometry is reasonable (initial fix lost in git?)
if((fabs(deltaOdometry.x()) > 3.f) || (fabs(deltaOdometry.y()) > 3.f))
{
deltaOdometry.set_x(0.f);
deltaOdometry.set_y(0.f);
deltaOdometry.set_h(0.f);
}
// Update the Particle Filter with the new observations/odometry
particleFilter->update(deltaOdometry, visionInput.message());
// Update the locMessage and the swarm (if logging)
portals::Message<messages::RobotLocation> locMessage(&particleFilter->
getCurrentEstimate());
#ifdef LOG_LOCALIZATION
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->
getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
#endif
output.setMessage(locMessage);
}
void LocalizationModule::update()
{
#ifndef OFFLINE
// Modify based on control portal
for (int i = 0; i < 2; i++) {
if (lastReset[i] != resetInput[i].message().timestamp())
{
std::cout<<"RESET LOC ON "<<i<<std::endl;
lastReset[i] = resetInput[i].message().timestamp();
particleFilter->resetLocTo(resetInput[i].message().x(),
resetInput[i].message().y(),
resetInput[i].message().h());
break;
}
}
#endif
curOdometry.set_x(motionInput.message().x());
curOdometry.set_y(motionInput.message().y());
curOdometry.set_h(motionInput.message().h());
#ifndef OFFLINE
bool inSet = (STATE_SET == gameStateInput.message().state());
// Update the Particle Filter with the new observations/odometry
if (inSet && (!gameStateInput.message().have_remote_gc() ||
gameStateInput.message().secs_remaining() != 600))
particleFilter->update(curOdometry, visionInput.message(), ballInput.message());
else
#endif
particleFilter->update(curOdometry, visionInput.message());
// Update the locMessage and the swarm (if logging)
portals::Message<messages::RobotLocation> locMessage(&particleFilter->
getCurrentEstimate());
#if defined( LOG_LOCALIZATION) || defined(OFFLINE)
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->
getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
#endif
output.setMessage(locMessage);
}
void LocalizationModule::update()
{
#ifndef OFFLINE
for (int i = 0; i < 2; i++) {
if (lastReset[i] != resetInput[i].message().timestamp())
{
// std::cout << "RESET LOC ON " << i << std::endl;
lastReset[i] = resetInput[i].message().timestamp();
// See PenaltyStates.py in the manualPlacement state
if ((int)(resetInput[i].message().x()) == 999) {
std::cout << "Resetting Loc to Manual Placement Values" << std::endl;
std::vector<particleLocationStruct> particleVector;
// Coordinates include green padding around field
particleVector.push_back(particleLocationStruct());
particleVector.at(0).x = 160;
particleVector.at(0).y = 160;
particleVector.at(0).h = 0;
particleVector.push_back(particleLocationStruct());
particleVector.at(1).x = 160;
particleVector.at(1).y = 320;
particleVector.at(1).h = 0;
particleVector.push_back(particleLocationStruct());
particleVector.at(2).x = 160;
particleVector.at(2).y = 420;
particleVector.at(2).h = 0;
particleVector.push_back(particleLocationStruct());
particleVector.at(3).x = 160;
particleVector.at(3).y = 600;
particleVector.at(3).h = 0;
particleVector.push_back(particleLocationStruct());
particleVector.at(4).x = 420;
particleVector.at(4).y = 360;
particleVector.at(4).h = 0;
particleFilter->resetLocToMany(particleVector);
} else {
particleFilter->resetLocTo(resetInput[i].message().x(),
resetInput[i].message().y(),
resetInput[i].message().h());
}
break;
}
}
#endif
// Save odometry and lines
curOdometry.set_x(motionInput.message().x());
curOdometry.set_y(motionInput.message().y());
curOdometry.set_h(motionInput.message().h());
curVision = visionInput.message();
curBall = ballInput.message();
// TODO: use CC
messages::CenterCircle curCircle = curVision.circle();
const messages::FilteredBall* ball = NULL;
#ifndef OFFLINE
bool inSet = (STATE_SET == gameStateInput.message().state());
if (inSet)
ball = &curBall;
#endif
// Update filter
particleFilter->update(curOdometry, curVision, ball);
//this is part of something old that never executes, check out
//the ifdef below; same code but it is executed when we want to
//to log localization
#if defined( LOG_LOCALIZATION) || defined(OFFLINE)
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
#endif
portals::Message<messages::RobotLocation> locMessage(&particleFilter->getCurrentEstimate());
output.setMessage(locMessage);
#ifdef USE_LOGGING
if(control::check(control::flags::locswarm)) {
++log_index;
std::string log_from = "loc";
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->
getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
messages::RobotLocation rl = *output.getMessage(true).get();
messages::ParticleSwarm ps = *particleOutput.getMessage(true).get();
messages::Vision vm = curVision;
nbl::logptr theLog = nbl::Log::emptyLog();
theLog->logClass = "locswarm";
theLog->createdWhen = time(NULL);
theLog->addBlockFromProtobuf(rl, "locswarm", 0, 0);
theLog->addBlockFromProtobuf(ps, "locswarm", 0, 0);
theLog->addBlockFromProtobuf(vm, "locswarm", 0, 0);
nbl::NBLog(theLog);
// std::string rl_buf;
// std::string ps_buf;
// std::string vm_buf;
// std::string log_buf;
//
// rl.SerializeToString(&rl_buf);
// ps.SerializeToString(&ps_buf);
// vm.SerializeToString(&vm_buf);
//
// log_buf.append(rl_buf);
// log_buf.append(ps_buf);
// log_buf.append(vm_buf);
//
// std::vector<SExpr> contents;
//
// SExpr naoLocation("location", log_from, clock(), log_index, rl_buf.length());
// contents.push_back(naoLocation);
//
// SExpr naoSwarm("swarm",log_from,clock(),log_index,ps_buf.length());
// contents.push_back(naoSwarm);
//
// SExpr naoVision("vision",log_from,clock(),log_index,vm_buf.length());
// contents.push_back(naoVision);
//
// NBLog(NBL_SMALL_BUFFER,"LOCSWARM",contents,log_buf);
}
#endif
}
void LocalizationModule::update()
{
#ifndef OFFLINE
for (int i = 0; i < 2; i++) {
if (lastReset[i] != resetInput[i].message().timestamp())
{
// std::cout<<"RESET LOC ON "<<i<<std::endl;
lastReset[i] = resetInput[i].message().timestamp();
particleFilter->resetLocTo(resetInput[i].message().x(),
resetInput[i].message().y(),
resetInput[i].message().h());
break;
}
}
#endif
// Save odometry and lines
curOdometry.set_x(motionInput.message().x());
curOdometry.set_y(motionInput.message().y());
curOdometry.set_h(motionInput.message().h());
curVision = visionInput.message();
curBall = ballInput.message();
// TODO: use CC
messages::CenterCircle curCircle = curVision.circle();
const messages::FilteredBall* ball = NULL;
#ifndef OFFLINE
bool inSet = (STATE_SET == gameStateInput.message().state());
if (inSet)
ball = &curBall;
#endif
// Update filter
particleFilter->update(curOdometry, curVision, ball);
//this is part of something old that never executes, check out
//the ifdef below; same code but it is executed when we want to
//to log localization
#if defined( LOG_LOCALIZATION) || defined(OFFLINE)
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
#endif
portals::Message<messages::RobotLocation> locMessage(&particleFilter->getCurrentEstimate());
output.setMessage(locMessage);
#ifdef USE_LOGGING
if(control::flags[control::LOCALIZATION]) {
++log_index;
std::string log_from = "loc";
portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->
getCurrentSwarm());
particleOutput.setMessage(swarmMessage);
messages::RobotLocation rl = *output.getMessage(true).get();
messages::ParticleSwarm ps = *particleOutput.getMessage(true).get();
messages::Vision vm = curVision;
std::string rl_buf;
std::string ps_buf;
std::string vm_buf;
std::string log_buf;
rl.SerializeToString(&rl_buf);
ps.SerializeToString(&ps_buf);
vm.SerializeToString(&vm_buf);
log_buf.append(rl_buf);
log_buf.append(ps_buf);
log_buf.append(vm_buf);
std::vector<SExpr> contents;
SExpr naoLocation("location", log_from, clock(), log_index, rl_buf.length());
contents.push_back(naoLocation);
SExpr naoSwarm("swarm",log_from,clock(),log_index,ps_buf.length());
contents.push_back(naoSwarm);
SExpr naoVision("vision",log_from,clock(),log_index,vm_buf.length());
contents.push_back(naoVision);
NBLog(NBL_SMALL_BUFFER,"LOCSWARM",contents,log_buf);
}
#endif
}
