/**
* \brief Thread method
*/
void Worker::updateInnerModel()
{
	QMutexLocker m(mutex);
	/// 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");
	}


	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;
	innerModel->updateTransformValues("robot", bStateOut.x, 0, bStateOut.z, 0, bStateOut.alpha, 0);
}
bool SpecificWorker::isInRestPosition(const RoboCompJointMotor::MotorStateMap &mMap)
{
	try
	{
		if (fabs(mMap.at("armY").pos-(0.0))>0.08)
		{
			printf("armY noRest %f\n", float(mMap.at("armY").pos));
			return false;
		}
		if (fabs(mMap.at("armX1").pos-(-1.34))>0.08)
		{
			printf("armX1 noRest %f\n", float(mMap.at("armX1").pos));
			return false;
		}
		if (fabs(mMap.at("armX2").pos-(2.5))>0.08)
		{
			printf("armX2 noRest %f\n", float(mMap.at("armX2").pos));
			return false;
		}
		if (fabs(mMap.at("wristX").pos-(0))>0.08)
		{
			printf("wristX noRest %f\n", float(mMap.at("wristX").pos));
			return false;
		}
	}
	catch(...)
	{
		qFatal("Can't query motor state!");
	}
	return true;
}
示例#3
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;
}