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