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++;
	}
    }
}
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++;
	}
}
/**
 * @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;
}
示例#4
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;
}