void Filtro_Particulas::spin()
{
ros::Rate loopRate(freq_);
while(n_.ok())
{
ros::spinOnce();
loopRate.sleep();
//cout<<free_ok_<<occ_ok_<<odom_ok_<<laser_ok_<<endl;
if (free_ok_ == true && occ_ok_ == true){
createParticles();
if(create_particle_ok_ == 0 && odom_ok_ == true && laser_ok_ == true && zerar_deltas_ == false){
pose_anterior_.x = pose_x_;
pose_anterior_.y = pose_y_;
pose_anterior_.theta = pose_theta_;
zerar_deltas_ = true;
moveParticles();
//cout<<"moveParticles()"<<endl;
}else if(create_particle_ok_ == 0 && odom_ok_ == true && laser_ok_ == true && zerar_deltas_ == true){
moveParticles();
}
}
}
}
void ParticleProducer::updateParticles(double dt)
{
if (!initialized) {
initialize();
}
moveParticles(dt);
removeOldParticles();
addNewParticles();
}
void PsoSolver::run(const bool enableGLNPSO, const double minIw) {
this->enableGLNPSO = enableGLNPSO;
initFitness();
gBest = particles[0].pBest;
gBestFitness = particles[0].pBestFitness;
updateGbest();
for (iteration = 0; iteration < maxIteration; iteration++) {
if (getDispersionIDX() < convergenceThreshold && getVelocityIDX() < convergenceThreshold) {
break;
}
moveParticles();
updateFitness();
updateGbest();
// linear interia weighting adjustment
iw = max(iw - 1.0/maxIteration, minIw);
} // end of iteration
}
void Simulation::tick()
{
// The idea behind event driven simulation is quite
// ingenious: we determine the time of all collisions
// happening between all particles and walls assuming
// that particles move by straight lines at constant
// speed without any resistance.
//
// We keep the collision events arranged by time in priority
// queue, so that we always know when and what collisions
// are going to happen.
//
// The expensive calculations have to be done only
// once when the priority queue is initialised. By the expensive
// calculations I mean the calculation of all collisions between
// all available particles O(n^2). Then the event driven model
// requires to recalculate new events only after some event (collision)
// happens, which requires no more than O(N). That's why this
// model is so swift.
//
// Of course some of the events in the queue have to be cancelled after
// the collision event happens (since particle's trajectories change),
// that's why the system allows to detect whether the event
// is stale/cancelled.
if (events.empty()) {
if (particles.size() == 0) {
throw SimulationError("Simulation can not be launched "
"with 0 particles");
}
initializeEvents();
}
if (is_paused) {
SDL_Delay(delay_ms);
return;
}
bool enough = false;
while (!enough) {
Event *ev = events.top();
events.pop();
if (ev->isStale()) {
delete ev;
continue;
}
// simulation system does time related calculations
// in relative time, not absolute. So we have to
// translate relative time time to absolute one
// and vice versa.
moveParticles(ev->getTime() - now);
SDL_Delay(simulationTimeToMS(ev->getTime() - now));
now = ev->getTime();
switch (ev->getType()) {
case EventType::WallCollision:
{
// Particle collides a wall. This requires to calculate
// the collisions of this particle with all other particles
// and walls.
WallCollisionEvent *wc_ev = dynamic_cast<WallCollisionEvent*>(ev);
wc_ev->getParticle().bounceWall(wc_ev->getWallType());
predictCollisions(wc_ev->getParticle());
break;
}
case EventType::ParticleCollision:
{
// Two particles collide each other. This requires to calculate
// the collisions of these two particles with all other particles
// and walls.
ParticleCollisionEvent *pc_ev = dynamic_cast<ParticleCollisionEvent*>(ev);
pc_ev->getFirstParticle().bounceParticle(pc_ev->getSecondParticle());
predictCollisions(pc_ev->getFirstParticle());
predictCollisions(pc_ev->getSecondParticle());
break;
}
case EventType::Refresh:
refresh();
enough = true;
events.push(new RefreshEvent(now + MSToSimulationTime(delay_ms)));
break;
}
delete ev;
}
}