bool updateModule() { Vector *imuData=iPort.read(); if (imuData==NULL) return false; Stamp stamp; iPort.getEnvelope(stamp); double t0=Time::now(); Vector gyro=imuData->subVector(6,8); Vector gyro_filt=gyroFilt.filt(gyro); gyro-=gyroBias; gyro_filt-=gyroBias; Vector mag_filt=magFilt.filt(imuData->subVector(9,11)); double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime()))); adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down); gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0)); double dt=Time::now()-t0; if (oPort.getOutputCount()>0) { Vector &outData=oPort.prepare(); outData=*imuData; outData.setSubvector(6,gyro); oPort.setEnvelope(stamp); oPort.write(); } if (bPort.getOutputCount()>0) { bPort.prepare()=gyroBias; bPort.setEnvelope(stamp); bPort.write(); } if (verbose) { yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption"); yInfo("gyro = %s",gyro.toString(3,3).c_str()); yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str()); yInfo("dt = %.0f [us]",dt*1e6); yInfo("\n"); } return true; }
bool updateModule() { if (imgOutPort.getOutputCount()>0) { if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false)) { Vector xa,oa; iarm->getPose(xa,oa); Matrix Ha=axis2dcm(oa); Ha.setSubcol(xa,0,3); Vector v(4,0.0); v[3]=1.0; Vector c=Ha*v; v=0.0; v[0]=0.05; v[3]=1.0; Vector x=Ha*v; v=0.0; v[1]=0.05; v[3]=1.0; Vector y=Ha*v; v=0.0; v[2]=0.05; v[3]=1.0; Vector z=Ha*v; v=solution; v.push_back(1.0); Vector t=Ha*v; Vector pc,px,py,pz,pt; int camSel=(eye=="left")?0:1; igaze->get2DPixel(camSel,c,pc); igaze->get2DPixel(camSel,x,px); igaze->get2DPixel(camSel,y,py); igaze->get2DPixel(camSel,z,pz); igaze->get2DPixel(camSel,t,pt); CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]); CvPoint point_x=cvPoint((int)px[0],(int)px[1]); CvPoint point_y=cvPoint((int)py[0],(int)py[1]); CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]); CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]); cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4); cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4); cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2); cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2); cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2); cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2); tip.clear(); tip.addInt(point_t.x); tip.addInt(point_t.y); imgOutPort.prepare()=*pImgBgrIn; imgOutPort.write(); } } return true; }
bool read(ConnectionReader &connection) { Bottle data; data.read(connection); if ((data.size()>=2) && enabled) { Vector xa,oa; iarm->getPose(xa,oa); Vector xe,oe; if (eye=="left") igaze->getLeftEyePose(xe,oe); else igaze->getRightEyePose(xe,oe); Matrix Ha=axis2dcm(oa); Ha.setSubcol(xa,0,3); Matrix He=axis2dcm(oe); He.setSubcol(xe,0,3); Matrix H=Prj*SE3inv(He)*Ha; Vector p(2); p[0]=data.get(0).asDouble(); p[1]=data.get(1).asDouble(); if (logPort.getOutputCount()>0) { Vector &log=logPort.prepare(); log=p; for (int i=0; i<H.rows(); i++) log=cat(log,H.getRow(i)); for (int i=0; i<Prj.rows(); i++) log=cat(log,Prj.getRow(i)); for (int i=0; i<Ha.rows(); i++) log=cat(log,Ha.getRow(i)); for (int i=0; i<He.rows(); i++) log=cat(log,He.getRow(i)); logPort.write(); } mutex.wait(); solver.addItem(p,H); mutex.post(); } return true; }
bool updateModule() { ImageOf<PixelBgr> *img=imgInPort.read(); if (img==NULL) return true; mutex.lock(); ImageOf<PixelMono> imgMono; imgMono.resize(img->width(),img->height()); cv::Mat imgMat=cv::cvarrToMat((IplImage*)img->getIplImage()); cv::Mat imgMonoMat=cv::cvarrToMat((IplImage*)imgMono.getIplImage()); cv::cvtColor(imgMat,imgMonoMat,CV_BGR2GRAY); if (initBoundingBox) { tracker->initialise(imgMonoMat,tl,br); initBoundingBox=false; } if (doCMT) { if (tracker->processFrame(imgMonoMat)) { if (dataOutPort.getOutputCount()>0) { Bottle &data=dataOutPort.prepare(); data.clear(); data.addInt((int)tracker->topLeft.x); data.addInt((int)tracker->topLeft.y); data.addInt((int)tracker->topRight.x); data.addInt((int)tracker->topRight.y); data.addInt((int)tracker->bottomRight.x); data.addInt((int)tracker->bottomRight.y); data.addInt((int)tracker->bottomLeft.x); data.addInt((int)tracker->bottomLeft.y); dataOutPort.write(); } if (imgOutPort.getOutputCount()>0) { cv::line(imgMat,tracker->topLeft,tracker->topRight,cv::Scalar(255,0,0),2); cv::line(imgMat,tracker->topRight,tracker->bottomRight,cv::Scalar(255,0,0),2); cv::line(imgMat,tracker->bottomRight,tracker->bottomLeft,cv::Scalar(255,0,0),2); cv::line(imgMat,tracker->bottomLeft,tracker->topLeft,cv::Scalar(255,0,0),2); for (size_t i=0; i<tracker->trackedKeypoints.size(); i++) cv::circle(imgMat,tracker->trackedKeypoints[i].first.pt,3,cv::Scalar(0,255,0)); } } } if (imgOutPort.getOutputCount()>0) { imgOutPort.prepare()=*img; imgOutPort.write(); } mutex.unlock(); return true; }
virtual void run() { int i_touching_left=0; int i_touching_right=0; int i_touching_diff=0; info.update(); skinContactList *skinContacts = port_skin_contacts->read(false); if(skinContacts) { for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){ if(it->getBodyPart() == LEFT_ARM) i_touching_left += it->getActiveTaxels(); else if(it->getBodyPart() == RIGHT_ARM) i_touching_right += it->getActiveTaxels(); } } i_touching_diff=i_touching_left-i_touching_right; if (abs(i_touching_diff)<5) { fprintf(stdout,"nothing!\n"); } else if (i_touching_left>i_touching_right) { fprintf(stdout,"Touching left arm! \n"); if (!left_arm_master) change_master(); } else if (i_touching_right>i_touching_left) { fprintf(stdout,"Touching right arm! \n"); if (left_arm_master) change_master(); } if (left_arm_master) { robot->ienc[LEFT_ARM] ->getEncoders(encoders_master); robot->ienc[RIGHT_ARM]->getEncoders(encoders_slave); if (port_left_arm->getOutputCount()>0) { port_left_arm->prepare()= Vector(16,encoders_master); port_left_arm->setEnvelope(info); port_left_arm->write(); } if (port_right_arm->getOutputCount()>0) { port_right_arm->prepare()= Vector(16,encoders_slave); port_right_arm->setEnvelope(info); port_right_arm->write(); } // robot->ipos[RIGHT_ARM] ->positionMove(3,encoders_master[3]); for (int i=jjj; i<5; i++) { robot->ipid[RIGHT_ARM]->setReference(i,encoders_master[i]); } } else { robot->ienc[RIGHT_ARM]->getEncoders(encoders_master); robot->ienc[LEFT_ARM] ->getEncoders(encoders_slave); for (int i=jjj; i<5; i++) { robot->ipid[LEFT_ARM]->setReference(i,encoders_master[i]); } } }