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