00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00033 #include <iostream>
00034 #include <pthread.h>
00035
00036 #include <math.h>
00037 #include <string>
00038 #include <sstream>
00039 #include <fstream>
00040 #include <stdio.h>
00041 #include <stdlib.h>
00042 #include <signal.h>
00043 #include <iostream>
00044
00045 #include <boost/asio.hpp>
00046 #include <boost/array.hpp>
00047 #include <boost/thread.hpp>
00048 #include <boost/asio/ip/tcp.hpp>
00049 #include <boost/thread/mutex.hpp>
00050 #include <boost/thread/locks.hpp>
00051 #include <boost/thread/thread.hpp>
00052
00053 #include <arpa/inet.h>
00054 #include <stdbool.h>
00055 #include <stdio.h>
00056 #include <sys/socket.h>
00057 #include <string.h>
00058 #include <unistd.h>
00059 #include <sys/types.h>
00060 #include <sys/ipc.h>
00061 #include <sys/socket.h>
00062 #include <netinet/in.h>
00063 #include <arpa/inet.h>
00064 #include <gtk/gtk.h>
00065 #include <stdlib.h>
00066 #include <sys/shm.h>
00067 #include <errno.h>
00068 #include <math.h>
00069 #include <ctype.h>
00070 #include <time.h>
00071 #include <sys/time.h>
00072 #include <fcntl.h>
00073 #include "cv.h"
00074 #include "highgui.h"
00075 #include <cairo.h>
00076
00077 #include <ros/ros.h>
00078
00079 #include <visualization_msgs/Marker.h>
00080 #include <visualization_msgs/MarkerArray.h>
00081
00082 #include <sensor_msgs/Range.h>
00083 #include <ros/package.h>
00084 #include <geometry_msgs/Vector3.h>
00085
00086 #include <Eigen/Dense>
00087 #include <Eigen/Geometry>
00088
00089
00090
00091 using namespace std;
00092
00093
00094
00095 extern "C"
00096 {
00097
00098 GtkBuilder *builderG;
00100 double read_values[17];
00102 double last_values[16];
00111 void delete_handler(GtkWidget * widget, GdkEvent * event, gpointer user_data)
00112 {
00113 g_print("You should not use the OS to force leave!\n");
00114 read_values[17]=0;
00115 gtk_main_quit();
00116 }
00117
00129 void bt_sair_on(GtkWidget * widget, GdkEventMotion * event, gpointer user_data)
00130 {
00131 read_values[17]=0;
00132 gtk_main_quit();
00133 }
00134
00146 void bt_ler_on(GtkWidget * widget, GdkEventMotion * event, gpointer user_data)
00147 {
00148 char estadosensor[512];
00149
00150 char estadoxx0[512];
00151 char estadoxx1[512];
00152 char estadoxx2[512];
00153 char estadoxx3[512];
00154 char estadoxx4[512];
00155 char estadoxx5[512];
00156 char estadoxx6[512];
00157 char estadoxx7[512];
00158
00159 char estadoyy0[512];
00160 char estadoyy1[512];
00161 char estadoyy2[512];
00162 char estadoyy3[512];
00163 char estadoyy4[512];
00164 char estadoyy5[512];
00165 char estadoyy6[512];
00166 char estadoyy7[512];
00167
00168 GtkWidget *senso= GTK_WIDGET(gtk_builder_get_object (builderG, "sensores"));
00169
00170 GtkWidget *x_0= GTK_WIDGET(gtk_builder_get_object (builderG, "x0"));
00171 GtkWidget *y_0= GTK_WIDGET(gtk_builder_get_object (builderG, "y0"));
00172
00173 GtkWidget *x_1= GTK_WIDGET(gtk_builder_get_object (builderG, "x1"));
00174 GtkWidget *y_1= GTK_WIDGET(gtk_builder_get_object (builderG, "y1"));
00175
00176 GtkWidget *x_2= GTK_WIDGET(gtk_builder_get_object (builderG, "x2"));
00177 GtkWidget *y_2= GTK_WIDGET(gtk_builder_get_object (builderG, "y2"));
00178
00179 GtkWidget *x_3= GTK_WIDGET(gtk_builder_get_object (builderG, "x3"));
00180 GtkWidget *y_3= GTK_WIDGET(gtk_builder_get_object (builderG, "y3"));
00181
00182 GtkWidget *x_4= GTK_WIDGET(gtk_builder_get_object (builderG, "x4"));
00183 GtkWidget *y_4= GTK_WIDGET(gtk_builder_get_object (builderG, "y4"));
00184
00185 GtkWidget *x_5= GTK_WIDGET(gtk_builder_get_object (builderG, "x5"));
00186 GtkWidget *y_5= GTK_WIDGET(gtk_builder_get_object (builderG, "y5"));
00187
00188 GtkWidget *x_6= GTK_WIDGET(gtk_builder_get_object (builderG, "x6"));
00189 GtkWidget *y_6= GTK_WIDGET(gtk_builder_get_object (builderG, "y6"));
00190
00191 GtkWidget *x_7= GTK_WIDGET(gtk_builder_get_object (builderG, "x7"));
00192 GtkWidget *y_7= GTK_WIDGET(gtk_builder_get_object (builderG, "y7"));
00193
00194 sprintf(estadosensor,"%f",last_values[0]);
00195
00196 sprintf(estadoxx0,"%f",last_values[1]);
00197 sprintf(estadoyy0,"%f",last_values[2]);
00198
00199 sprintf(estadoxx1,"%f",last_values[3]);
00200 sprintf(estadoyy1,"%f",last_values[4]);
00201
00202 sprintf(estadoxx2,"%f",last_values[5]);
00203 sprintf(estadoyy2,"%f",last_values[6]);
00204
00205 sprintf(estadoxx3,"%f",last_values[7]);
00206 sprintf(estadoyy3,"%f",last_values[8]);
00207
00208 sprintf(estadoxx4,"%f",last_values[9]);
00209 sprintf(estadoyy4,"%f",last_values[10]);
00210
00211 sprintf(estadoxx5,"%f",last_values[11]);
00212 sprintf(estadoyy5,"%f",last_values[12]);
00213
00214 sprintf(estadoxx6,"%f",last_values[13]);
00215 sprintf(estadoyy6,"%f",last_values[14]);
00216
00217 sprintf(estadoxx7,"%f",last_values[15]);
00218 sprintf(estadoyy7,"%f",last_values[16]);
00219
00220 gtk_entry_set_text((GtkEntry *)senso,estadosensor);
00221
00222 gtk_entry_set_text((GtkEntry *)x_0,estadoxx0);
00223 gtk_entry_set_text((GtkEntry *)y_0,estadoyy0);
00224
00225 gtk_entry_set_text((GtkEntry *)x_1,estadoxx1);
00226 gtk_entry_set_text((GtkEntry *)y_1,estadoyy1);
00227
00228 gtk_entry_set_text((GtkEntry *)x_2,estadoxx2);
00229 gtk_entry_set_text((GtkEntry *)y_2,estadoyy2);
00230
00231 gtk_entry_set_text((GtkEntry *)x_3,estadoxx3);
00232 gtk_entry_set_text((GtkEntry *)y_3,estadoyy3);
00233
00234 gtk_entry_set_text((GtkEntry *)x_4,estadoxx4);
00235 gtk_entry_set_text((GtkEntry *)y_4,estadoyy4);
00236
00237 gtk_entry_set_text((GtkEntry *)x_5,estadoxx5);
00238 gtk_entry_set_text((GtkEntry *)y_5,estadoyy5);
00239
00240 gtk_entry_set_text((GtkEntry *)x_6,estadoxx6);
00241 gtk_entry_set_text((GtkEntry *)y_6,estadoyy6);
00242
00243 gtk_entry_set_text((GtkEntry *)x_7,estadoxx7);
00244 gtk_entry_set_text((GtkEntry *)y_7,estadoyy7);
00245
00246
00247 }
00248
00260 void bt_calibrar_on(GtkWidget * widget, GdkEventMotion * event, gpointer user_data)
00261 {
00262 double x0,x1,x2,x3,x4,x5,x6,x7,y0,y1,y2,y3,y4,y5,y6,y7,sensors;
00263
00264 const gchar* sensor;
00265
00266 const gchar* xx0;
00267 const gchar* xx1;
00268 const gchar* xx2;
00269 const gchar* xx3;
00270 const gchar* xx4;
00271 const gchar* xx5;
00272 const gchar* xx6;
00273 const gchar* xx7;
00274
00275 const gchar* yy0;
00276 const gchar* yy1;
00277 const gchar* yy2;
00278 const gchar* yy3;
00279 const gchar* yy4;
00280 const gchar* yy5;
00281 const gchar* yy6;
00282 const gchar* yy7;
00283
00284 GtkWidget *senso= GTK_WIDGET(gtk_builder_get_object (builderG, "sensores"));
00285
00286 GtkWidget *x_0= GTK_WIDGET(gtk_builder_get_object (builderG, "x0"));
00287 GtkWidget *y_0= GTK_WIDGET(gtk_builder_get_object (builderG, "y0"));
00288
00289 GtkWidget *x_1= GTK_WIDGET(gtk_builder_get_object (builderG, "x1"));
00290 GtkWidget *y_1= GTK_WIDGET(gtk_builder_get_object (builderG, "y1"));
00291
00292 GtkWidget *x_2= GTK_WIDGET(gtk_builder_get_object (builderG, "x2"));
00293 GtkWidget *y_2= GTK_WIDGET(gtk_builder_get_object (builderG, "y2"));
00294
00295 GtkWidget *x_3= GTK_WIDGET(gtk_builder_get_object (builderG, "x3"));
00296 GtkWidget *y_3= GTK_WIDGET(gtk_builder_get_object (builderG, "y3"));
00297
00298 GtkWidget *x_4= GTK_WIDGET(gtk_builder_get_object (builderG, "x4"));
00299 GtkWidget *y_4= GTK_WIDGET(gtk_builder_get_object (builderG, "y4"));
00300
00301 GtkWidget *x_5= GTK_WIDGET(gtk_builder_get_object (builderG, "x5"));
00302 GtkWidget *y_5= GTK_WIDGET(gtk_builder_get_object (builderG, "y5"));
00303
00304 GtkWidget *x_6= GTK_WIDGET(gtk_builder_get_object (builderG, "x6"));
00305 GtkWidget *y_6= GTK_WIDGET(gtk_builder_get_object (builderG, "y6"));
00306
00307 GtkWidget *x_7= GTK_WIDGET(gtk_builder_get_object (builderG, "x7"));
00308 GtkWidget *y_7= GTK_WIDGET(gtk_builder_get_object (builderG, "y7"));
00309
00310 sensor=gtk_entry_get_text((GtkEntry *)senso);
00311
00312 xx0=gtk_entry_get_text((GtkEntry *)x_0);
00313 yy0=gtk_entry_get_text((GtkEntry *)y_0);
00314
00315 xx1=gtk_entry_get_text((GtkEntry *)x_1);
00316 yy1=gtk_entry_get_text((GtkEntry *)y_1);
00317
00318 xx2=gtk_entry_get_text((GtkEntry *)x_2);
00319 yy2=gtk_entry_get_text((GtkEntry *)y_2);
00320
00321 xx3=gtk_entry_get_text((GtkEntry *)x_3);
00322 yy3=gtk_entry_get_text((GtkEntry *)y_3);
00323
00324 xx4=gtk_entry_get_text((GtkEntry *)x_4);
00325 yy4=gtk_entry_get_text((GtkEntry *)y_4);
00326
00327 xx5=gtk_entry_get_text((GtkEntry *)x_5);
00328 yy5=gtk_entry_get_text((GtkEntry *)y_5);
00329
00330 xx6=gtk_entry_get_text((GtkEntry *)x_6);
00331 yy6=gtk_entry_get_text((GtkEntry *)y_6);
00332
00333 xx7=gtk_entry_get_text((GtkEntry *)x_7);
00334 yy7=gtk_entry_get_text((GtkEntry *)y_7);
00335
00336 sensors=atof(sensor);
00337
00338 x0=atof(xx0);
00339 y0=atof(yy0);
00340
00341 x1=atof(xx1);
00342 y1=atof(yy1);
00343
00344 x2=atof(xx2);
00345 y2=atof(yy2);
00346
00347 x3=atof(xx3);
00348 y3=atof(yy3);
00349
00350 x4=atof(xx4);
00351 y4=atof(yy4);
00352
00353 x5=atof(xx5);
00354 y5=atof(yy5);
00355
00356 x6=atof(xx6);
00357 y6=atof(yy6);
00358
00359 x7=atof(xx7);
00360 y7=atof(yy7);
00361
00362
00363 read_values[0]=(sensors);
00364
00365 read_values[1]=(x0);
00366 read_values[2]=(y0);
00367
00368 read_values[3]=(x1);
00369 read_values[4]=(y1);
00370
00371 read_values[5]=(x2);
00372 read_values[6]=(y2);
00373
00374 read_values[7]=(x3);
00375 read_values[8]=(y3);
00376
00377 read_values[9]=(x4);
00378 read_values[10]=(y4);
00379
00380 read_values[11]=(x5);
00381 read_values[12]=(y5);
00382
00383 read_values[13]=(x6);
00384 read_values[14]=(y6);
00385
00386 read_values[15]=(x7);
00387 read_values[16]=(y7);
00388
00389 read_values[17]=1;
00390 gtk_main_quit();
00391 }
00392
00393 }
00394
00395 class Interface
00396 {
00397 public:
00402 void start()
00403 {
00404 int startup;
00405
00406 gtk_init(0,0);
00407 builderG = gtk_builder_new ();
00408
00409 startup=gtk_builder_add_from_file(builderG,"home/sergio/workincopies/lar4/src/sensors/optoelectric/src/Interface/calibrar.glade", NULL);
00410 if( ! startup )
00411 {
00412 g_print("%s File was not found. Aborting!\n", "/home/sergio/workingcopies/lar4/src/sensors/optoelectric/src/Interface/calibrar.glade");
00413 }
00414
00415
00416 gtk_builder_connect_signals (builderG, NULL);
00417
00418
00419
00420 GtkWidget *m = GTK_WIDGET(gtk_builder_get_object (builderG, "Main"));
00421
00422
00423
00424
00425 if(m)
00426 {
00427 g_signal_connect(G_OBJECT(m), "delete-event", G_CALLBACK(delete_handler), NULL);
00428 }
00429
00430
00431 gtk_main();
00432 }
00433
00434 };
00440 class Plane
00441 {
00442 public:
00448 Plane()
00449 {
00450 a=0;b=0;c=0;d=0;
00451 }
00452
00453 double a,b,c,d;
00454 };
00461 class SharpMath
00462 {
00463 public:
00469 SharpMath()
00470 {
00471
00472 }
00478 void readParameters(string parameters_file)
00479 {
00480 cv::FileStorage parameters(parameters_file.c_str(),cv::FileStorage::READ);
00481
00482 number_sensors = (int)parameters["number_sensors"];
00483
00484 for(uint s = 0;s<8;s++)
00485 {
00486 Eigen::Vector2d pos;
00487
00488 pos(0) = parameters["x"+to_string(s)];
00489 pos(1) = parameters["y"+to_string(s)];
00490
00491 sensors_positions.push_back(pos);
00492
00493 Eigen::VectorXd curve(4);
00494
00495 curve(0) = parameters["p1_"+to_string(s)];
00496 curve(1) = parameters["p2_"+to_string(s)];
00497 curve(2) = parameters["p3_"+to_string(s)];
00498 curve(3) = parameters["p4_"+to_string(s)];
00499
00500 sensors_curve_parameters.push_back(curve);
00501 }
00502
00503
00504 last_values[0]=number_sensors;
00505
00506 for(uint nn = 0;nn<number_sensors;nn++)
00507 {
00508 last_values[2*nn+1]=sensors_positions[nn](0);
00509 last_values[2*nn+2]=sensors_positions[nn](1);
00510 }
00511
00512 }
00519 Plane getPlane(vector<double> values)
00520 {
00521 Eigen::VectorXd g(number_sensors);
00522 g = Eigen::VectorXd::Ones(number_sensors);
00523
00524
00525
00526 Eigen::MatrixXd F(number_sensors,3);
00527
00528
00529
00530 for(uint i=0;i<number_sensors;i++)
00531 {
00532 F(i,0) = read_values[2*i+1];
00533 F(i,1) = read_values[2*i+2];
00534 F(i,2) = values[i];
00535 }
00536
00537
00538
00539 Eigen::VectorXd r(3);
00540
00541
00542
00543 r = ((F.transpose()*F).inverse() * F.transpose())*g;
00544
00545
00546
00547 double zp = ((-r(0)/r(2))*sensors_positions[0](0)) - ((r(1)/r(2))*sensors_positions[0](1));
00548
00549 double d = values[0] - zp;
00550 double a = r(0)*d;
00551 double b = r(1)*d;
00552 double c = r(2)*d;
00553
00554 Plane temp;
00555
00556 temp.a=a;
00557 temp.b=b;
00558 temp.c=c;
00559 temp.d=d;
00560
00561 return temp;
00562 }
00569 double getPitch( Plane p)
00570 {
00571 double a, b, c, d;
00572 a=p.a;
00573 b=p.b;
00574 c=p.c;
00575 d=p.d;
00576 return ((180./M_PI)*(atan2(d,(d*c/a))));
00577 }
00584 double getRoll( Plane p)
00585 {
00586 double a, b, c, d;
00587 a=p.a;
00588 b=p.b;
00589 c=p.c;
00590 d=p.d;
00591 return ((180./M_PI)*(atan2(d,(d*c/b))));
00592 }
00593
00599 void writeParameters(string parameters_file)
00600 {
00601 double pitch_h=0;
00602 double roll_h=0;
00603
00604 for (uint m=0; m<pitch_bias.size(); m++)
00605 {
00606 pitch_h = pitch_h + pitch_bias[m];
00607 }
00608
00609 for (uint m=0; m<roll_bias.size(); m++)
00610 {
00611 roll_h = roll_h + roll_bias[m];
00612 }
00613
00614 pitch_h = pitch_h / pitch_bias.size();
00615 roll_h = roll_h / roll_bias.size();
00616
00617
00618
00619
00620
00621 cv::FileStorage parameters(parameters_file.c_str(),cv::FileStorage::WRITE);
00622
00623 parameters << "number_sensors" << read_values[0] << "pitch_bias" << pitch_h << "roll_bias" << roll_h << "x0" << read_values[1] << "y0" << read_values[2] << "x1" << read_values[3] << "y1" << read_values[4] << "x2" << read_values[5] << "y2" << read_values[6] << "x3" << read_values[7] << "y3" << read_values[8] << "x4" << read_values[9] << "y4" << read_values[10] << "x5" << read_values[11] << "y5" << read_values[12] << "x6" << read_values[13] << "y6" << read_values[14] << "x7" << read_values[15] << "y7" << read_values[16] << "p1_0" << sensors_curve_parameters[0](0)<< "p2_0" << sensors_curve_parameters[0](1)<< "p3_0" << sensors_curve_parameters[0](2) << "p4_0" << sensors_curve_parameters[0](3)<< "p1_1" << sensors_curve_parameters[1](0)<< "p2_1" << sensors_curve_parameters[1](1) << "p3_1" << sensors_curve_parameters[1](2)<< "p4_1" << sensors_curve_parameters[1](3)<< "p1_2" << sensors_curve_parameters[2](0) << "p2_2" << sensors_curve_parameters[2](1)<< "p3_2" << sensors_curve_parameters[2](2)<< "p4_2" <<
00624 sensors_curve_parameters[2](3) << "p1_3" << sensors_curve_parameters[3](0)<< "p2_3" << sensors_curve_parameters[3](1)<< "p3_3" << sensors_curve_parameters[3](2) << "p4_3" << sensors_curve_parameters[3](3)<< "p1_4" << sensors_curve_parameters[4](0)<< "p2_4" << sensors_curve_parameters[4](1) << "p3_4" << sensors_curve_parameters[4](2)<< "p4_4" << sensors_curve_parameters[4](3)<< "p1_5" << sensors_curve_parameters[5](0) << "p2_5" << sensors_curve_parameters[5](1)<< "p3_5" << sensors_curve_parameters[5](2)<< "p4_5" << sensors_curve_parameters[5](3) << "p1_6" << sensors_curve_parameters[6](0)<< "p2_6" << sensors_curve_parameters[0](1)<< "p3_6" << sensors_curve_parameters[6](2) << "p4_6" << sensors_curve_parameters[6](3)<< "p1_7" << sensors_curve_parameters[7](0)<< "p2_7" << sensors_curve_parameters[7](1) << "p3_7" << sensors_curve_parameters[7](2)<< "p4_7" << sensors_curve_parameters[7](3);
00625
00626 parameters.release();
00627
00628 }
00629
00630 vector<double> pitch_bias;
00631 vector<double> roll_bias;
00632
00633
00634 private:
00635
00636 uint number_sensors;
00637 vector<Eigen::Vector2d> sensors_positions;
00638 vector<Eigen::VectorXd> sensors_curve_parameters;
00639
00640 };
00646 class OptoelectricVehiclePose
00647 {
00648 public:
00649 public:
00654 OptoelectricVehiclePose(const ros::NodeHandle& nh):
00655 nh_(nh)
00656 {
00657
00658 setupMessaging();
00659
00660 nh_.param("parameters",parameters_url,std::string("no parameters file"));
00661
00662 string package_tag = "package://";
00663 int pos = parameters_url.find(package_tag);
00664 if(pos != string::npos)
00665 {
00666
00667 int fpos = parameters_url.find("/",pos+package_tag.length());
00668 string package = parameters_url.substr(pos+package_tag.length(),fpos-(pos+package_tag.length()));
00669
00670 string package_path = ros::package::getPath(package);
00671 parameters_url.replace(pos,fpos-pos,package_path);
00672 }
00673
00674
00675 s.readParameters(parameters_url);
00676 }
00677
00682 void setupMessaging()
00683 {
00685
00686 new_data.resize(8,false);
00687
00688 for(uint i=0;i<8;i++)
00689 {
00690 ros::Subscriber sub = nh_.subscribe<sensor_msgs::Range>("/optoelectic_vehicle_base/sharp_"+to_string(i), 1, boost::bind(&OptoelectricVehiclePose::sharpCallback,this,_1,i));
00691
00692 sharp_subscriber.push_back(sub);
00693 }
00694
00695
00697
00698
00699 }
00706 void sharpCallback(const sensor_msgs::Range::ConstPtr msg,int i)
00707 {
00708
00709 sharp_values[i]=msg->range;
00710 new_data[i] = true;
00711 }
00716 void loop()
00717 {
00718 ros::Rate r(100);
00719
00720 int count=0;
00721
00722 while(ros::ok() && (count<51))
00723 {
00724 r.sleep();
00725 ros::spinOnce();
00726
00727 bool process = true;
00728
00729 for(bool v: new_data)
00730 {
00731 if(v==false)
00732 {
00733 process = false;
00734 break;
00735 }
00736 }
00737
00738 if(process==false)
00739 continue;
00740
00741 vector<double> values;
00742 for(uint i=0;i<8;i++)
00743 {
00744 double val = sharp_values[i];
00745 values.push_back(val);
00746 }
00747
00748 Plane plane = s.getPlane(values);
00749
00750 double pitch = s.getPitch(plane);
00751 double roll = s.getRoll(plane);
00752
00753 s.pitch_bias.push_back(pitch);
00754 s.roll_bias.push_back(roll);
00755
00756 new_data.clear();
00757 new_data.resize(8,false);
00758
00759 count ++;
00760
00761 }
00762
00763 s.writeParameters(parameters_url);
00764 }
00765
00766 private:
00767
00768 ros::NodeHandle nh_;
00769
00770 SharpMath s;
00771
00772 vector<ros::Subscriber> sharp_subscriber;
00773
00774 int sharp_values[8];
00775 vector<bool> new_data;
00776
00777 string parameters_url;
00778 };
00783 int main(int argc, char* argv[])
00784 {
00785 Interface inter;
00786
00787
00788 ros::init(argc, argv, "calibration_orientation");
00789
00790 ros::NodeHandle nh("~");
00791
00792
00793 OptoelectricVehiclePose opto(nh);
00794
00795 inter.start();
00796
00797 if (read_values[17]==1)
00798 {
00799 opto.loop();
00800 }
00801
00802 return 0;
00803 }