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