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

			}
		}
	}
}
Esempio n. 2
0
void ParticleProducer::updateParticles(double dt)
{
    if (!initialized) {
        initialize();
    }
    moveParticles(dt);
    removeOldParticles();
    addNewParticles();
}
Esempio n. 3
0
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
}
Esempio n. 4
0
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;
    }
}