C++ os::BufferedPort类代码示例

template <class T> void fbe_broadcastData(T& _values, yarp::os::BufferedPort<T>& _port)
{
    if (_port.getOutputCount()>0 )
    {
        _port.prepare()  = _values ;
        _port.write();
    }
}

 yarp::os::Bottle BlobMatch::getYarpBottle(yarp::os::BufferedPort<yarp::os::Bottle>& outport) {
     yarp::os::Bottle& result = outport.prepare();
     result.clear();
     
     result.addString("colour");
     result.addString(color.c_str());
     result.addDouble(pt.x);
     result.addDouble(pt.y);
     result.addInt(size);
     outport.writeStrict();
     return result;
 }

    bool configure(ResourceFinder & rf){

        string moduleName;
        string inPortName;
        string outPortName;

        moduleName =  rf.check("name", Value("FOEFinder"), "module name (String)").asString();
        setName(moduleName.c_str());

        //open input BufferedPort
        inPortName = "/";
        inPortName += getName();
        inPortName += "/vels:i";
        if (!vGrabber.open(inPortName.c_str())){
            cerr << getName() << ": Sorry. Unable to open input port" << inPortName << endl;
            return false;
        }
        vGrabber.useCallback();

        //open output Port
        outPortName = "/";
        outPortName += getName();
        outPortName += "/FOEMap:o";
        if (!outPort.open(outPortName.c_str()) ){
            cerr << getName() << "" << outPortName << endl;
            return false;
        }

        foeFinder.setOutPort(&outPort);

        inPortName = "/";
        inPortName += getName();
        inPortName += "/handler";
        handlerPort.open(inPortName.c_str());
        attach(handlerPort);

        ts = 0;
        return true;
    }

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

    virtual bool on_timeout()
    {
        char cmd[] = "lynx -auth=admin:password -dump \" http://10.0.0.250/cgi-bin/cgi?req=frm&frm=info.html&rand=1616217005\" | grep -i status";
        FILE* pipe =popen(cmd,"r");
        if (!pipe) return "ERROR";
        char buffer[128];
        string result = "";
        while(!feof(pipe)) {
    	if(fgets(buffer, 128, pipe) != NULL)
    		result += buffer;
        }
        pclose(pipe);
        
        static int signal=0;
        char tmp[20];
        static double strenght=0.0;

        sscanf(result.c_str(),"%*s %*s %d %*s %s",&signal,tmp );
        strenght = atof (tmp+1);
   
        Bottle& outBot = monitorOutput.prepare();
        outBot.clear();
        outBot.addInt(signal);    
        outBot.addDouble(strenght);
        monitorOutput.write();

        graphics->update_graphics(signal,strenght);

        return true;
    }

bool yarp_init(yarp::os::Property &cmd) {
    std::cout << "Setting up YARP... ";

    Network yarp;    
	
    // check for configuration file
    Property file;

	// the following values are read from the configuration file, but can be overwritten from the command line
	std::string robot = "MoBeE"; // default is simulator

	// open left end effector port
	std::string portname_r, portname_l, part;	// whether or not to use the left hand (default is right)

	// a port for sending commands to MoBeE right arm
	part = "right_arm";
    portname_l = "/SDLCtrlr/" + part + "/" + "cmd:o";	    
    portname_r = "/" + robot + "/" + part + "/" + "cmd:i";	// the name of the RPC server	
    right_hand.open(portname_l.c_str());
    yarp::os::Network::connect(portname_l.c_str(), portname_r.c_str());    

	// a port for sending commands to MoBeE left arm
    part = "left_arm";
    portname_l = "/SDLCtrlr/" + part + "/" + "cmd:o";	    
    portname_r = "/" + robot + "/" + part + "/" + "cmd:i";	// the name of the RPC server	
    left_hand.open(portname_l.c_str());
    yarp::os::Network::connect(portname_l.c_str(), portname_r.c_str());    
    
    // setup the grasp ports
    robot = "icub";
    // the name of the RPC port used by the core module
    portname_r = "/" + robot + "/" + part + "/" + "grasp";
    portname_l = "/SDLCtrlr/" + part + "/" + "grasp:o";	    
    left_grasp_port.open(portname_l.c_str());
    yarp::os::Network::connect(portname_l.c_str(), portname_r.c_str());    

	part = "right_arm";
    // the name of the RPC port used by the core module
    portname_r = "/" + robot + "/" + part + "/" + "grasp";
    portname_l = "/SDLCtrlr/" + part + "/" + "grasp:o";	    
    right_grasp_port.open(portname_l.c_str());
    yarp::os::Network::connect(portname_l.c_str(), portname_r.c_str());        
    
    // Finished reading configuration file
    std::cout << "\tdone!" << std::endl << std::endl;
    
    return true;
}

    virtual bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        //check if the yarp networ is running
        if (yarp.checkNetwork()==false)
        {
            return false;
        }

        moduleName = rf.check("name", Value(1), "module name (string)").asString();
        setName(moduleName.c_str());
        
        period = rf.check("period", Value(5000), "update period (int)").asInt();

        string pname  = "/" + moduleName + ":o";
        monitorOutput.open(pname.c_str());
         
        picBlocks = rf.findFile(rf.check("pic_blocks", Value(1), "module name (string)").asString());
        picBackground = rf.findFile(rf.check("pic_background", Value(1), "module name (string)").asString());
        picNumbers = rf.findFile(rf.check("pic_numbers", Value(1), "module name (string)").asString());

        graphics = new GraphicsManager(picBackground.c_str(),picBlocks.c_str(),picNumbers.c_str());
        m_timer = Glib::signal_timeout().connect(sigc::mem_fun(*this, &CtrlModule::on_timeout), period);
        on_timeout();

        //start GTK loop
        gtk_main->run(*graphics);

        return true;
    }

 bool interruptModule(){
     cout << "Interrupting.." << endl;
     vGrabber.interrupt();
     outPort.interrupt();
     handlerPort.interrupt();
     return true;
 }

 bool close(){
     cout << "closing .." << endl;
     vGrabber.close();
     outPort.close();
     handlerPort.close();
     return true;
 }

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;
}

 virtual bool close()
 {
     monitorOutput.close();
     m_timer.disconnect();
     fprintf(stdout,"done...\n");
     yarp::os::exit(1);        
     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; 
    }

 virtual bool interruptModule()
 {
     gtk_main->quit();
     monitorOutput.interrupt();
     if (graphics) delete graphics;
     delete gtk_main;
     close();
     return true;
 }

void yarp_cleanup() {
    right_hand.interrupt();
    left_hand.interrupt();
    right_grasp_port.interrupt();
    left_grasp_port.interrupt();

    right_hand.close();
    left_hand.close();
    right_grasp_port.close();
    left_grasp_port.close();


    // Finished reading configuration file
    std::cout << "YARP clean-up... \tdone!" << std::endl;
}

    /**
    * Performs module configuration, parsing user options stored in the resource finder.
    * Available options are described in main module documentation.
    * @return true if the module was successfully configured and opened, false otherwise.
    */
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        m_rpcPort.open("/"+m_module_name+"/rpc");
        attach(m_rpcPort);
        //attachTerminal();
        m_rf = rf;
        
        //configuration file checking
        Bottle general_group = m_rf.findGroup("GENERAL");
        if (general_group.isNull())
        {
            yError() << "Missing GENERAL group!";
            return false;
        }

        Bottle initial_group = m_rf.findGroup("INITIAL_POS");
        if (initial_group.isNull())
        {
            yError() << "Missing INITIAL_POS group!";
            return false;
        }

        Bottle localization_group = m_rf.findGroup("LOCALIZATION");
        if (localization_group.isNull())
        {
            yError() << "Missing LOCALIZATION group!";
            return false;
        }

        Bottle ros_group = m_rf.findGroup("ROS");

        Bottle tf_group = m_rf.findGroup("TF");
        if (tf_group.isNull())
        {
            yError() << "Missing TF group!";
            return false;
        }

        Bottle odometry_group = m_rf.findGroup("ODOMETRY");
        if (odometry_group.isNull())
        {
            yError() << "Missing ODOMETRY group!";
            return false;
        }

        Bottle map_group = m_rf.findGroup("MAP");
        if (map_group.isNull())
        {
            yError() << "Missing MAP group!";
            return false;
        }
        yDebug() << map_group.toString();

        //general group
        if (general_group.check("module_name") == false)
        {
            yError() << "Missing `module_name` in [GENERAL] group";
            return false;
        }
        m_module_name = general_group.find("module_name").asString();

        if (general_group.check("enable_ros") == false)
        {
            yError() << "Missing `ros_enable` in [GENERAL] group";
            return false;
        }
        m_ros_enabled = (general_group.find("enable_ros").asInt()==1);

        //ROS group
        if (m_ros_enabled)
        {
            m_rosNode = new yarp::os::Node("/"+m_module_name);
            
            //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
            if (ros_group.check ("occupancygrid_topic"))
            {
                m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
                if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
                {
                     if (m_rosNode)
                     {
                         delete m_rosNode;
                         m_rosNode=0;
                     }
                     yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
                     return false;
                }
            }

             //initialize an initial pose publisher
            if (ros_group.check ("initialpose_topic")) 
            {
                m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
//.........这里部分代码省略.........