void CinematicMap::SetLaser(LaserData laserdata)
{
pointsCloud= laserdata.getPoints();
angleCloud = laserdata.getAngles();
rangeCloud = laserdata.getRanges();
Obstacle();
}
void Vision2D::SetLaser(LaserData laserdata) {
//1.01229097=58° -> Field of View (Horizontal, Vertical, Diagonal) = 58° H, 45° V, 70° D
//1.01229097/2=0.506145485
//resolution appears to be 1200x960 pixels at a framerate of 30Hz
//Deberia ser 1200 puntos en horizontal pero no deja mas de 541 puntos, src/data/laserdata.h mal¿?
//Operation range = 0.8m - 3.5m //dejamos el por defecto, 10 metros
//numstep=1.01229097/541=0.00187115;
//laserdata.setProperties() //NO FUNCIONA!!!!!
pointLaser = laserdata.getPoints();
anglelaser = laserdata.getAngles();
range = laserdata.getRanges();
startangle = laserdata.getStartAngle();
steplaser = laserdata.getStep();
//MANUALMENTE,el angulo +-0.50614 va del index 212-328, el rango que sea 8metros
//DE -PI/2 A +PI/2 (index 89-451, 451-89+1=363, rango=8metros.
//-pi/3 a +pi/3 (index 150-390)
int idxini=150;
int idxFin=390;
int idxRango=idxFin-idxini+1;
pointKinect2D.clear();
pointKinect2D.resize(idxRango);
rangeKinect2D.clear();
rangeKinect2D.resize(idxRango);
angleKinect2D.clear();
angleKinect2D.resize(idxRango);
int j = 0;
for (int i = idxini; i <= idxFin; i++) {
pointKinect2D[j] = pointLaser[i];
rangeKinect2D[j] = range[i];
angleKinect2D[j] = anglelaser[i];
j++;
}
//Calculo index objetos
indexObject.clear();
rangeObject.clear();
for (int i = 0; i < pointKinect2D.size(); i++) {
if (rangeKinect2D[i] < 8.0) { //Rango en que la Kinect detecta obstaculos
indexObject.push_back(i); //Guardamos el indice de los objetos, que junto a el vector points
rangeObject.push_back(rangeKinect2D[i]); //Sabremos donde esta el objeto (Haciendo la correspondiendte transformacion)
}
}
Object();
}
void CinematicMap::SetLaser(LaserData laserdata)
{
vector<Vector2D> points2D=laserdata.getPoints();
double zpos=0.4f; //high laser2D
pointsCloud.resize(points2D.size());
for(int i=0;i<points2D.size();i++)
{
pointsCloud[i]=Vector3D(points2D[i].x,points2D[i].y,zpos);
}
angleCloud = laserdata.getAngles();
rangeCloud = laserdata.getRanges();
Obstacle();
}
int main(int argc, char** argv)
{
mrcoreInit();
AriaBase robot(argc,argv);
WheeledBaseServer server(&robot,"base");
LaserSensorServer laserServer(&robot,"laser");
server.init(13000,1,true);
laserServer.init(13001,1,true);
while(1)
{
Sleep(500);
robot.watchdog();
LaserData data;
robot.getData(data);
cout<<"MEdidas: "<<data.getRanges().size()<<endl;
if(data.getRanges().size()>0)
cout<<"Medida: "<<data.getRanges()[data.getRanges().size()/2]<<endl;
}
server.close();
LOG_INFO("End of server");
return 1;
}
void Vision2D::SetLaser(LaserData laserdata) {
//1.01229097=58° -> Field of View (Horizontal, Vertical, Diagonal) = 58° H, 45° V, 70° D
//1.01229097/2=0.506145485
//resolution appears to be 1200x960 pixels at a framerate of 30Hz = 1200 puntos en horizontal
//Operation range = 0.8m - 3.5m //Demasiado poco, dejamos a 8 metros.
//numstep=1.01229097/1200
pointLaser = laserdata.getPoints();
anglelaser = laserdata.getAngles();
range = laserdata.getRanges();
startangle = laserdata.getStartAngle();
steplaser = laserdata.getStep();
//MANUALMENTE,el angulo +-0.50614 va del index 212-328, el rango que sea 8metros
//DE -PI/2 A +PI/2 (index 89-451, rango=8metros.
int idxini=89;
int idxFin=451;
int idxRango=idxFin-idxini+1;
//Añadir puntos al rango
pointKinect2D.clear();
pointKinect2D.resize(idxRango);
rangeKinect2D.clear();
rangeKinect2D.resize(idxRango);
angleKinect2D.clear();
angleKinect2D.resize(idxRango);
int j = 0;
for (int i = idxini; i <= idxFin; i++) {
pointKinect2D[j] = pointLaser[i];
rangeKinect2D[j] = range[i];
angleKinect2D[j] = anglelaser[i];
j++;
}
//Para el frontal los puntos de -pi/6 a pi/6, es decir, angulo -0.5236 a 0.5236
//Puntos 210-330, 330-210+1=121
//-20º a 20º, angulo 0.34906585, puntos 230-310
//-pi/3 a pi/3, angulo 1,04719, 150-390
int idxFrontini=210;
int idxFrontfin=330;
int idxFrontRan=idxFrontfin-idxFrontini+1;
//Añadir puntos para la frontal
pointKinect2DFront.clear();
pointKinect2DFront.resize(idxFrontRan);
rangeKinect2DFront.clear();
rangeKinect2DFront.resize(idxFrontRan);
angleKinect2DFront.clear();
angleKinect2DFront.resize(idxFrontRan);
int jj = 0;
for (int i = idxFrontini; i <= idxFrontfin; i++) {
pointKinect2DFront[jj] = pointLaser[i];
rangeKinect2DFront[jj] = range[i];
angleKinect2DFront[jj] = anglelaser[i];
jj++;
}
//Calculo index objetos
indexObject.clear();
rangeObject.clear();
for (int i = 0; i < pointKinect2D.size(); i++) {
if (rangeKinect2D[i] < 8.0) { //Rango en que la Kinect detecta obstaculos
indexObject.push_back(i); //Guardamos el indice de los objetos, que junto a el vector points
rangeObject.push_back(rangeKinect2D[i]); //Sabremos donde esta el objeto (Haciendo la correspondiendte transformacion)
}
}
Object();
//Calculo index objetos Frontal
indexObjectFront.clear();
rangeObjectFront.clear();
for (int i = 0; i < pointKinect2DFront.size(); i++) {
if (rangeKinect2DFront[i] < 8.0) { //Rango en que la Kinect detecta obstaculos
indexObjectFront.push_back(i); //Guardamos el indice de los objetos, que junto a el vector points
rangeObjectFront.push_back(rangeKinect2DFront[i]); //Sabremos donde esta el objeto (Haciendo la correspondiendte transformacion)
}
}
ObjectFrontal();
}
