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