/**
* \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;
}
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;
}
