/**
* Updates the particle set according to the motion.
*
* @TODO Currently assumes odometry is how FakeOdometryModule creates it,
* ie (delta X, delta Y, etc...
* Verify when motion module is pulled
* @return the updated ParticleSet.
*/
void MotionSystem::update(ParticleSet& particles,
const messages::RobotLocation& deltaMotionInfo)
{
ParticleIt iter;
for(iter = particles.begin(); iter != particles.end(); iter++)
{
Particle* particle = &(*iter);
/** Should be used if odometry gives global **/
/** Should also be TESTED extensively **/
// float sinh, cosh;
// sincosf(deltaMotionInfo.h() - particle->getLocation().h(),
// &sinh, &cosh);
// float changeX = cosh * deltaMotionInfo.x() + sinh * deltaMotionInfo.y();
// float changeY = cosh * deltaMotionInfo.y() - sinh * deltaMotionInfo.x();
float changeX = deltaMotionInfo.x();
float changeY = deltaMotionInfo.y();
float changeH = deltaMotionInfo.h();
particle->shift(changeX, changeY, changeH);
randomlyShiftParticle(particle);
}
// std::cout << "\n\n Updated Particles w/ Motion \n";
}
/**
* Updates the particle set according to the motion.
*
* @return the updated ParticleSet.
*/
void MotionSystem::update(ParticleSet& particles,
const messages::RobotLocation& odometry,
float error)
{
// Store the last odometry and set the current one
lastOdometry.set_x(curOdometry.x());
lastOdometry.set_y(curOdometry.y());
lastOdometry.set_h(curOdometry.h());
curOdometry.set_x(odometry.x());
curOdometry.set_y(odometry.y());
curOdometry.set_h(odometry.h());
// change in the robot frame
float dX_R = curOdometry.x() - lastOdometry.x();
float dY_R = curOdometry.y() - lastOdometry.y();
float dH_R = curOdometry.h() - lastOdometry.h();
if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) || (std::fabs(dH_R) > 0.35f) ) {
// Probably reset odometry somewhere, so skip frame
// std::cout << "std::fabs(dX_R) = " << std::fabs(dX_R) << " std::fabs(dY_R) = " << std::fabs(dY_R) << std::endl;
// std::cout << "curOdometry.x() = " << curOdometry.x() << " curOdometry.y() = " << curOdometry.y() << std::endl;
std::cout << "Odometry reset, motion frame skipped in loc" << std::endl;
return;
}
float dX, dY, dH;
ParticleIt iter;
for(iter = particles.begin(); iter != particles.end(); iter++)
{
Particle* particle = &(*iter);
// Rotate from the robot frame to the global to add the translation
float sinh, cosh;
sincosf(curOdometry.h() - particle->getLocation().h(),
&sinh, &cosh);
dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X;
dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y;
dH = dH_R * FRICTION_FACTOR_H;
particle->shift(dX, dY, dH);
// noiseShiftWithOdo(particle, dX, dY, dH, error);
randomlyShiftParticle(particle, false);
}
}
