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