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++;
	}
    }
}
Пример #2
0
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++;
	}
}
Пример #4
0
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;
}
Пример #6
0
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;
    }
}
Пример #8
0
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;

    }
}