Example #1
0
    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;
    }
Example #2
0
    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;
    }
Example #3
0
    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;
    }
Example #4
0
    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;
    }
Example #5
0
    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]);
            }

        }
    }