Exemple #1
0
int main(int argc, char* argv[]) {
	ros::init(argc, argv, "rvo_example");
	ros::NodeHandle nh("rvo_example");

	/* Create a new simulator instance. */
	RVO::RVOSimulator* sim = new RVO::RVOSimulator();

	/* Set up the scenario. */
	setupScenario(sim);

	/* Perform (and manipulate) the simulation. */
	ROS_INFO("Iteration");
	do {
#if RVO_OUTPUT_TIME_AND_POSITIONS
		updateVisualization(sim);
#endif
		setPreferredVelocities(sim);
		sim->doStep();
	} while (!reachedGoal(sim));
	ROS_INFO("End");

	delete sim;

	return 0;
}
Exemple #2
0
int main(int argc, const char* argv[])
{
    /* Create a new simulator instance. */
    RVO::RVOSimulator* sim = new RVO::RVOSimulator();

    /* Set up the scenario. */
    setupScenario(sim);

    /* Perform (and manipulate) the simulation. */
    do {
        updateVisualization(sim);
        setPreferredVelocities(sim);
        sim->doStep();
    } while (!reachedGoal(sim));

    delete sim;

    return 0;
}
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);
	}
}