// draw the obstacles (roads)
void draw_obstacles(){
	glColor3f( 0.15f, 0.15f, 0.15f);
	size_t obstacles = sim->getNumObstacleVertices();

	glBegin(GL_QUADS);
	for( size_t i =0; i < obstacles-4; i++){
		RVO::Vector2 vec = sim->getObstacleVertex(i);
		glVertex3f(vec.x(), 0.0, vec.y());
	}
	glEnd();
}
void agent::calculateVelocities(RVO::Vector2 position,RVO::Vector2 velocity,
                                float timeStep)
{ 
    if( posGoal_realpixel == sf::Vector2f(-1,-1) )
        return;

    calculateVL(velocity, timeStep);
    calculateTeta(velocity,timeStep);
    calculateP();
    //qDebug()<<"***********************";

    setPosition( position.x(), position.y() );
}
Exemple #3
0
void rvo_sim()
{
    /* rvo - add agents */
    float mix_weight = param[8];
    std::vector<pv> origPVList; // original Pos and Vel 
    origPVList = pvList;
    
	for (int i = 0; i < pvList.size(); i ++)
	{
		sim->addAgent(RVO::Vector2(pvList[i].x, pvList[i].y));
		sim->setAgentVelocity(i, RVO::Vector2(pvList[i].vx, pvList[i].vy));
        //mix_velocity(pvList[i].prx, pvList[i].pry, pvList[i].vx, pvList[i].vy, 0.99);
        mix_velocity(pvList[i], mix_weight);
		sim->setAgentPrefVelocity(i, RVO::Vector2(pvList[i].prx, pvList[i].pry));
        
        //mexPrintf("%f %f %f %f\n", pvList[i].x, pvList[i].y, pvList[i].prx, pvList[i].pry);
	}
    
    /* simulate multiple times */
    int sim_times = (int)param[7];
    for (int i = 0; i < sim_times; i ++)
    {
        sim->doStep();
        
        for (int j = 0; j < pvList.size(); j ++)
        {
            RVO::Vector2 vel;
            vel = sim->getAgentVelocity(j);
            pvList[j].vx = vel.x(); 
            pvList[j].vy = vel.y();
            pvList[j].prx = origPVList[j].prx; // reset preferred velocity
            pvList[j].pry = origPVList[j].pry;
            mix_velocity(pvList[j], mix_weight);
            sim->setAgentPrefVelocity(j, RVO::Vector2(pvList[j].prx, pvList[j].pry));
        }
    }
    
//     /* result */
// 	RVO::Vector2 pos, vel, prv;
// 	pos = sim->getAgentPosition(id-1);
// 	vel = sim->getAgentVelocity(id-1);
//     prv = sim->getAgentPrefVelocity(id-1);
// 
// 	x = pos.x(); y = pos.y();
// 	vx = vel.x(); vy = vel.y();
}
void agent::calculateVL(RVO::Vector2 velocity, float timeStep)
{
    //float denominador = qPow( cos(teta*M_PI/180)  ,2) - qPow( sin(teta*M_PI/180)  ,2);

    cv::Mat m1(2,2,CV_32F),
            m2(2,2,CV_32F),
            m,
            v(2,1,CV_32F),
            u;

    m1.at<float>(0,0) = cos(teta);
    m1.at<float>(0,1) = cos(teta);
    m1.at<float>(1,0) = sin(teta);
    m1.at<float>(1,1) = sin(teta);
    //tools::math::printMat(m1);

    m2.at<float>(0,0) = sin(teta);
    m2.at<float>(0,1) = -sin(teta);
    m2.at<float>(1,0) = -cos(teta);
    m2.at<float>(1,1) = cos(teta);
    //tools::math::printMat(m2);

    m = 0.5f*m1 + (D_real/L_real)*m2;

    v.at<float>(0,0) = entornoGrafico::mapa::pixel2MedidaReal( velocity.x() )*100;
    v.at<float>(1,0) = entornoGrafico::mapa::pixel2MedidaReal( velocity.y() )*100;
    //tools::math::printMat(v);

    u = m.inv()*v;
    //tools::math::printMat(u);

    vL_linear = u.at<float>(0,0);
    vR_linear = u.at<float>(1,0);

    vL_angular = (vL_linear/wheelRadius_real)*(180/M_PI);
    vR_angular = (vR_linear/wheelRadius_real)*(180/M_PI);

    //si está disponible enviar velocidaddes.
    if( isAvaliable() )
        emit velocidadesCalculadas(ID,
                                   vR_angular,
                                   vL_angular);
}
void glutDisplay (void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
    glLoadIdentity();
	glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
    gluLookAt(10.0, 50.0, 80.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

	camera(); // update camera position
	
	// draw each agent
	for(size_t i = 0; i < agents; i+=4){
		agent_pos = sim->getAgentPosition(i);
		glColor4f(1, 0, 0, 1);
		draw_agent(agent_pos.x(), agent_pos.y());

		agent_pos = sim->getAgentPosition(i+1);
		glColor4f(0, 1, 0, 1);
		draw_agent(agent_pos.x(), agent_pos.y());

		agent_pos = sim->getAgentPosition(i+2);
		glColor4f(0, 0, 1, 1);
		draw_agent(agent_pos.x(), agent_pos.y());

		agent_pos = sim->getAgentPosition(i+3);
		glColor4f(1, 1, 0, 1);
		draw_agent(agent_pos.x(), agent_pos.y());
	}

	//draw_obstacles();
	draw_floor();
	draw_buildings();
	draw_intersection();

	draw_park();

	drawRoadMap(SHOW_GOALS);

	draw_boids( &birds ); // draw swarm (white spheres)

    glutSwapBuffers();

}
void AgentClass::run()
{	
	//Funkcija se periodicki poziva i radi korak simulacije u RVO simulatoru
	if (!(addAgent1True_))
	{
		gazebo_msgs::GetModelState pozicija;
		pozicija.request.model_name = "pioneer1";
		pozicija_.call(pozicija);						//Ocitavanje pozicije pioneera1 u Gazebu

		//Stvaranje objekta kvaterniona klase tf::Quaternion kako bi mogao koristiti funkcije za dobivanje kuta rotacije  
		tf::Quaternion q = tf::Quaternion(pozicija.response.pose.orientation.x,pozicija.response.pose.orientation.y,pozicija.response.pose.orientation.z,pozicija.response.pose.orientation.w);

		float theta = round(100*q.getAngleShortestPath())/100.0; //Kut rotacije najkraćim putem
		tf::Vector3 os = q.getAxis();
		float X,Y,V;
		RVO::Vector2 trenutna_brzina;
		if (os.z() >= 0.0)
		{
			X = round(100*pozicija.response.pose.position.x)/100.0 - (0.34/2)*cos(theta);
			Y = round(100*pozicija.response.pose.position.y)/100.0 + (0.34/2)*sin(theta);

			V = round(100*pozicija.response.twist.linear.x)/100.0;
			trenutna_brzina = RVO::Vector2(-1*V*cos(theta),V*sin(theta));
		}
		else
		{
			X = round(100*pozicija.response.pose.position.x)/100.0 - (0.34/2)*cos(theta);
			Y = round(100*pozicija.response.pose.position.y)/100.0 - (0.34/2)*sin(theta);

			V = round(100*pozicija.response.twist.linear.x)/100.0;
			trenutna_brzina = RVO::Vector2(-1*V*cos(theta),-1*V*sin(theta));
		}
		simulator->setAgentVelocity(pioneer1_num,trenutna_brzina);	//Trenutna brzina iz Gazeba u RVO simulator
		simulator->setAgentPosition(pioneer1_num,RVO::Vector2(X,Y)); //Trenutna pozicija iz Gazeba u RVO simulator
	}

	if (!(addAgent2True_))
	{
		gazebo_msgs::GetModelState pozicija;
		pozicija.request.model_name = "pioneer2";
		pozicija_.call(pozicija);						//Ocitavanje pozicije pioneera2 u Gazebu
		
		//Stvaranje objekta kvaterniona klase tf::Quaternion kako bi mogao koristiti funkcije za dobivanje kuta rotacije  
		tf::Quaternion q = tf::Quaternion(pozicija.response.pose.orientation.x,pozicija.response.pose.orientation.y,pozicija.response.pose.orientation.z,pozicija.response.pose.orientation.w);

		float theta = round(100*q.getAngleShortestPath())/100.0; //Kut rotacije najkraćim putem
		tf::Vector3 os = q.getAxis();
		float X,Y,V;
		RVO::Vector2 trenutna_brzina;
		if (os.z() >= 0.0)
		{
			X = round(100*pozicija.response.pose.position.x)/100.0 - (0.34/2)*cos(theta);
			Y = round(100*pozicija.response.pose.position.y)/100.0 + (0.34/2)*sin(theta);

			V = round(100*pozicija.response.twist.linear.x)/100.0;
			trenutna_brzina = RVO::Vector2(-1*V*cos(theta),V*sin(theta));
		}
		else
		{
			X = round(100*pozicija.response.pose.position.x)/100.0 - (0.34/2)*cos(theta);
			Y = round(100*pozicija.response.pose.position.y)/100.0 - (0.34/2)*sin(theta);

			V = round(100*pozicija.response.twist.linear.x)/100.0;
			trenutna_brzina = RVO::Vector2(-1*V*cos(theta),-1*V*sin(theta));
		}
		simulator->setAgentVelocity(pioneer1_num,trenutna_brzina);	//Trenutna brzina iz Gazeba u RVO simulator
		simulator->setAgentPosition(pioneer1_num,RVO::Vector2(X,Y)); //Trenutna pozicija iz Gazeba u RVO simulator
	}

	
	if (!addAgent1True_)
	{
		simulator->doStep(); //Provođenje koraka simulacije

		geometry_msgs::Twist brzina_twist;
		RVO::Vector2 brzina = simulator->getAgentVelocity(pioneer1_num);
		float theta_sad = atan2(brzina.y(),brzina.x()); //Kut vektora brzine u ovom koraku simulacije
		brzina_twist.linear.x = sqrt(pow(brzina.x(),2)+ pow(brzina.y(),2));
		brzina_twist.angular.z = (theta_sad-theta_prije)/simulator->getTimeStep();	
		theta_prije = theta_sad; //Spremanje kuta za sljedeći korak simulacije
		pub_.publish(brzina_twist);
	}
}