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
00032 #include "plplot_graph.h"
00033
00034 singlePlot::singlePlot(uint id_)
00035 {
00036 id=id_;
00037 x=NULL;
00038 y=NULL;
00039 Lsize=0;
00040 allocated_size=0;
00041 color=PL_BLACK;
00042 do_not_draw=false;
00043 shape=PL_NOTRELEVANT;
00044 }
00045
00046 void singlePlot::SetColor(plcolors color_)
00047 {
00048 color=color_;
00049 }
00050
00051 void singlePlot::SetXLinspace(double min,double max,uint size)
00052 {
00053 if(x!=NULL)
00054 free(x);
00055
00056 x=(double*)malloc(size*sizeof(double));
00057
00058 double step=(max-min)/size;
00059 uint i=0;
00060 for(double v=min;v<max;v+=step,i++)
00061 x[i]=v;
00062
00063 Lsize=size;
00064 }
00065
00066 void singlePlot::SetEnv(double xmin_,double xmax_,double ymin_,double ymax_)
00067 {
00068 xmin=xmin_;
00069 xmax=xmax_;
00070 ymin=ymin_;
00071 ymax=ymax_;
00072 }
00073
00074 void singlePlot::DynamicScaleEnv(std::vector<t_posePtr>& path,plshape shape_)
00075 {
00076 shape=shape_;
00077
00078 if(!path.size())
00079 {
00080 xmin=-1;
00081 xmax=1;
00082 ymin=-1;
00083 ymax=1;
00084 return;
00085 }
00086
00087 xmax=max_x(path);
00088 xmin=min_x(path);
00089
00090 ymax=max_y(path);
00091 ymin=min_y(path);
00092
00093 double dx=xmax-xmin;
00094 double dy=ymax-ymin;
00095
00096 xmax+=0.1*dx;
00097 xmin-=0.1*dx;
00098
00099 ymax+=0.1*dy;
00100 ymin-=0.1*dy;
00101
00102 double xc,yc;
00103
00104 if(shape==PL_SQUARE)
00105 {
00106 if(dx>dy)
00107 {
00108 yc=(ymin+ymax)/2;
00109 ymin=yc-dx/2.;
00110 ymax=yc+dx/2.;
00111 }else
00112 {
00113 xc=(xmin+xmax)/2;
00114 xmin=xc-dy/2.;
00115 xmax=xc+dy/2.;
00116 }
00117 }
00118
00119 if(xmin==xmax)
00120 {
00121 xmin-=1;
00122 xmax+=1;
00123 }
00124
00125 if(ymin==ymax)
00126 {
00127 ymin-=1;
00128 ymax+=1;
00129 }
00130 }
00131
00132 void singlePlot::Env()
00133 {
00134 if(xmin==xmax)
00135 {
00136 xmin=-1;
00137 xmax=1;
00138 }
00139
00140 if(ymin==ymax)
00141 {
00142 ymin=-1;
00143 ymax=1;
00144 }
00145
00146 plenv(xmin,xmax,ymin,ymax,shape,2);
00147 }
00148
00149 void singlePlot::SetLabel(string x_label_,string y_label_,string title_)
00150 {
00151 title=title_;
00152 x_label=x_label_;
00153 y_label=y_label_;
00154 }
00155
00156 void singlePlot::Label()
00157 {
00158 pllab(x_label.c_str(),y_label.c_str(),title.c_str());
00159 }
00160
00161 void singlePlot::SetSize(uint size)
00162 {
00163 if(size>allocated_size)
00164 {
00165 allocated_size=1024+2*size;
00166
00167 x=(double*)realloc(x,allocated_size*sizeof(double));
00168 y=(double*)realloc(y,allocated_size*sizeof(double));
00169 }
00170
00171 Lsize=size;
00172 }
00173
00174 void singlePlot::Plot()
00175 {
00176 if(!Lsize)
00177 return;
00178
00179 if(do_not_draw)
00180 return;
00181
00182 plcol0(color);
00183 plline(Lsize,x,y);
00184 plcol0(PL_BLACK);
00185 }
00186
00187 plSpace::plSpace(uint width,uint height, bool draw_)
00188 {
00189 draw=draw_;
00190
00191 if(!draw)
00192 return;
00193
00194 plsdev("xcairo");
00195
00196 plsetopt("np",NULL);
00197 plsetopt("geometry","800x800");
00198
00199 plscolor(true);
00200 plscolbg(255,255,255);
00201 plssub(width,height);
00202
00203 for(uint i=0;i<width*height;i++)
00204 {
00205 singlePlotPtr sPlot(new singlePlot(i));
00206
00207 sPlot->id=i;
00208 sPlot->SetEnv(0,1,0,1);
00209 sPlot->SetLabel("N/A","N/A","Unused");
00210
00211 plotVector.push_back(sPlot);
00212
00213 sPlot.reset(new singlePlot(i));
00214 sPlot->SetEnv(0,1,0,1);
00215 sPlot->id=i;
00216
00217 plotVector_2.push_back(sPlot);
00218
00219 sPlot.reset(new singlePlot(i));
00220 sPlot->SetEnv(0,1,0,1);
00221 sPlot->id=i;
00222
00223 plotVector_3.push_back(sPlot);
00224
00225 sPlot.reset(new singlePlot(i));
00226 sPlot->SetEnv(0,1,0,1);
00227 sPlot->id=i;
00228
00229 plotVector_4.push_back(sPlot);
00230 }
00231
00232 plinit ();
00233 plscol0(14,0,0,0);
00234 plscol0(2,255,210,0);
00235 plcol0(14);
00236 }
00237
00238 plSpace::~plSpace()
00239 {};
00240
00241 void plSpace::Plot()
00242 {
00243 if(!draw)
00244 return;
00245
00246 for(uint i=0;i<plotVector.size();i++)
00247 {
00248 plotVector[i]->Env();
00249 plotVector[i]->Label();
00250 plotVector[i]->Plot();
00251
00252 plotVector_2[i]->Plot();
00253 plotVector_3[i]->Plot();
00254 plotVector_4[i]->Plot();
00255 }
00256 }
00257
00258 void plSpace::SetThirdPlot(uint pos,vector<t_posePtr>& p,double (func)(t_pose&),plcolors color, double Dt)
00259 {
00260 if(!draw)
00261 return;
00262
00263 if(!p.size())
00264 return;
00265
00266 t_posePtr pose=p.back();
00267
00268 double latest_timestamp=pose->timestamp;
00269 double iterating_timestamp;
00270
00271 uint vector_size=0;
00272
00273 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it)
00274 {
00275 iterating_timestamp=(*it)->timestamp;
00276 if(fabs(latest_timestamp-iterating_timestamp)<Dt)
00277 vector_size++;
00278 }
00279
00280 plotVector_3[pos]->SetSize(vector_size);
00281
00282 uint i=0;
00283 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it,i++)
00284 {
00285 iterating_timestamp=(*it)->timestamp;
00286 if(fabs(latest_timestamp-iterating_timestamp)>Dt)
00287 break;
00288
00289 plotVector_3[pos]->x[i]=iterating_timestamp-latest_timestamp;
00290 plotVector_3[pos]->y[i]=func(*(*it));
00291 }
00292
00293 plotVector_3[pos]->SetColor(color);
00294 }
00295
00296 void plSpace::SetFourthPlot(uint pos,vector<t_posePtr>& p,double (func)(t_pose&),plcolors color, double Dt)
00297 {
00298 if(!draw)
00299 return;
00300
00301 if(!p.size())
00302 return;
00303
00304 t_posePtr pose=p.back();
00305
00306 double latest_timestamp=pose->timestamp;
00307 double iterating_timestamp;
00308
00309 uint vector_size=0;
00310
00311 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it)
00312 {
00313 iterating_timestamp=(*it)->timestamp;
00314 if(fabs(latest_timestamp-iterating_timestamp)<Dt)
00315 vector_size++;
00316 }
00317
00318 plotVector_4[pos]->SetSize(vector_size);
00319
00320 uint i=0;
00321 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it,i++)
00322 {
00323 iterating_timestamp=(*it)->timestamp;
00324 if(fabs(latest_timestamp-iterating_timestamp)>Dt)
00325 break;
00326
00327 plotVector_4[pos]->x[i]=iterating_timestamp-latest_timestamp;
00328 plotVector_4[pos]->y[i]=func(*(*it));
00329 }
00330
00331 plotVector_4[pos]->SetColor(color);
00332 }
00333
00334 void plSpace::SetSecondaryPlot(uint pos,vector<t_posePtr>& p,double (func)(t_pose&),plcolors color, double Dt)
00335 {
00336 if(!draw)
00337 return;
00338
00339 if(!p.size())
00340 return;
00341
00342 t_posePtr pose=p.back();
00343
00344 double latest_timestamp=pose->timestamp;
00345 double iterating_timestamp;
00346
00347 uint vector_size=0;
00348
00349 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it)
00350 {
00351 iterating_timestamp=(*it)->timestamp;
00352 if(fabs(latest_timestamp-iterating_timestamp)<Dt)
00353 vector_size++;
00354 }
00355
00356 plotVector_2[pos]->SetSize(vector_size);
00357
00358 uint i=0;
00359 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it,i++)
00360 {
00361 iterating_timestamp=(*it)->timestamp;
00362 if(fabs(latest_timestamp-iterating_timestamp)>Dt)
00363 break;
00364
00365 plotVector_2[pos]->x[i]=iterating_timestamp-latest_timestamp;
00366 plotVector_2[pos]->y[i]=func(*(*it));
00367 }
00368
00369 plotVector_2[pos]->SetColor(color);
00370 }
00371
00372 void plSpace::SetMember_UseTimestamp(uint pos,vector<t_posePtr>& p,double (func)(t_pose&),plcolors color,pair<double,double> limits,string Xlabel,string Ylabel,string title, double Dt)
00373 {
00374 if(!draw)
00375 return;
00376
00377 if(!p.size())
00378 return;
00379
00380 t_posePtr pose=p.back();
00381
00382 double latest_timestamp=pose->timestamp;
00383 double iterating_timestamp;
00384
00385 uint vector_size=0;
00386
00387 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it)
00388 {
00389 iterating_timestamp=(*it)->timestamp;
00390 if(fabs(latest_timestamp-iterating_timestamp)<Dt)
00391 vector_size++;
00392 }
00393
00394 plotVector[pos]->SetSize(vector_size);
00395
00396 uint i=0;
00397 for(vector<t_posePtr>::reverse_iterator it=p.rbegin();it<p.rend();++it,i++)
00398 {
00399 iterating_timestamp=(*it)->timestamp;
00400 if(fabs(latest_timestamp-iterating_timestamp)>Dt)
00401 break;
00402
00403 plotVector[pos]->x[i]=iterating_timestamp-latest_timestamp;
00404 plotVector[pos]->y[i]=func(*(*it));
00405 }
00406
00407 plotVector[pos]->ex_timestamp=latest_timestamp;
00408 plotVector[pos]->SetEnv(-Dt,0.5,limits.first,limits.second);
00409
00410 plotVector[pos]->SetLabel(Xlabel.c_str(),Ylabel.c_str(),title.c_str());
00411 plotVector[pos]->SetColor(color);
00412 }
00413
00414 void plSpace::SetRunTime(uint pos,vector<double>& rt,plcolors color,string title,double tmax, double max)
00415 {
00416 if(!draw)
00417 return;
00418
00419 if(rt.size()>max)
00420 {
00421 plotVector[pos]->SetSize(max);
00422
00423 uint i=0;
00424 for(vector<double>::iterator it=rt.end()-max;it!=rt.end();it++,i++)
00425 {
00426 plotVector[pos]->x[i]=i;
00427 plotVector[pos]->y[i]=*it;
00428 }
00429
00430 plotVector[pos]->SetEnv(0,max,0,tmax);
00431
00432 }else
00433 {
00434 plotVector[pos]->SetSize(rt.size());
00435
00436 for(uint i=0;i<rt.size();i++)
00437 {
00438 plotVector[pos]->x[i]=i;
00439 plotVector[pos]->y[i]=rt[i];
00440 }
00441
00442 plotVector[pos]->SetEnv(0,rt.size(),0,tmax);
00443 }
00444
00445 plotVector[pos]->SetColor(color);
00446 plotVector[pos]->SetLabel("Steps (each 1/50 s)","Run time (ms)",title.c_str());
00447 }
00448
00449 void plSpace::SetMember(uint pos,vector<t_posePtr>& pt,double(func)(s_pose&),plcolors color,double Vmax,double Vmin,string Xlabel,string Ylabel,string title, double max)
00450 {
00451 if(!draw)
00452 return;
00453
00454 if(pt.size()>max)
00455 {
00456 plotVector[pos]->SetSize(max);
00457
00458 uint i=0;
00459 for(vector<t_posePtr>::iterator it=pt.end()-max;it!=pt.end();it++,i++)
00460 {
00461 plotVector[pos]->x[i]=i;
00462 plotVector[pos]->y[i]=func(*(*it));
00463 }
00464
00465 plotVector[pos]->SetEnv(0,max,Vmin,Vmax);
00466
00467 }else
00468 {
00469 plotVector[pos]->SetSize(pt.size());
00470
00471 for(uint i=0;i<pt.size();i++)
00472 {
00473 plotVector[pos]->x[i]=i;
00474 plotVector[pos]->y[i]=func(*pt[i]);
00475 }
00476
00477 plotVector[pos]->SetEnv(0,pt.size(),Vmin,Vmax);
00478 }
00479
00480 plotVector[pos]->SetLabel(Xlabel.c_str(),Ylabel.c_str(),title.c_str());
00481 plotVector[pos]->SetColor(color);
00482 }
00483
00484 void plSpace::SetLinearSpeed(uint pos,vector<t_posePtr>& pt,plcolors color,double tmax, double max)
00485 {
00486 if(!draw)
00487 return;
00488
00489 if(pt.size()>max)
00490 {
00491 plotVector[pos]->SetSize(max);
00492
00493 uint i=0;
00494 for(vector<t_posePtr>::iterator it=pt.end()-max;it!=pt.end();it++,i++)
00495 {
00496 plotVector[pos]->x[i]=i;
00497 plotVector[pos]->y[i]=(*it)->vl;
00498 }
00499
00500 plotVector[pos]->SetEnv(0,max,0,tmax);
00501 }else
00502 {
00503 plotVector[pos]->SetSize(pt.size());
00504
00505 for(uint i=0;i<pt.size();i++)
00506 {
00507 plotVector[pos]->x[i]=i;
00508 plotVector[pos]->y[i]=pt[i]->vl;
00509 }
00510
00511 plotVector[pos]->SetEnv(0,pt.size(),0,tmax);
00512 }
00513
00514 plotVector[pos]->SetLabel("Steps (each 1/50 s)","Linear Speed (m/s)","Linear Speed");
00515 plotVector[pos]->SetColor(color);
00516 }
00517
00518 void plSpace::SetPath(uint pos,std::vector<t_posePtr>& path,string title)
00519 {
00520 if(!draw)
00521 return;
00522
00523 plotVector[pos]->SetSize(path.size());
00524
00525 for(uint i=0;i<path.size();i++)
00526 {
00527 plotVector[pos]->x[i]=path[i]->x;
00528 plotVector[pos]->y[i]=path[i]->y;
00529 }
00530
00531 plotVector[pos]->DynamicScaleEnv(path,PL_SQUARE);
00532 plotVector[pos]->SetLabel("x (m)","y (m)",title);
00533 }
00534
00535 void plSpace::SetSecondaryPath(uint pos,std::vector<t_posePtr>& path,plcolors color)
00536 {
00537 if(!draw)
00538 return;
00539
00540 plotVector_2[pos]->SetSize(path.size());
00541
00542 for(uint i=0;i<path.size();i++)
00543 {
00544 plotVector_2[pos]->x[i]=path[i]->x;
00545 plotVector_2[pos]->y[i]=path[i]->y;
00546 }
00547
00548 plotVector_2[pos]->SetColor(color);
00549 }
00550
