C++ BufferedPort::read方法代码示例

    void run()
    {
		Bottle *input = port.read();
		if (input!=NULL)
		{
			if (first_run)
			{
			    for (int i=0; i<input->size(); i++)
				{
                    previous.resize(input->size());
                    current.resize(input->size());
                    diff.resize(input->size());
                    for (int i=0; i<input->size(); i++) current[i]=previous[i]=input->get(i).asDouble();
				}
				first_run=false;
			}

            bool print = false;
            for (int i=0; i<input->size(); i++)
			{
                previous[i]=current[i];
                current[i]=input->get(i).asDouble();
                diff[i]=current[i]-previous[i];

                tolerance = 10/100;
                double percent = fabs(diff[i]/current[i]);
                if (percent > tolerance) 
                    {
                        fprintf(stdout,"ch: %d percent +6.6%f\n", i , percent);
                        print = true;    
                    }
	        }

            if (print == true)
            {
                for (int i=0; i<input->size(); i++)
			    { 
                    fprintf(stdout,"+6.6%f  ",diff[i]);
                }
            }
			fprintf (stdout,"\n");
		}

		/*
		static double time_old_wd=Time::now();
		double time_wd=Time::now();
		fprintf(stdout,"time%+3.3f       ", time_wd-time_old_wd);
		cout << "       " << output.toString().c_str() << endl;
		time_old_wd=time_wd;
		*/
	}

static gboolean on_expose_event(gpointer data)
{


	Bottle* in_RA;
     	in_RA=jtsPort.read(true);


	cr = gdk_cairo_create(darea->window);
	cairo_set_source_rgb (cr, 1, 1, 1);
	cairo_paint (cr);
	cairo_set_source_rgb (cr, 1.0, 0.0, 0.0);
	cairo_set_line_width (cr, 5.0);

	cr2 = gdk_cairo_create(darea2->window);
	cairo_set_source_rgb (cr2, 1, 1, 1);
	cairo_paint (cr2);
	cairo_set_source_rgb (cr2, 1.0, 0.0, 0.0);
	cairo_set_line_width (cr2, 5.0);

	cr3 = gdk_cairo_create(darea3->window);
	cairo_set_source_rgb (cr3, 1, 1, 1);
	cairo_paint (cr3);
	cairo_set_source_rgb (cr3, 1.0, 0.0, 0.0);
	cairo_set_line_width (cr3, 5.0);

     	if (in_RA!=NULL) {
		for(int i=0; i<3;i++){
			ordonnee_RA[i]=((*in_RA).get(i).asDouble());
			ordonnee_RA[i]/=15.0;
			ordonnee_RA[i]*=150;
		}
       	}
	for(int i=100;i>0;i--){
		curvetoplot[i]=curvetoplot[i-1];
		curvetoplot2[i]=curvetoplot2[i-1];
		curvetoplot3[i]=curvetoplot3[i-1];
	}
	curvetoplot[0]=ordonnee_RA[0];
	curvetoplot2[0]=ordonnee_RA[1];
	curvetoplot3[0]=ordonnee_RA[2];

	for ( i=0 ; i < intcurve-1 ; i++ )
	{
    		cairo_move_to(cr, i*6, -curvetoplot[i]+170); // échelle: +15/-15 600x300
    		cairo_line_to(cr, (i+1)*6, -curvetoplot[i+1]+170);
		cairo_move_to(cr2, i*6, -curvetoplot2[i]+170); // échelle: +15/-15 600x300
    		cairo_line_to(cr2, (i+1)*6, -curvetoplot2[i+1]+170);
		cairo_move_to(cr3, i*6, -curvetoplot3[i]+170); // échelle: +15/-15 600x300
    		cairo_line_to(cr3, (i+1)*6, -curvetoplot3[i+1]+170);
 	}
	cairo_stroke(cr);
	cairo_stroke(cr2);
	cairo_stroke(cr3);
	cairo_destroy(cr);
	cairo_destroy(cr2);
	cairo_destroy(cr3);
	return true;
}

    /**
    * The main loop. Receives localization data and stores it internally, so an external module can retrieve it 
    * using a Localization2DClient connected to this server. Two localization sources are currently implemented:
    * from a YARP port or using the tfClient/tfServer mechanism.
    * @return true if everything is ok. Otherwise returning false will terminate module execution.
    */
    virtual bool updateModule()
    {
        double current_time = yarp::os::Time::now();
        
        //print some stats every 10 seconds
        if (current_time - m_last_statistics_printed > 10.0)
        {
            printStats();
            m_last_statistics_printed = yarp::os::Time::now();
        }

        LockGuard lock(m_mutex);
        //receives localization data from odometry port if m_use_localization_from_odometry_port is enabled
        if (m_use_localization_from_odometry_port)
        {
            yarp::sig::Vector *loc = m_port_odometry_input.read(false);
            if (loc)
            {
                m_last_odometry_data_received = yarp::os::Time::now();
                m_localization_data.x = loc->data()[0];
                m_localization_data.y = loc->data()[1];
                m_localization_data.theta = loc->data()[2];
            }
            if (current_time - m_last_odometry_data_received > 0.1)
            {
                yWarning() << "No localization data received for more than 0.1s!";
            }
        }
        //receives localization data from a tf server if m_use_localization_from_tf is enabled
        else if (m_use_localization_from_tf)
        {
            yarp::sig::Vector iv;
            yarp::sig::Vector pose;
            iv.resize(6, 0.0);
            pose.resize(6, 0.0);
            bool r = m_iTf->transformPose(m_frame_robot_id, m_frame_map_id, iv, pose);
            if (r)
            {
                //data is formatted as follows: x, y, angle (in degrees)
                m_tf_data_received = yarp::os::Time::now();
                m_localization_data.x = pose[0];
                m_localization_data.y = pose[1];
                m_localization_data.theta = pose[5] * RAD2DEG;
            }
            if (current_time - m_tf_data_received > 0.1)
            {
                yWarning() << "No localization data received for more than 0.1s!";
            }
        }
        //if no localization data is available, the module cannot proceed.
        else
        {
            yWarning() << "Localization disabled";
            return false;
        }

        return true; 
    }