void SpecificWorker::updateLaser()
{
RoboCompLaser::TLaserData laserData;
laserData = laser_proxy->getLaserData();
//if(int(laserData.size()) != lidarParams.numRays){
if(int(laserData.size()) != lidarParams.numRays){
printf("Incorrect number of Laser Scan rays!\n");
printf("received: %d\n",int(laserData.size()));
}
else
{
int j=0, cont=0;
for(auto i : laserData)
{
// if(cont>=100 and cont<=668)
{
const QString transf = QString::fromStdString("laserPointTransf_")+QString::number(cont);
lidarParams.laserScan[j] = i.dist/1000.f;
innerModel->updateTransformValues(transf, i.dist*sin(i.angle), 0, i.dist*cos(i.angle), 0, 0, 0, "redTransform");
j++;
}
cont++;
}
}
}
int HokuyoHandler::SiguienteNoNulo(RoboCompLaser::TLaserData & laserData,int pos,int aStart,int aEnd)
{
int i=1;
float temp=0.;
int posaux=0;
while ( (((pos-i)>aStart)||((pos+i)<aEnd)) && ((temp<ERROR_C)||(temp>ERROR_L)) )
{
if ((pos-i)>aStart)
temp=laserData.at(pos-i).dist;
if (((pos+i)<aEnd) && ((temp<ERROR_C)||(temp>ERROR_L)))
temp=laserData.at(pos+i).dist;
i++;
}
i--;
if ( (temp>=ERROR_C) && (temp<=ERROR_L) )
{
if ((pos-i)>=aStart)
{
if ( (laserData.at(pos-i).dist>=ERROR_C) && (laserData.at(pos-i).dist<=ERROR_L) )
posaux=pos-i;
else
posaux=pos+i;
}
}
pos=posaux;
return ( temp );
}
void SpecificWorker::drawParticles()
{
for(uint i = 0; i<localization->particles.size(); ++i)
{
const QString transf = QString::fromStdString("particle_")+QString::number(i);
const QString item = QString::fromStdString("plane_")+QString::number(i);
const QString item2= QString::fromStdString("orientacion_")+QString::number(i);
InnerModelDraw::addTransform(innerModelViewer,transf,"floor");
InnerModelDraw::addPlane_notExisting(innerModelViewer, item,transf,QVec::vec3(0,0,0),QVec::vec3(1,0,0),"#0000AA",QVec::vec3(100, 50, 100));
InnerModelDraw::addPlane_notExisting(innerModelViewer, item2,transf,QVec::vec3(0,0,100),QVec::vec3(1,0,0),"#FF00AA",QVec::vec3(200, 20, 20));
}
InnerModelDraw::addTransform(innerModelViewer,"redTransform","floor");
InnerModelDraw::addPlane_notExisting(innerModelViewer,"red","redTransform",QVec::vec3(0,0,0),QVec::vec3(1,0,0),"#AA0000",QVec::vec3(200, 1000, 200));
InnerModelDraw::addPlane_notExisting(innerModelViewer, "orientacion","redTransform",QVec::vec3(0,500,200),QVec::vec3(1,0,0),"#00FF00",QVec::vec3(200, 50, 50));
//Draw Laser
int contador=0;
RoboCompLaser::TLaserData laserData;
laserData = laser_proxy->getLaserData();
for(uint i=0;i<laserData.size();i++)
{
// if(contador>=100 and contador<=668)
{
const QString item = QString::fromStdString("laserPoint_")+QString::number(i);
const QString transf = QString::fromStdString("laserPointTransf_")+QString::number(i);
InnerModelDraw::addTransform(innerModelViewer,transf,"redTransform");
InnerModelDraw::addPlane_notExisting(innerModelViewer, item,transf,QVec::vec3(0,0,0),QVec::vec3(0,1,0),"#FFFFFF",QVec::vec3(50, 50, 50));
}
contador++;
}
}
void SpecificWorker::compute()
{
const float limite = 400;
float velocidadAvance = 5
float velocidadRotacion = 0.5;
try
{
RoboCompLaser::TLaserData ldata = laser_proxy->getLaserData(); //read laser data
std::sort( ldata.begin()+10, ldata.end()-10, [](RoboCompLaser::TData a, RoboCompLaser::TData b){ return a.dist < b.dist; }) ; //sort laser data from small to large distances using a lambda function.
if( (ldata.data()+10)->dist < limite)
{
//Rota
differentialrobot_proxy->setSpeedBase(getSpeedAdv((ldata.data()+10)->angle,ldata.data()+10)->dist);
}
else
{
//Avanza
differentialrobot_proxy->setSpeedBase(250, 0);
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
/**
* @brief Computes de numerical derivative of the laser force field wrt to the point, by perturbing it locally.
* @param innermodel
* @param ball, point of the trajectory to be analyzed
* @param laserData
* @param forceDistanceLimit, max effective action of the force field.
*/
void
ElasticBand::computeDistanceField(InnerModel *innermodel, WayPoint &ball, const RoboCompLaser::TLaserData &laserData,
float forceDistanceLimit)
{
ball.minDist = ball.bMinusX = ball.bPlusX = ball.bMinusY = ball.bPlusY = std::numeric_limits<float>::max();
ball.minDistHasChanged = false;
QVec c = ball.pos;
c[1] = innermodel->getNode("laser")->getTr()[1]; //Put the y coordinate to laser height so norm() works allright
int index = -1;
for (uint i = 0; i < laserData.size(); i++)
{
QVec l = innermodel->laserTo("world", "laser", laserData[i].dist, laserData[i].angle);
float dist = (l - c).norm2();
if (dist < ball.minDist)
{
ball.minDist = dist;
index = i;
}
float h = DELTA_H; //Delta
dist = (l - (c - QVec::vec3(h, 0, 0))).norm2();
if (dist < ball.bMinusX) ball.bMinusX = dist;
dist = (l - (c + QVec::vec3(h, 0, 0))).norm2();
if (dist < ball.bPlusX) ball.bPlusX = dist;
dist = (l - (c - QVec::vec3(0, 0, h))).norm2();
if (dist < ball.bMinusY) ball.bMinusY = dist;
dist = (l - (c + QVec::vec3(0, 0, h))).norm2();
if (dist < ball.bPlusY) ball.bPlusY = dist;
}
QVec lw = innermodel->laserTo("world", "laser", laserData[index].dist, laserData[index].angle);
ball.minDistPoint = (lw - c).normalize();
//correct minDist to take into account the size of the robot in the ball.minDistPoint direction. For now let's suposse it is a ball
ball.minDistPoint -= ROBOT_RADIUS;
if (ball.minDist < forceDistanceLimit)
{
if (fabs(ball.minDist - ball.minDistAnt) > 2) //mm
{
ball.minDistAnt = ball.minDist;
ball.minDistHasChanged = true;
}
}
else
ball.minDist = forceDistanceLimit;
}
void Worker::compute()
{
/// Clear laser measurement
for (int32_t i=0; i<LASER_SIZE; ++i)
{
(*laserDataW)[i].dist = maxLength;
}
/// Update InnerModel with joint information
if (updateJoint)
{
RoboCompJointMotor::MotorStateMap motorMap;
try
{
jointmotor->getAllMotorState(motorMap);
for (RoboCompJointMotor::MotorStateMap::iterator it=motorMap.begin(); it!=motorMap.end(); ++it)
{
innerModel->updateJointValue(it->first.c_str(), it->second.pos);
}
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to jointMotor: " << ex << endl;
}
}
else
{
printf("not using joint\n");
}
/// FOR EACH OF THE CONFIGURED PROXIES
// #pragma omp parallel for
for (uint r=0; r<rgbds.size(); ++r)
{
#ifdef STORE_POINTCLOUDS_AND_EXIT
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // PCL
#endif
if (rgbds[r].bus == true) /// If the proxy is a bus
{
/// FOR EACH OF THE CAMERAS OF THE BUS
RoboCompRGBDBus::CameraList clist;
clist.resize(1);
RoboCompRGBDBus::CameraParamsMap::iterator iter;
for (iter=rgbds[r].cameras.begin(); iter!=rgbds[r].cameras.end(); iter++)
{
if (iter->first == rgbds[r].id)
{
clist[0] = iter->first;
RoboCompRGBDBus::ImageMap images;
try
{
if (DECIMATION_LEVEL == 0)
rgbds[r].proxyRGBDBus->getImages(clist,images);
else
rgbds[r].proxyRGBDBus->getDecimatedImages(clist, DECIMATION_LEVEL, images);
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to rgbd: " << ex << endl;
continue;
}
/// Get the corresponding (stored) protocloud
RoboCompRGBDBus::PointCloud pointCloud = rgbds[r].protoPointClouds[clist[0]];
/// Multiply the protocloud by the depth
for (uint32_t pi=0; pi<pointCloud.size(); pi++)
{
pointCloud[pi].x *= images[iter->first].depthImage[pi];
pointCloud[pi].y *= images[iter->first].depthImage[pi];
pointCloud[pi].z = images[iter->first].depthImage[pi];
}
/// Inserts the resulting points in the virtual laser
RTMat TR = innerModel->getTransformationMatrix(base, QString::fromStdString(iter->first));
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points.resize(pointCloud.size());
#endif
for (uint32_t ioi=0; ioi<pointCloud.size(); ioi+=3)
{
if ((not isnan(images[iter->first].depthImage[ioi])) and images[iter->first].depthImage[ioi] > 10)
{
QVec p = (TR * QVec::vec4(pointCloud[ioi].x, pointCloud[ioi].y, pointCloud[ioi].z, 1)).fromHomogeneousCoordinates();
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points[ioi].x = p(0)/1000;
cloud->points[ioi].y = p(1)/1000;
cloud->points[ioi].z = -p(2)/1000;
#endif
if ( (p(1)>=minHeight and p(1)<=maxHeight) /* or (p(1)<minHeightNeg) */)
{
p(1) = 0;
float d = sqrt(p(0)*p(0) + p(2)*p(2));
if (d>maxLength) d = maxLength;
const float a = atan2(p(0), p(2));
const int32_t bin = angle2bin(a);
if (bin>=0 and bin<LASER_SIZE and (*laserDataW)[bin].dist > d)
{
if ( (*laserDataW)[bin].dist > d)
(*laserDataW)[bin].dist = d;
}
}
}
}
}
}
}
else /// If the proxy is a good old RGBD interface
{
RoboCompRGBD::PointSeq points;
try
{
rgbds[r].proxyRGBD->getXYZ(points, hState, bState);
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to rgbd: " << ex << endl;
continue;
}
RTMat TR = innerModel->getTransformationMatrix(base, QString::fromStdString(rgbds[r].id));
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points.resize(points.size());
#endif
// printf("%d\n", __LINE__);
uint32_t pw = 640;
uint32_t ph = 480;
uint32_t step = 13;
if (points.size() == 320*240) { pw=320; ph=240; step=11; }
if (points.size() == 160*120) { pw=160; ph=120; step=5; }
if (points.size() == 80*60) { pw=80; ph=60; step=3; }
for (uint32_t rr=0; rr<ph; rr+=step)
{
for (uint32_t cc=rr%5; cc<pw; cc+=2)
{
uint32_t ioi = rr*pw+cc;
if (ioi<points.size())
{
const QVec p = (TR * QVec::vec4(points[ioi].x, points[ioi].y, points[ioi].z, 1)).fromHomogeneousCoordinates();
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points[ioi].x = p(0)/1000;
cloud->points[ioi].y = p(1)/1000;
cloud->points[ioi].z = -p(2)/1000;
#endif
if ( (p(1)>=minHeight and p(1)<=maxHeight) or (p(1)<minHeightNeg) )
{
// p(1) = 0;
float d = sqrt(p(0)*p(0) + p(2)*p(2));
if (d>maxLength) d = maxLength;
const float a = atan2(p(0), p(2));
const int32_t bin = angle2bin(a);
if (bin>=0 and bin<LASER_SIZE and (*laserDataW)[bin].dist > d)
{
(*laserDataW)[bin].dist = d;
}
}
}
}
}
// printf("%d\n", __LINE__);
}
#ifdef STORE_POINTCLOUDS_AND_EXIT
writePCD(rgbds[r].id+".pcd", cloud);
#endif
}
#ifdef STORE_POINTCLOUDS_AND_EXIT
qFatal("done");
#endif
try
{
RoboCompLaser::TLaserData alData = laser->getLaserData();
for (uint i=0; i<alData.size(); i++)
{
if (i==alData.size()/2) printf("PC %d (%f _ %f)\n", i, alData[i].dist, alData[i].angle);
const QVec p = innerModel->laserTo(actualLaserID, actualLaserID, alData[i].dist, alData[i].angle);
if (i==alData.size()/2) p.print("en base");
if (i==alData.size()/2) printf("(%s)", base.toStdString().c_str());
const float angle = atan2(p(0), p(2));
const float dist = p.norm2();
if (i==alData.size()/2) printf("enlaser %f %f\n", dist, angle);
const int j = LASER_SIZE*angle/FOV + (LASER_SIZE/2);
if (i==alData.size()/2) printf("index %d\n", j);
// printf("FOV:%f, angle:%f, LASER_SIZE=%f, j:%d\n", (float)FOV, angle, (float)LASER_SIZE, j);
if (j>=0 and j<(int)laserDataW->size())
{
if ((*laserDataW)[j].dist > dist)
{
(*laserDataW)[j].dist = dist;
}
}
}
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to laser: " << ex << endl;
}
RoboCompOmniRobot::TBaseState oState;
try { omnirobot->getBaseState(oState); }
catch (Ice::Exception e) { qDebug()<<"error talking to base"<<e.what(); }
bStateOut.x = oState.x;
bStateOut.z = oState.z;
bStateOut.alpha = oState.alpha;
// Double buffer swap
RoboCompLaser::TLaserData *t = laserDataR;
mutex->lock();
laserDataR = laserDataW;
mutex->unlock();
innerModel->updateTransformValues("robot", bStateOut.x, 0, bStateOut.z, 0, bStateOut.alpha,0);
// printf("%f %f ___ %f\n", bStateOut.x, bStateOut.z, bStateOut.alpha);
#ifdef USE_EXTENSION
extended->update(*laserDataR);
static QTime te = QTime::currentTime();
float re = 11.*te.elapsed()/1000.;
te = QTime::currentTime();
// printf("S ------------ %d %f\n", extended->size(), re);
extended->relax(re, innerModel, "laser", "root");
#endif
// medianFilter();
laserDataW = t;
}
void SpecificWorker::compute( )
{
static float rot = 0.1f; // rads/sec
static float adv = 100.f; // mm/sec
static float turnSwitch = 1;
const float advIncLow = 0.8; // mm/sec
const float advIncHigh = 2.f; // mm/sec
const float rotInc = 0.25; // rads/sec
const float rotMax = 0.4; // rads/sec
const float advMax = 200; // milimetres/sec
const float distThreshold = 500; // milimetres
static int frame = 0;
static double last_t = tic();
RoboCompCamera::imgType img;
if( INPUTIFACE == Camera)
{
//For cameras
try
{
camera_proxy->getYImage(0,img, cState, bState);
memcpy(image_gray.data, &img[0], m_width*m_height*sizeof(uchar));
searchTags(image_gray);
}
catch(const Ice::Exception &e)
{
std::cout << "Error reading from Camera" << e << std::endl;
}
}
else if( INPUTIFACE == RGBD)
{
try
{
//For RGBD
RoboCompRGBD::ColorSeq colorseq;
RoboCompRGBD::DepthSeq depthseq;
rgbd_proxy->getRGB(colorseq, hState, bState);
memcpy(image_color.data , &colorseq[0], m_width*m_height*3);
cv::cvtColor(image_color, image_gray, CV_RGB2GRAY);
searchTags(image_gray);
}
catch(const Ice::Exception &e)
{
std::cout << "Error reading form RGBD " << e << std::endl;
}
}
else if( INPUTIFACE == RGBDBus)
{
qFatal("Support for RGBDBus is not implemented yet!");
}
else
qFatal("Input device not defined. Please specify one in the config file");
/*
vector< ::AprilTags::TagDetection> detections = m_tagDetector->extractTags(image_gray);
// print out each detection
cout << detections.size() << " tags detected:" << endl;
print_detection(detections);
//
if (m_draw)
{
for (uint i=0; i<detections.size(); i++)
{
detections[i].draw(image_gray);
}
//imshow("AprilTags", image_gray); // OpenCV call
}*/
// print out the frame rate at which image frames are being processed
frame++;
if (frame % 10 == 0)
{
double t = tic();
cout << " " << 10./(t-last_t) << " fps" << endl;
last_t = t;
}
try
{
RoboCompLaser::TLaserData ldata = laser_proxy->getLaserData();
std::sort( ldata.begin(), ldata.end(), [](RoboCompLaser::TData a, RoboCompLaser::TData b){ return a.dist < b.dist; }) ;
if( ldata.front().dist < distThreshold)
{
adv = adv * advIncLow;
rot = rot + turnSwitch * rotInc;
if( rot < -rotMax) rot = -rotMax;
if( rot > rotMax) rot = rotMax;
differentialrobot_proxy->setSpeedBase(adv, rot);
}
else
{
adv = adv * advIncHigh;
if( adv > advMax) adv = advMax;
rot = 0.f;
differentialrobot_proxy->setSpeedBase(adv, 0.f);
turnSwitch = -turnSwitch;
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
void SpecificWorker::compute()
{
RoboCompDifferentialRobot::TBaseState bState;
differentialrobot_proxy->getBaseState ( bState );
RoboCompLaser::TLaserData datosLaser;
datosLaser= laser_proxy->getLaserData();//Obtenemos datos del Laser
inner->updateTransformValues ( "robot",bState.x,0,bState.z,0,bState.alpha,0 );
// datosLaser= laser_proxy->getLaserData();//Obtenemos datos del Laser
std::sort ( datosLaser.begin() +20, datosLaser.end()-20,[] ( auto a, auto b )
{
return a.dist< b.dist;
} ); //Ordenamos las distancias del frente
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
float vRot = atan2 ( tR.x(),tR.z() ); //devuelve radianes del angulo q forma donde apunta el robot con el punto destino.
//Refresca el estado
if ( target.isEmpty() == false && target.haCambiado())
{
//Calcular punto inicial,punto final,y trayectoria entre ellos
// parIni = //Falta el punto inicial
parxz = target.get();//Punto final
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
estado= Estado::AVANZANDO;
target.setCambiado(false);
}
switch ( estado )
{
case Estado::PARADO:
qDebug() << "PARADO";
if ( target.isEmpty() == false)
{
//Calcular punto inicial,punto final,y trayectoria entre ellos
// parIni = //Falta el punto inicial
parxz = target.get();//Punto final
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
estado= Estado::AVANZANDO;
target.setCambiado(false);
}
break;
case Estado::AVANZANDO:
qDebug() << "AVANZANDO";
if ( datosLaser[20].dist < UMBRAL ) //Si hay obstaculo
{
//Pasa a estado de Giro
estado = Estado::GIRANDO;
break ;
}
if ( d > 50 )
{
//Si no ha llegado
float velAvance = d;// version antigua
if ( vRot > MAX_ROT )
{
vRot = MAX_ROT;
}
velAvance=MAX_ADV*sigmoide ( d ) *gauss ( vRot,0.3,0.5 );
differentialrobot_proxy->setSpeedBase ( velAvance,vRot );
}
else
{
//Si ha llegado al sitio
estado = Estado::LLEGADO;
}
break;
case Estado::GIRANDO:
qDebug() << "GIRANDO";
if ( datosLaser[20].dist>UMBRAL )
{
estado=Estado::BORDEANDO;
}
else
{
//float d= rand()%1000000+100000;
differentialrobot_proxy->setSpeedBase ( 0,0.75 );
//usleep(d);
}
break;
case Estado::BORDEANDO:
qDebug() << "BORDEANDO";
if ( datosLaser[20].dist>UMBRAL && ( vRot<ANGULO_VISION && vRot > -(ANGULO_VISION) )) //Que no haya obstaculos en el frente y vaya hacia el objetivo
{
estado=Estado::AVANZANDO;
}
if ( datosLaser[20].dist<UMBRAL ) //Si hay dos obstaculos en L, debe volver a girar para no tener obstaculo en frente
{
estado=Estado::GIRANDO;
}
if ( d < 280 )
{
estado=Estado::LLEGADO;
}
//Condicion 2 para llegar al objetivo
//if (
//Ordenar los 20 primeros
std::sort ( datosLaser.end()-19, datosLaser.end()-10,[] ( auto a, auto b )
{
return a.dist< b.dist;
} ); //Valores de la izda
//Evaluamos distancia izda
//qDebug() << "Distancia:" <<datosLaser[81].dist;
if ( datosLaser[81].dist < 400 ) //Que la distancia izda sea esa
{
//Avanzar
differentialrobot_proxy->setSpeedBase ( 100,0 );
}
else
{
differentialrobot_proxy->setSpeedBase ( 0,-0.75 );
}
break;
case Estado::LLEGADO:
qDebug() << "LLEGADO";
differentialrobot_proxy->setSpeedBase ( 0,0 );
target.setEmpty();
estado=Estado::PARADO;
break;
}
}
