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);
}
}
void AgentClass::setupScenario()
{
simulator->setTimeStep(0.1f); //Postavljanje vremena koraka simulacije
simulator->setAgentDefaults(1.0f,5,0.1f,5.0f,0.4f,1.0f);//Osnovni parametri za svakog novog robota dodanog u simulaciju
}
void AgentClass::callback_function_2(const geometry_msgs::Twist::ConstPtr& data)
{
if (addAgent2True_)
{
gazebo_msgs::GetModelState pozicija;
pozicija.request.model_name = "pioneer2";
pozicija_.call(pozicija);
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;
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);
}
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);
}
simulator->addAgent(RVO::Vector2(X,Y)); //Prvotno dodavanje pioneera u RVO simulator
pioneer2_num = simulator->getNumAgents()-1; //Pamćenje rednog broja robota u simulatoru za povrat informacije iz simulatora
addAgent2True_ = false; //Zastavica da je robot dodan u simulator
//Postavljanje preferirane brzine 2. pioneera
if (os.z() >= 0.0)
{
RVO::Vector2 brzina_pref = RVO::Vector2(-1*data->linear.x*cos(theta),data->linear.x*sin(theta)); //Vektor preferirane brzine
simulator->setAgentPrefVelocity(pioneer2_num, brzina_pref); //Postavljanje prefrerirane brzine 2. pioneera
}
else
{
RVO::Vector2 brzina_pref = RVO::Vector2(-1*data->linear.x*cos(theta),-1*data->linear.x*sin(theta)); //Vektor preferirane brzine
simulator->setAgentPrefVelocity(pioneer2_num, brzina_pref); //Postavljanje prefrerirane brzine 2. pioneera
}
}
else
{
gazebo_msgs::GetModelState pozicija;
pozicija.request.model_name = "pioneer2";
pozicija_.call(pozicija);
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();
if (os.z() >= 0.0)
{
RVO::Vector2 brzina_pref = RVO::Vector2(-1*data->linear.x*cos(theta),data->linear.x*sin(theta)); //Vektor preferirane brzine
simulator->setAgentPrefVelocity(pioneer2_num, brzina_pref); //Postavljanje prefrerirane brzine 2. pioneera
}
else
{
RVO::Vector2 brzina_pref = RVO::Vector2(-1*data->linear.x*cos(theta),-1*data->linear.x*sin(theta)); //Vektor preferirane brzine
simulator->setAgentPrefVelocity(pioneer2_num, brzina_pref); //Postavljanje prefrerirane brzine 2. pioneera
}
}
}