Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
    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;
    }
Exemplo n.º 3
0
    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;
    }
Exemplo n.º 4
0
    /**
    * 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();
                {
                    if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose))
                    {
                        if (m_rosNode)
                        {
                            delete m_rosNode;
                            m_rosNode=0;
                        }
                        yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration";
                        return false;
                    }
                }
            }
        }

        //localization group
        if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); }
        if (localization_group.check("use_localization_from_tf"))   { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); }

        if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true)
        {
            yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!";
            return false;
        }

        //map server group
        yDebug() << map_group.toString();

        if (map_group.check("connect_to_yarp_mapserver") == false)
        {
            yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group";
            return false;
        }
        m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1);

        //tf group
        if (tf_group.check("map_frame_id") == false)
        {
            yError() << "Missing `map_frame_id` in [TF] group";
            return false;
        }
        if (tf_group.check("robot_frame_id") == false)
        {
            yError() << "Missing `robot_frame_id` in [TF] group";
            return false;
        }
        m_frame_map_id = tf_group.find("map_frame_id").asString();
        m_frame_robot_id = tf_group.find("robot_frame_id").asString();

        //odometry group
        if (odometry_group.check("odometry_broadcast_port") == false)
        {
            yError() << "Missing `odometry_port` in [ODOMETRY] group";
            return false;
        }
        m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString();

        //device driver opening and/or connections
        if (m_use_localization_from_odometry_port)
        {
            //opens a YARP port to receive odometry data
            std::string odom_portname = "/" + m_module_name + "/odometry:i";
            bool b1 = m_port_odometry_input.open(odom_portname.c_str());
            bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false);
            bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str());
            if (b1 == false || b2 ==false || b3==false)
            {
                yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str();
                return false;
            }
        }

        if (m_use_localization_from_tf)
        {
            //opens a client to receive localization data from transformServer
            Property options;
            options.put("device", "transformClient");
            options.put("local", "/"+m_module_name + "/TfClient");
            options.put("remote", "/transformServer");
            if (m_ptf.open(options) == false)
            {
                yError() << "Unable to open transform client";
                return false;
            }
            m_ptf.view(m_iTf);
            if (m_ptf.isValid() == false || m_iTf == 0)
            {
                yError() << "Unable to view iTransform interface";
                return false;
            }
        }

        if (m_use_map_server)
        {
            //opens a client to send/received data from mapServer
            Property map_options;
            map_options.put("device", "map2DClient");
            map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name.
            map_options.put("remote", "/mapServer");
            if (m_pmap.open(map_options) == false)
            {
                yWarning() << "Unable to open mapClient";
            }
            else
            {
                yInfo() << "Opened mapClient";
                m_pmap.view(m_iMap);
                if (m_pmap.isValid() == false || m_iMap == 0)
                {
                    yError() << "Unable to view map interface";
                    return false;
                }
            }
        }

        //initial location initialization
        if (initial_group.check("initial_x"))     { m_initial_loc.x = initial_group.find("initial_x").asDouble(); }
        else { yError() << "missing initial_x param"; return false; }
        if (initial_group.check("initial_y"))     { m_initial_loc.y = initial_group.find("initial_y").asDouble(); }
        else { yError() << "missing initial_y param"; return false; }
        if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); }
        else { yError() << "missing initial_theta param"; return false; }
        if (initial_group.check("initial_map"))   { m_initial_loc.map_id = initial_group.find("initial_map").asString(); }
        else { yError() << "missing initial_map param"; return false; }
        this->initializeLocalization(m_initial_loc);

        return true;
    }
Exemplo n.º 5
0
int main (int argc, char *argv[])
{
 	Network yarp;
  	ResourceFinder rf;
	GtkWidget *pLabel, *pLabel2, *pLabel3;
	GtkWidget *pImage, *pImage2, *pImage3;
	GtkWidget *pHBox;
	GtkWidget *pHBox2;
	GtkWidget *pHBox3;
	GtkWidget *pVBox;

    /***********string guiName;***************/
	intcurve=100;
	curvetoplot=(float*)malloc(100*sizeof(float));
	curvetoplot2=(float*)malloc(100*sizeof(float));
	curvetoplot3=(float*)malloc(100*sizeof(float));
	for( i=0 ; i < intcurve ; i++ ){
		curvetoplot[i]=0.0;
		curvetoplot2[i]=0.0;
		curvetoplot3[i]=0.0;
	}

	if(!jtsPort.open("/jtsCalibrationGUI/jts:i")){
		cout << ": unable to open input port" << endl;
		return false; // unable to open; let RFModule know so that it won't run
     	}

	gtk_idle_add(on_expose_event, NULL);
	gtk_init (&argc, &argv);    // initialize gtk

//   	if(!initNetwork(yarp, rf, argc, argv, guiName, gXpos, gYpos))
//      return 0;

    	pWindow = gtk_window_new(GTK_WINDOW_TOPLEVEL);
    	gtk_window_set_default_size(GTK_WINDOW(pWindow), 820, 900);
 // 	gtk_window_set_resizable(GTK_WINDOW(pWindow), true);
    	gtk_window_set_title(GTK_WINDOW(pWindow), "Joints Torque Sensors Calibration GUI");

	pVBox = gtk_vbox_new(TRUE, 0);
        gtk_container_add(GTK_CONTAINER(pWindow), pVBox);

	pHBox = gtk_hbox_new(FALSE, 0);
        gtk_box_pack_start(GTK_BOX(pVBox), pHBox, FALSE, TRUE, 0);

	pImage= gtk_image_new_from_file("/home/guillaume/icub-main/src/tools/jtsCalibrationGUI/src/echelle.png");
	pLabel = gtk_label_new("0");
	gtk_box_pack_start(GTK_BOX(pHBox), pImage, FALSE, TRUE, 0);
	darea = gtk_drawing_area_new();
	gtk_box_pack_start(GTK_BOX(pHBox), darea, TRUE, TRUE, 0);
	pLabel = gtk_label_new("Right Arm Joint 0 (N)");
	gtk_box_pack_start(GTK_BOX(pHBox), pLabel, FALSE, TRUE, 0);


	pHBox2 = gtk_hbox_new(FALSE, 0);
        gtk_box_pack_start(GTK_BOX(pVBox), pHBox2, FALSE, TRUE, 0);

	pImage2= gtk_image_new_from_file("/home/guillaume/icub-main/src/tools/jtsCalibrationGUI/src/echelle.png");
	gtk_box_pack_start(GTK_BOX(pHBox2), pImage2, FALSE, TRUE, 0);
	darea2 = gtk_drawing_area_new();
	gtk_box_pack_start(GTK_BOX(pHBox2), darea2, TRUE, TRUE, 0);
	pLabel2 = gtk_label_new("Right Arm Joint 1 (N)");
	gtk_box_pack_start(GTK_BOX(pHBox2), pLabel2, FALSE, TRUE, 0);


	pHBox3 = gtk_hbox_new(FALSE, 0);
        gtk_box_pack_start(GTK_BOX(pVBox), pHBox3, FALSE, TRUE, 0);

	pImage3= gtk_image_new_from_file("/home/guillaume/icub-main/src/tools/jtsCalibrationGUI/src/echelle.png");
	gtk_box_pack_start(GTK_BOX(pHBox3), pImage3, FALSE, TRUE, 0);
	darea3 = gtk_drawing_area_new();
	gtk_box_pack_start(GTK_BOX(pHBox3), darea3, TRUE, TRUE, 0);
	pLabel3 = gtk_label_new("Right Arm Joint 3 (N)");
	gtk_box_pack_start(GTK_BOX(pHBox3), pLabel3, FALSE, TRUE, 0);

  	gtk_widget_show_all(GTK_WIDGET(pWindow));
	std::cout<< "gtk_main"<<std::endl;


 	gtk_main ();


//  	gdk_threads_leave();


    	return 0;
}
Exemplo n.º 6
0
    bool threadInit()
    {
		port.open ("/FTsensorTest:i");
		fprintf(stdout,"init \n");
		return true;
    }