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