#include "wifi_sensor_emulator.hpp" int main (int argc, char *argv

C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
#include "wifi_sensor_emulator.hpp"
int main (int argc, char *argv[])
{
ros::init(argc,argv,"wifi_sensor_emulator");
wifi mi_wifi;
while (ros::ok())
{
ros::spinOnce();
mi_wifi.publicadora();
}
return 0;
}
wifi::wifi()
{
std::string ficheros;
double a;
n_robot=0;
ros::NodeHandle manejador_priv("~");
if(manejador_priv.hasParam("high"))
manejador_priv.getParam("high",a);
else
a = 100.0;
high=(float) a;
manejador_priv.getParam("weigh",a);
weigh=(float) a;
manejador_priv.getParam("resolution",a);
resolution=(float) a;
n=high/resolution;
m=weigh/resolution;
char *ptr;
char frase[2500];
manejador_priv.getParam("ficheros",ficheros);
strcpy(frase,ficheros.c_str());
ptr=strtok(frase," ");
while(ptr != NULL)
{
std::string fichero_a_leer(ptr);
if(cargar_fichero(fichero_a_leer))
{
//mensaje de error al leer el fichero
}
ptr = strtok(NULL, " ");
}
-1-
domingo, 20 de septiembre de 2015 17:41
C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
domingo, 20 de septiembre de 2015 17:41
for(int i=0; i<red_wifi.size(); i++)
{
std::string aux4=red_wifi[i];
for(int j=0; j<m ; j++)
{
for(int k=0; k<n ; k++)
{
if(all_RssiMap[aux4].n_medidas[j][k] != 0)
{
all_RssiMap[aux4].level[j][k]=all_RssiMap[aux4].level[j][k]/all_RssiMap[
aux4].n_medidas[j][k];
all_RssiMap[aux4].varianza[j][k]=sqrt(all_RssiMap[aux4].varianza[j][k]/
all_RssiMap[aux4].n_medidas[j][k]);
}
}
}
}
flooding();
numero_robot=manejador.subscribe("/namespaces",1, &wifi::cuantosrobots,this);
}
bool wifi::cargar_fichero(std::string nombre_fichero)
{
std::ifstream input(nombre_fichero.c_str());
float clock, pose_age, x, y, z, roll, pitch, yaw;
std::string sensor_id, mac, essid, line;
int level, quality, existe;
std::vector <int> aux;
std::vector <float> aux2;
std::vector <float> aux3;
for(int i=0;i<n;i++)
{
aux.push_back(0);
aux2.push_back(100);
aux3.push_back(0);
}
for(int i=1; i<=8 ; i++)
{
getline(input,line);
}
while(!input.eof())
{
existe=0;
input >> clock >> pose_age >> x >> y >> z >> roll >> pitch >> yaw >> sensor_id >> mac
>> essid >> level >> quality;
for(int a=0; a<red_wifi.size(); a++)
{
if(mac==red_wifi[a])
{
-2-
C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
domingo, 20 de septiembre de 2015 17:41
existe=1;
}
}
if(existe==0)
{
red_wifi.push_back(mac);
mac_essid[mac]=essid;
for(int a=0;a<m;a++)
{
all_RssiMap[mac].resolution=resolution;
all_RssiMap[mac].n_medidas.push_back(aux);
all_RssiMap[mac].level.push_back(aux2);
all_RssiMap[mac].varianza.push_back(aux3);
}
}
int i = (x+weigh/2)/resolution;
int j = (-y+high/2)/resolution;
if(i>=0 && i<weigh/resolution-1 && j>=0 && j<high/resolution-1)
{
all_RssiMap[mac].level[i][j] = all_RssiMap[mac].level[i][j] + level;
all_RssiMap[mac].varianza[i][j] = all_RssiMap[mac].varianza[i][j]+level*level;
all_RssiMap[mac].n_medidas[i][j]++;
}
}
return true;
}
void wifi::flooding()
{
std::vector <std::vector <float> > flood;
for(int i=0; i<m; i++)
{
flood.push_back(std::vector <float>(n));
}
float numerador, denominador;
for(int p=0; p<red_wifi.size(); p++)
{
std::string aux4=red_wifi[p];
for(int i=0; i<m; i++)
{
for(int j=0; j<n; j++)
{
numerador=0.0;
denominador=0.0;
for(int k=-1; k<=1; k++)
{
for(int b=-1; b<=1; b++)
{
if(i+k>=0 && i+k<m && j+b>=0 && j+b<n)
{
if(all_RssiMap[aux4].level[i+k][j+b] != EMPTY)
{
-3-
C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
domingo, 20 de septiembre de 2015 17:41
numerador=numerador+all_RssiMap[aux4].level[i+k][j+b]*
all_RssiMap[aux4].n_medidas[i+k][j+b];
denominador=denominador+all_RssiMap[aux4].n_medidas[i+k][j+b];
}
}
}
}
if(numerador!=0 && denominador!=0)
{
flood[i][j]=numerador/denominador;
}
}
}
for(int i=0; i<m ; i++)
{
for(int j=0; j<n ; j++)
{
if(flood[i][j]!=0)
all_RssiMap[aux4].level[i][j]=flood[i][j];
}
}
}
}
void wifi::publicadora()
{
wifi_sensor::WifiScan msg;
for(int i=0;i<sensores.size();i++)
{
sensores[i].actualiza();
for(int j=0; j<red_wifi.size(); j++)
{
std::string aux4=red_wifi[j];
msg.essid.clear();
msg.level.clear();
msg.mac.clear();
msg.mac.push_back(aux4);
msg.essid.push_back(mac_essid[aux4]);
if(!sensores[i].datos.level.empty())
{
msg.level.push_back(sensores[i].datos.level[j]);
if(sensores[i].datos.level[j] != EMPTY)
{
sensores[i].publica_wifi.publish(msg);
}
}
}
}
}
void wifi::cuantosrobots(const multirobot_simulator_nes::robotsNamespaces nombres)
{
if (!sensores.empty())
-4-
C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
domingo, 20 de septiembre de 2015 17:41
{
if(nombres.NRobots != n_robot)
{
sensores.clear();
sensores.reserve(nombres.NRobots);
for (unsigned i=0; i<nombres.NRobots; i++)
{
sensores.push_back(sensor_wifi(nombres.Namespace[i],&all_RssiMap,red_wifi,
weigh,high));
sensores[i].posicion=sensores[i].manejador.subscribe(nombres.Namespace[i]+
"base_pose_ground_truth",1000, &sensor_wifi::posiciones,&sensores[i]);
}
}
}
else
{
sensores.reserve(nombres.NRobots);
for (unsigned i=0; i<nombres.NRobots; i++)
{
sensores.push_back(sensor_wifi(nombres.Namespace[i],&all_RssiMap,red_wifi,weigh,
high));
sensores[i].posicion=sensores[i].manejador.subscribe(nombres.Namespace[i]+
"base_pose_ground_truth",1000, &sensor_wifi::posiciones,&sensores[i]);
}
}
}
sensor_wifi::sensor_wifi(const std::string nombre,std::map <std::string, RssiMap> *
map_recibed,std::vector <std::string> redes, float w, float h)
{
all_RssiMap=map_recibed;
red_wifi=redes;
std::string aux4=red_wifi[0];
high=h;
weigh=w;
int resolution=(*all_RssiMap)[aux4].resolution;
int n=high/resolution;
int m=weigh/resolution;
publica_wifi=manejador.advertise<wifi_sensor::WifiScan>(nombre+"wifi",1,true);
}
void sensor_wifi::posiciones(const nav_msgs::OdometryConstPtr& msj_posicion)
{
datos.z=msj_posicion->pose.pose.position.z;
probando(msj_posicion->pose.pose.position.x,msj_posicion->pose.pose.position.y);
}
void sensor_wifi::probando(float x, float y)
{
datos.x=x;
datos.y=y;
}
void sensor_wifi::actualiza()
{
-5-
C:\Users\Jaime\Desktop\Proyecto\tfg\src\multirobot_simulator_nes\src\wifi_sensor_emulator\wifi_sensor_emulator.cpp
std::string aux4=red_wifi[0];
int i=(datos.x+weigh/2)/(*all_RssiMap)[aux4].resolution;
int j=(-datos.y+high/2)/(*all_RssiMap)[aux4].resolution;
if(i>0 && i<weigh-1 && j>0 && j<high-1)
{
datos.level.clear();
for(int p=0; p<red_wifi.size(); p++)
{
aux4=red_wifi[p];
datos.level.push_back((*all_RssiMap)[aux4].level[i][j]);
}
}
}
-6-
domingo, 20 de septiembre de 2015 17:41