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