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