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