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