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 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; }
/** * 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; }
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; }
bool threadInit() { port.open ("/FTsensorTest:i"); fprintf(stdout,"init \n"); return true; }