C++ Stamp类代码示例

// BEGIN KAWIGIEDIT TESTING
// Generated by KawigiEdit 2.1.4 (beta) modified by pivanof
bool KawigiEdit_RunTest(int testNum, string p0, int p1, int p2, bool hasAnswer, int p3) {
    cout << "Test " << testNum << ": [" << "\"" << p0 << "\"" << "," << p1 << "," << p2;
    cout << "]" << endl;
    Stamp *obj;
    int answer;
    obj = new Stamp();
    clock_t startTime = clock();
    answer = obj->getMinimumCost(p0, p1, p2);
    clock_t endTime = clock();
    delete obj;
    bool res;
    res = true;
    cout << "Time: " << double(endTime - startTime) / CLOCKS_PER_SEC << " seconds" << endl;
    if (hasAnswer) {
        cout << "Desired answer:" << endl;
        cout << "\t" << p3 << endl;
    }
    cout << "Your answer:" << endl;
    cout << "\t" << answer << endl;
    if (hasAnswer) {
        res = answer == p3;
    }
    if (!res) {
        cout << "DOESN'T MATCH!!!!" << endl;
    } else if (double(endTime - startTime) / CLOCKS_PER_SEC >= 2) {
        cout << "FAIL the timeout" << endl;
        res = false;
    } else if (hasAnswer) {
        cout << "Match :-)" << endl;
    } else {
        cout << "OK, but is it right?" << endl;
    }
    cout << "" << endl;
    return res;
}

    void onRead(T &obj) override
    {
        if (++cnt==dwnsample)
        {
            if (firstIncomingData)
            {
                yInfo() << "Incoming data detected";
                firstIncomingData=false;
            }

            DumpItem item;
            Stamp info;

            BufferedPort<T>::getEnvelope(info);
            item.seqNumber=info.getCount();

            if (txTime || (info.isValid() && !rxTime))
                item.timeStamp.setTxStamp(info.getTime());

            if (rxTime || !info.isValid())
                item.timeStamp.setRxStamp(Time::now());

            item.obj=factory(obj);

            buf.lock();
            buf.push_back(item);
            buf.unlock();

            cnt=0;
        }
    }

void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {	
	mutex.wait();
		
	if(stamp.getTime() == 0) {
		std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
		stamp.update();
	}
	
	// store the current (start) indices in the circular buffer
	int tIdx = torsoIdx;
	int hIdx = headIdx;
	
	// helpers
	int idxH = -1, idxT = -1;
	int hH, hT;
	double smallestDiffH = 10000;
	double smallestDiffT = 10000;	
	double tDiff;

	// the bottles to be copied out of the circ buffer
	Bottle head, torso;
	
	// going through the circular buffer
    idxH = 1;
    idxT = 1;
//	for(int k = 0; k < LIST_LENGTH; k++) {
//		hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;	
//		hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//		
//		tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
//		std::cout << stamp.getTime() << " idx: " << hT <<  " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;		
//		if( tDiff < smallestDiffT ) {
//			idxT = hT;				
//			smallestDiffT = tDiff;
//		}
//
//		tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
//		if( tDiff < smallestDiffH ) { 
//			idxH = hH;		
//			smallestDiffH = tDiff;
//		}
//	}
	
	if( idxH != -1 ) head = headState[idxH];	
	if( idxT != -1 ) torso = torsoState[idxT];
	std::cout <<  "idxT: " << idxT << "/" << idxT-tIdx <<  " val: " << torso.toString() <<  std::endl;			
	std::cout <<  "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() <<  std::endl;				
	
	for(int i = 0; i < 6; i++) {
		headjnt_pos[i] = head.get(i).asDouble();
		if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
	}
	
	mutex.post();
}

void InputPortProcessor::onRead(yarp::sig::Vector &v)
{
    now=Time::now();
    mutex.wait();

    if (count>0)
    {
        double tmpDT=now-prev;
        deltaT+=tmpDT;
        if (tmpDT>deltaTMax)
            deltaTMax=tmpDT;
        if (tmpDT<deltaTMin)
            deltaTMin=tmpDT;

        //compare network time
        if (tmpDT*1000<ANALOG_TIMEOUT)
        {
            state=IAnalogSensor::AS_OK;
        }
        else
        {
            state=IAnalogSensor::AS_TIMEOUT;
        }
    }

    prev=now;
    count++;

    lastVector=v;
    Stamp newStamp;
    getEnvelope(newStamp);

    //initialialization (first received data)
    if (lastStamp.isValid()==false)
    {
        lastStamp = newStamp;
    }

    //now compare timestamps
    if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
    {
        state=IAnalogSensor::AS_OK;
    }
    else
    {
        state=IAnalogSensor::AS_TIMEOUT;
    }
    lastStamp = newStamp;

    mutex.post();
}

void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
    now=SystemClock::nowSystem();
    mutex.wait();

    if (count>0)
    {
        double tmpDT=now-prev;
        deltaT+=tmpDT;
        if (tmpDT>deltaTMax)
            deltaTMax=tmpDT;
        if (tmpDT<deltaTMin)
            deltaTMin=tmpDT;

        //compare network time
        if (tmpDT*1000<LASER_TIMEOUT)
        {
            state = b.get(1).asInt();
        }
        else
        {
            state = IRangefinder2D::DEVICE_TIMEOUT;
        }
    }

    prev=now;
    count++;

    lastBottle=b;
    Stamp newStamp;
    getEnvelope(newStamp);

    //initialialization (first received data)
    if (lastStamp.isValid()==false)
    {
        lastStamp = newStamp;
    }

    //now compare timestamps
    if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
    {
        state = b.get(1).asInt();
    }
    else
    {
        state = IRangefinder2D::DEVICE_TIMEOUT;
    }
    lastStamp = newStamp;

    mutex.post();
}

    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()
    {
        LockGuard lg(mutex);

        double dumpTime=Time::now();
        Bottle &bDump=dumpPort.prepare();
        bDump.clear();

        bDump.addString(actionTag);
        bDump.addString(objectTag);

        opc.checkout();        

        // agent body + position
        agentName = getPartnerName();
        if (Entity *e=opc.getEntity(agentName))
        {
            if (Agent *agent=dynamic_cast<Agent*>(e))
            {
                bDump.addList()=agent->m_body.asBottle();
                Bottle &bAgent=bDump.addList();
                bAgent.addString(agent->name());
                bAgent.addDouble(agent->m_ego_position[0]);
                bAgent.addDouble(agent->m_ego_position[1]);
                bAgent.addDouble(agent->m_ego_position[2]);
                bAgent.addInt(agent->m_present==1.0?1:0);
            }
        }

        // objects position
        list<Entity*> lEntity=opc.EntitiesCache();
        for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
        {
            string entityName=(*itEnt)->name();
            string entityType=(*itEnt)->entity_type();
            if (entityType==EFAA_OPC_ENTITY_OBJECT)
            {
                if (Object *object=dynamic_cast<Object*>(*itEnt))
                {
                    Bottle &bObject=bDump.addList();
                    bObject.addString(object->name());
                    bObject.addDouble(object->m_ego_position[0]);
                    bObject.addDouble(object->m_ego_position[1]);
                    bObject.addDouble(object->m_ego_position[2]);
                    bObject.addInt(object->m_present==1.0?1:0);
                }
            }
        }

        bDump.addInt(gate);        

        dumpStamp.update(dumpTime);
        dumpPort.setEnvelope(dumpStamp);
        dumpPort.writeStrict();

        yInfo()<<bDump.toString();

        return true;
    }

 ~Statistic()
 {
   const Debug::Stream Dbg("Core::RawScaner::Statistic");
   const Stamp spent = Timer.Elapsed();
   Dbg("Total processed: %1%", TotalData);
   Dbg("Time spent: %1%", Time::Duration<Stamp::ValueType, Stamp>(1, spent).ToString());
   const uint64_t useful = ArchivedData + ModulesData;
   Dbg("Useful detected: %1% (%2% archived + %3% modules)", useful, ArchivedData, ModulesData);
   Dbg("Coverage: %1%%%", useful * 100 / TotalData);
   Dbg("Speed: %1% b/s", spent.Get() ? (TotalData * Stamp::PER_SECOND / spent.Get()) : TotalData);
   StatisticBuilder<7> builder;
   builder.Add(MakeStatLine(), 0);
   StatItem total;
   total.Name = "Total";
   for (DetectMap::const_iterator it = Detection.begin(), lim = Detection.end(); it != lim; ++it)
   {
     builder.Add(MakeStatLine(it->second), 1 + it->second.Index);
     total += it->second;
   }
   builder.Add(MakeStatLine(total), 1 + Detection.size());
   Dbg(builder.Get().c_str());
 }

void SpriteSheet::ExportStampTiles(const PlatformConfig& config, const Stamp& referenceStamp, std::stringstream& stream, const std::string& actorName) const
{
#if defined _DEBUG
	ion::debug::Assert(referenceStamp.CheckTilesBatched(), "SpriteSheet::ExportStampTiles() - Tiles not in sequential order");
#endif

	//Get all unique tiles
	std::vector<TileId> tileIndices;
	int numUniqueTiles = referenceStamp.GetSortedUniqueTileBatch(tileIndices);

	//Offset by first index
	int firstIndex = tileIndices[0];

	int frameIdx = 0;
	for(std::vector<SpriteSheetFrame>::const_iterator it = m_frames.begin(), end = m_frames.end(); it != end; ++it, ++frameIdx)
	{
		std::stringstream label;
		label << "actor_" << actorName << "_sheet_" << m_name << "_frame_" << frameIdx;
		stream << label.str() << ":" << std::endl;

		//Export in sorted unique order
		for(int i = 0; i < tileIndices.size(); i++)
		{
			//Find index of first use of this tile id in reference stamp
			int tileIndex = referenceStamp.GetTileIndex(tileIndices[i]);

			//Transpose to sprite (column major) order
			ion::Vector2i pos(tileIndex / referenceStamp.GetWidth(), tileIndex % referenceStamp.GetWidth());
			int rowMajorIndex = (pos.y * referenceStamp.GetHeight()) + pos.x;

			//Export tile of animation frame
			(*it)[rowMajorIndex].Export(config, stream);

			stream << std::endl;
		}
	}
}

bool EdisonSegmModule::updateModule()
{
    ImageOf<PixelRgb> *yrpImgIn;
    static int cycles = 0;

    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;

    bool use_private_stamp;
    Stamp s;
    if(!_imgPort.getEnvelope(s))
    {
            cout << "No stamp found in input image. Will use private stamp" << endl;
            use_private_stamp = true;
    }
    else
    {
            cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
            use_private_stamp = false;
    }

    if(cycles == 0)
            _timestart = yarp::os::Time::now();
    cycles++;

    IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();

    //computing the ROI to crop the image
	/*struct _IplROI roi;
	roi.coi = 0; // all channels are selected
	roi.height = height_;
	roi.width = width_;
	roi.xOffset = ( orig_width_ - width_ ) / 2;
	roi.yOffset = ( orig_height_ - height_ ) / 2;*/
	
	//copying roi data to buffer
	/*iplimg->roi = &roi;
	cvCopy( iplimg, inputImage.getIplImage());*/

    //Rescale image if required
    if( (width_ != orig_width_) || (height_ != orig_height_ ) )
            cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
    else
            cvCopy( iplimg, inputImage.getIplImage());

    double edgetime = yarp::os::Time::now();
    //compute gradient and confidence maps
    BgEdgeDetect edgeDetector(gradWindRad);
    BgImage bgImage;
    bgImage.SetImage(inputImage_, width_, height_, true);
    edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
    //compute the weigth map
    for(int i = 0; i < width_*height_; i++) {
      if(gradMap_[i] > 0.02) {
		weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
      } else {
		weightMap_[i] = 0;
      }
    }
	///////////////////////////// This block can be parallelized
	cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;

	msImageProcessor iProc;
	if( dim_ == 3 )
		iProc.DefineImage(inputImage_, COLOR, height_, width_);
	else
	{	
		cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
		cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
		iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
	}
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	iProc.SetWeightMap(weightMap_, threshold);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}


	double filtertime = yarp::os::Time::now();
	iProc.Filter(sigmaS, sigmaR, speedup);
        if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
	cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;


    //obtain the filtered image
    iProc.GetResults(filtImage_);
	if(iProc.ErrorStatus) {
		cout << "MeanShift Error" << endl;
		return false;
	}
    
    //fuse regions
//.........这里部分代码省略.........

void stereoCalibThread::stereoCalibRun()
{

    imageL=new ImageOf<PixelRgb>;
    imageR=new ImageOf<PixelRgb>;

    Stamp TSLeft;
    Stamp TSRight;

    bool initL=false;
    bool initR=false;

    int count=1;
    Size boardSize, imageSize;
    boardSize.width=this->boardWidth;
    boardSize.height=this->boardHeight;


   while (!isStopping()) { 
        ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
        ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);

        if(tmpL!=NULL)
        {
            *imageL=*tmpL;
            imagePortInLeft.getEnvelope(TSLeft);
            initL=true;
        }
        if(tmpR!=NULL) 
        {
            *imageR=*tmpR;
            imagePortInRight.getEnvelope(TSRight);
            initR=true;
        }

        if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){

            bool foundL=false;
            bool foundR=false;

            mutex->wait();
            if(startCalibration>0) {

                string pathImg=imageDir;
                preparePath(pathImg.c_str(), pathL,pathR,count);
                string iml(pathL);
                string imr(pathR);
                imgL= (IplImage*) imageL->getIplImage();
                imgR= (IplImage*) imageR->getIplImage();
                Mat Left(imgL);
                Mat Right(imgR);


                std::vector<Point2f> pointbufL;
                std::vector<Point2f> pointbufR;
                if(boardType == "CIRCLES_GRID") {
                    foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID  | CALIB_CB_CLUSTERING);
                    foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID  | CALIB_CB_CLUSTERING);
                } else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
                    foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
                    foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
                } else {
                    foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                    foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
                }
                
                if(foundL && foundR) {
                        cvCvtColor(imgL,imgL,CV_RGB2BGR);
                        cvCvtColor(imgR,imgR, CV_RGB2BGR);
                        saveStereoImage(pathImg.c_str(),imgL,imgR,count);

                        imageListR.push_back(imr);
                        imageListL.push_back(iml);
                        imageListLR.push_back(iml);
                        imageListLR.push_back(imr);
                        Mat cL(pointbufL);
                        Mat cR(pointbufR);
                        drawChessboardCorners(Left, boardSize, cL, foundL);
                        drawChessboardCorners(Right, boardSize, cR, foundR);
                        count++;
                }

                if(count>numOfPairs) {
                    fprintf(stdout," Running Left Camera Calibration... \n");
                    monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);

                    fprintf(stdout," Running Right Camera Calibration... \n");
                    monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);

                    stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);

                    fprintf(stdout," Saving Calibration Results... \n");
                    updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
                    updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");

                    Mat Rot=Mat::eye(3,3,CV_64FC1);
                    Mat Tr=Mat::zeros(3,1,CV_64FC1);

                    updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");

//.........这里部分代码省略.........

void DataPlot::initSignalDimensions()
{
    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
    //getting info on the connected port
    nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
    for (int k = 0; k < numberOfInputPorts; k++)
        {
            Bottle *b = NULL;
            int timeout = 0;
            const int maxTimeout = 1000;
            while(b==NULL && timeout < maxTimeout) 
                {
                    b = inputPorts[k].read(false);
                    Time::delay(0.005);
                    timeout++;
                }
            if (timeout==maxTimeout)
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
                    realTime = false;
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }
            else
                {
                    if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
                    inputVectorSize = b->size();
                    Stamp stmp;
                    inputPorts[k].getEnvelope(stmp);
                    if (stmp.isValid())
                        {     
                            if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
                            realTime = true;
                            initialTime = stmp.getTime();
                        }
                
                    //  Initialize data
                    for (int j = 0; j< numberOfPlots[k]; j++)
                        {
                            if (b->size()-1 < index[k][j])
                                if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
                            for (int i = 0; i < PLOT_SIZE; i++)
                                {
                                    d_x[i] = i;     // time axis
#ifdef ENABLE_REALTIME
                                    if (realTime)
                                        d_x_real_time[i] = i;
#endif
                                    //if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
                                    d_y[k][j][i] = 0;
                                }
                        }
                }

            //if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
            // Assign a title
            insertLegend(new QwtLegend(), QwtPlot::BottomLegend);

            nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
            for(int i=0; i < numberOfPlots[k]; i++)
                {
                    //Set title
                    char cTitle[256];
                    sprintf(cTitle, "Data(%d)", index[k][i]);
                    QwtText curveTitle(cTitle, QwtText::PlainText);
                    curveTitle.setFont(plotFont);
                    nonTimeBasedCurve[k][i].setTitle(curveTitle);

                    //if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
                    // Insert new curves
                    nonTimeBasedCurve[k][i].attach(this);

                    // Set curve styles
                    nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);

                    // Attach (don't copy) data. Both curves use the same x array.
                    nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
                }

            // Axis 
            QwtText axisTitle("Time/seconds");
            axisTitle.setFont(plotFont);
            setAxisTitle(QwtPlot::xBottom, axisTitle);
            setAxisScale(QwtPlot::xBottom, 0, 100);
            setAxisFont(QwtPlot::xBottom, plotFont);

            setAxisTitle(QwtPlot::yLeft, "Values");
            //setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
            setAxisAutoScale(QwtPlot::yLeft);
            setAxisAutoScale(QwtPlot::xBottom);
            setAxisFont(QwtPlot::yLeft, plotFont);

//.........这里部分代码省略.........

void comBalancingThread::run()
{    
    // if(!Opt_display){
    
   
        //updating Time Stamp
        static Stamp timeStamp;
        timeStamp.update();
        
        //***************************** Reading F/T measurements and encoders ****************

        #ifdef FOR_PLOTS_ONLY 
                Vector* F_ext_RLt = port_ft_foot_right->read(true);
                Vector* F_ext_LLt = port_ft_foot_left->read(true);

                if (F_ext_LL==0) F_ext_LL= new Vector(6,0.0);
                if (F_ext_RL==0) F_ext_RL = new Vector(6,0.0);

                (*F_ext_LL) = -1.0*((*F_ext_LLt) - (Offset_Lfoot));
                (*F_ext_RL) = -1.0*((*F_ext_RLt) - (Offset_Rfoot));

                //Transformation from ankle to f/t sensor
                Matrix foot_hn(4,4); foot_hn.zero();
                foot_hn(0,2)=1; foot_hn(0,3)=-7.75;
                foot_hn(1,1)=-1;
                foot_hn(2,0)=-1;
                foot_hn(3,3)=1;           

                Vector tmp1, tmp2;
                tmp1 = (*F_ext_RL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_RL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_RL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_RL)[i] = tmp2[i-3];

                tmp1 = (*F_ext_LL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
                tmp2 = (*F_ext_LL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;

                for (int i=0; i<3; i++) (*F_ext_LL)[i] = tmp1[i];
                for (int i=3; i<6; i++) (*F_ext_LL)[i] = tmp2[i-3];

                //fprintf(stderr, "F_EXT_RL from module: %s\n", (*F_ext_RL).toString().c_str());

        #else
                if(Opt_ankles_sens)
                {
                    F_ext_RL = EEWRightAnkle->read(true);
                    F_ext_LL = EEWLeftAnkle->read(true);

                }
                else{
                    F_ext_RL = EEWRightLeg->read(true);
                    F_ext_LL = EEWLeftLeg->read(true);
                }   
        #endif      


        Ienc_RL->getEncoders(encs_r.data());
        Ienc_LL->getEncoders(encs_l.data());


        // check if the robot is not in contact with the ground
        static bool on_ground = true;
        //printf("%f %f \n", (*F_ext_LL)[0], (*F_ext_RL)[0]);
        if ((*F_ext_LL)[0] > 50 &&
            (*F_ext_RL)[0] > 50  )
            {
              on_ground = true;
            }
        else
            {
              on_ground = false;
              for (int jj=0; jj<6; jj++) (*F_ext_LL)[jj] =  (*F_ext_RL)[jj] = 1e-20;
              (*F_ext_LL)[0] =  (*F_ext_RL)[0] = 1e+20;
            }
        //printf("%s\n", (*F_ext_LL).toString().c_str());
        //gif ((*F_ext_LL)[3])
        //***************************** Computing ZMP ****************************************

        computeZMP(&zmp_xy, F_ext_LL, F_ext_RL, encs_l, encs_r);
        
        #ifdef VERBOSE
        fprintf(stderr, "ZMP coordinates: %f %f       %d \n",zmp_xy[0],zmp_xy[1],(int)(torso));
        fprintf(stderr, "ZMP desired: %f %f\n",zmp_des[0], zmp_des[1]);
        #endif

        //*********************** SENDING ZMP COORDINATES TO GuiBalancer *********************
        // if (objPort->getOutputCount() > 0)
        // {
            objPort->prepare() = zmp_xy;
            objPort->setEnvelope(timeStamp);
            objPort->write();
        // }
        
        //********************** SENDING ZMP COORDS TO BE PLOT BY ICUBGUI *****************************
        if (objPort2->getOutputCount() > 0)
        {
            Matrix Trans_lastRot(4,4); Trans_lastRot.zero();
            Trans_lastRot.setSubmatrix(rot_f,0,0);
            Trans_lastRot(3,3) = 1;
//.........这里部分代码省略.........

void velImpControlThread::run()
{
	double t_start = yarp::os::Time::now();


	if (getIterations()>100)
	{
		fprintf(stderr, "Thread ran %d times, est period %lf[ms], used %lf[ms]\n",
				getIterations(),
				getEstPeriod(),
				getEstUsed());
		resetStat();
	}
	_mutex.wait();

	////getting new commands from the fast command port
	//this command receives also feedforward velocities
	yarp::os::Bottle *bot = command_port.read(false);
	if(bot!=NULL)
	{
        nb_void_loops = 0;
		//fprintf(stderr, "\n Receiving command: \t");
		int size = bot->size()/2;
		for(int i=0;i<size;i++)
		{	
			int ind = bot->get(2*i).asInt();
			if(ind < VELOCITY_INDEX_OFFSET) 
			{//this is a position command
				targets(ind) = bot->get(2*i+1).asDouble();
			} 
			else 
			{//this is a velocity command
				ffVelocities(ind - VELOCITY_INDEX_OFFSET) = bot->get(2*i+1).asDouble();
			}
			//fprintf(stderr, "for joint *%d, received %f, \t", ind, targets(ind));
		}
		first_command++;
	} else {
        nb_void_loops++;
        if(nb_void_loops > 5) {
            ffVelocities = 0.0;
        }
    }

#if SWITCH    
    switchImp(ffVelocities[0]); //change stiffness according to shoulder/hips velocities
	compute_stiffness(requestedStiff, requestedDamp, currStiff, currDamp);
	//printf("0: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[0], requestedDamp[0], currStiff[0], currDamp[0]);
	//printf("1: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[1], requestedDamp[1], currStiff[1], currDamp[1]);
	//printf("2: %+3.5f %+3.5f %+3.5f %+3.5f *** ",requestedStiff[2], requestedDamp[2], currStiff[2], currDamp[2]);
	//printf("\n");
	if (impedance_enabled==true)
	{
		for(int i=0; i< nJoints; i++) iimp->setImpedance(i,  currStiff[i], currDamp[i]);
	}
#endif
	Bottle stiffness_output;
	Bottle damping_output;
	Bottle velocity_output;
    for(int i=0; i< nJoints; i++) 
	{
		stiffness_output.addDouble(currStiff[i]);
		damping_output.addDouble(currDamp[i]);
		velocity_output.addDouble(command[i]);
	}
	Stamp stmp;
	stmp = itime->getLastInputStamp();
	if (stmp.isValid())
	{
		stiffness_port.setEnvelope(stmp);
		damping_port.setEnvelope(stmp);
		velocity_port.setEnvelope(stmp);
	}
	else
	{
		stmp=Stamp(-1,0.0);
		stiffness_port.setEnvelope(stmp);
		damping_port.setEnvelope(stmp);
		velocity_port.setEnvelope(stmp);
	}
    velocity_port.write(velocity_output);
	stiffness_port.write(stiffness_output);
	damping_port.write(damping_output);

	//getting commands from the slow port
	yarp::sig::Vector *vec = command_port2.read(false);
	if (vec!=0)
	{
		targets=*vec;
		first_command++;
	}

	static int count=0;
	count++;

    // normale by randaz
	ienc->getEncoders(encoders.data());
    
    // versione che prende direttam la refernce del pid
    for(int i=0; i<nJoints; i++)
//.........这里部分代码省略.........

bool GBSegmModule::updateModule()
{
    ImageOf<PixelRgb> *yrpImgIn;
    static int cycles = 0;

    yrpImgIn = _imgPort.read();
    if (yrpImgIn == NULL)   // this is the case if module is requested to quit while waiting for image
        return true;

    bool use_private_stamp;
    Stamp s;
    if(!_imgPort.getEnvelope(s))
    {
            cout << "No stamp found in input image. Will use private stamp" << endl;
            use_private_stamp = true;
    }
    else
    {
            cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
            use_private_stamp = false;
    }

    if(cycles == 0)
            _timestart = yarp::os::Time::now();
    cycles++;

    //IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
 cout << "converting image of size " << yrpImgIn->width() << yrpImgIn->height() <<" to size" << input->width() << input->height() << endl;
    YarpImageToRGBImage(input, yrpImgIn);
    cout << "converted" << endl;
    segMutex.wait();
    if(seg)
        delete seg;
    seg=segment_image(input, sigma, k, min_size, &num_components);
    segMutex.post();
    
    cout << "processed" << endl;
    //prepare timestamps
    if(use_private_stamp)
    {
        _stamp.update();
        _viewPort.setEnvelope(_stamp);
    }
    else
    {
        _viewPort.setEnvelope(s);
    }



    ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
    //Rescale image if required

    yrpImgView.resize(seg->width(), seg->height());
    RGBImageToYarpImage(seg, &yrpImgView);
    _viewPort.write();


    //report the frame rate
    if(cycles % 100 == 0)
    {
            double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
            printf("fps: %02.2f\n", cps);
    }
    return true;
}