/** * 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); } }
/** * Updates the particle set according to the motion. * * @return the updated ParticleSet. */ void MotionSystem::update(ParticleSet& particles, const messages::RobotLocation& odometry, bool nearMid) { // 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) ) { //Probably reset odometry somewhere so skip a frame 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; // just add the rotation particle->shift(dX, dY, dH); noiseShiftWithOdo(particle, dX, dY, dH); //randomlyShiftParticle(particle, nearMid); } }