Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

guirrt.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
00006 // All Rights Reserved.
00007 // 
00008 // Permission to use, copy, and distribute this software and its 
00009 // documentation is hereby granted free of charge, provided that 
00010 // (1) it is not a component of a commercial product, and 
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation. 
00013 // 
00014 // Iowa State University and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.  
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #include <math.h>
00020 #include <stdio.h>
00021 #include <fstream.h>
00022 
00023 #include "guirrt.h"
00024 #include "defs.h"
00025 
00026 #include <LEDA/REDEFINE_NAMES.h>
00027 
00028 
00030 //
00031 //  GuiRRT class
00032 //
00034 
00035 GuiRRT::GuiRRT(Render *render, RRT *r):Gui(render) {
00036   rrt = r;
00037 
00038   GifSize = 7.0;
00039   DumpTrees = false;
00040   LineWidth = 1.0;
00041   PSLineWidth = 1.0;
00042   DrawIndexX = 0;
00043   DrawIndexY = 1;
00044 
00045   CreateControlWindow();
00046 }
00047 
00048 
00049 
00050 void GuiRRT::Start(){
00051 
00052     // Read the planner type, if exists
00053     if (is_file(rrt->P->FilePath + "RRT"))
00054       ButtonHandle(201);
00055     if (is_file(rrt->P->FilePath + "RRTGoalBias"))
00056       ButtonHandle(202);
00057     if (is_file(rrt->P->FilePath + "RRTGoalPull"))
00058       ButtonHandle(203);
00059     if (is_file(rrt->P->FilePath + "RRTCon"))
00060       ButtonHandle(204);
00061     if (is_file(rrt->P->FilePath + "RRTPolar"))
00062       ButtonHandle(208);
00063     if (is_file(rrt->P->FilePath + "RRTHull"))
00064       ButtonHandle(209);
00065     if (is_file(rrt->P->FilePath + "RRTStar"))
00066       ButtonHandle(212);
00067     if (is_file(rrt->P->FilePath + "RRTGoalZoom"))
00068       ButtonHandle(207);
00069     if (is_file(rrt->P->FilePath + "RRTDual")) 
00070       ButtonHandle(205);
00071     if (is_file(rrt->P->FilePath + "RRTExtExt"))
00072       ButtonHandle(206);
00073     if (is_file(rrt->P->FilePath + "RRTExtCon"))
00074       ButtonHandle(210);
00075     if (is_file(rrt->P->FilePath + "RRTConCon"))
00076       ButtonHandle(211);
00077     if (is_file(rrt->P->FilePath + "RRTHsu"))
00078       ButtonHandle(213);
00079     if (is_file(rrt->P->FilePath + "RRTNaive"))
00080       ButtonHandle(214);    
00081 
00082     Gui::Start(rrt->P->InitialState);
00083 }
00084 
00085 
00086 void GuiRRT::CreateControlWindow(){
00087   menu *m2,*m3,*m4,*mm2;
00088   W = new window(600,40,"RRT Planner, Iowa State University, 1998-2000");
00089   m2 = new menu();
00090   m3 = new menu();
00091   m4 = new menu();
00092   mm2 = new menu();
00093 
00094   W->button("Explore",22);
00095   W->button("Plan",23);
00096 
00097   mm2->button("Clear RRT",24);
00098   mm2->button("RRT",201);
00099   mm2->button("RRTGoalBias",202);
00100   mm2->button("RRTGoalPull",203);
00101   mm2->button("RRTCon",204);
00102   mm2->button("RRTPolar",208);
00103   mm2->button("RRTHull",209);
00104   mm2->button("RRTStar",212);
00105   mm2->button("RRTGoalZoom",207);
00106   mm2->button("RRTDual",205);
00107   mm2->button("RRTExtExt",206);
00108   mm2->button("RRTExtCon",210);
00109   mm2->button("RRTConCon",211);
00110   mm2->button("RRTHsu",213);
00111   mm2->button("RRTNaive",214);
00112   W->button("Planner",*mm2);
00113 
00114   m3->button("Animate Path",35);
00115   m3->button("Draw Path",34);
00116   m3->button("2D RRT Projection",31);
00117   //m3->button("3D RRT Projection",32);
00118   //m3->button("Voronoi Diagram",33);
00119   m3->button("Reset Window",36);
00120   W->button("Display",*m3);
00121 
00122   m4->button("2D RRT Projection to PS",41);
00123   //m4->button("Voronoi Diagram to PS",42);
00124   m4->button("Write RRT",44);
00125   m4->button("Read RRT",45);
00126   m4->button("Write Path",48);
00127   m4->button("Read Path",49);
00128   m4->button("Write Animation Frames",46);
00129   m4->button("Read Animation Frames",47);
00130   W->button("File",*m4);
00131 
00132   W->button("Settings",50);
00133 
00134   W->button("Exit",-1,exit);
00135 
00136   W->display();
00137 }
00138 
00139 
00140 void GuiRRT::ResetRRT()
00141 {
00142   R->SetScene(new Scene(rrt->P,FilePath));
00143   Path.clear();
00144   rrt->G.clear();
00145   rrt->G2.clear();
00146   rrt->SatisfiedCount = 0;
00147   rrt->GoalDist = INFINITY;
00148   rrt->BestState = rrt->P->InitialState;
00149 }
00150 
00151 
00152 // A friend of RuiRRT
00153 void GuiRRT::ButtonHandle(int b){
00154   //cout << "Button " << b << "\n";
00155   window m(200,152);
00156   
00157   switch (b) {
00158   case 12: cout << "Read Inputs\n";
00159     //ReadInputs();
00160     break;
00161   case 13: cout << "Read Initial State\n";
00162     //ReadInitialState();
00163     break;
00164   case 14: cout << "Read Goal State\n";
00165     //ReadGoalState();
00166     break;
00167   case 15: cout << "Change File Path\n";
00168     //NewPathName();
00169     break;
00170   case 22: cout << "Explore\n";
00171     rrt->Explore();
00172     //U.DrawRRT();
00173     break;
00174   case 23: cout << "Plan\n";
00175     if (rrt->Plan(Path)) {
00176       cout << "Success\n";
00177     }
00178       //  U.DrawPath();
00179     //U.DrawRRT();
00180     break;
00181   case 24: cout << "Clear RRT\n";
00182     rrt->G.clear();
00183     rrt->G2.clear();
00184     break;
00185   case 31: cout << "2D RRT Projection\n";
00186     DrawRRT();
00187     break;
00188   case 32: cout << "3D RRT Projection\n";
00189     //DrawRRTNodes();
00190     break;
00191   case 33: cout << "Draw Voronoi Diagram\n";
00192     //DrawVoronoi();
00193     break;
00194   case 34: cout << "Draw Path\n";
00195     if (Path.length() >= 2) {
00196       R->StackBodies(Path,rrt->TimeList); 
00197     }
00198     else
00199       cout << "No Path\n";
00200     break;
00201   case 35: cout << "Animate Path\n";
00202     R->Animated = true;
00203     if ((Path.length() >= 2)||
00204         (R->FrameList.length() > 0))
00205       {
00206         R->Animate(Path,rrt->TimeList); 
00207       }
00208     else
00209       cout << "No Path\n";
00210     break;
00211   case 36: cout << "Reset Rendering Window(s)\n";
00212     R->Reset();
00213     break;
00214   case 41: cout << "Write 2D RRT Projection to PS\n";
00215     PSDrawRRT();
00216     break;
00217   case 42: cout << "Write Voronoi Edges to PS\n";
00218     //PSDrawVoronoi();
00219     break;
00220   case 43: cout << "Write Path to PS\n";
00221     //PSDrawPath();
00222     break;
00223   case 44: cout << "Write RRT\n";
00224     WriteRRT();
00225     break;
00226   case 45: cout << "Read RRT\n";
00227     ReadRRT();
00228     break;
00229   case 46: cout << "Write Animation Frames\n";
00230     WriteAnimationFrames();
00231     break;
00232   case 47: cout << "Read Animation Frames\n";
00233     ReadAnimationFrames();      
00234     break;
00235   case 48: cout << "Write Path\n";
00236     WritePath();
00237     break;
00238   case 49: cout << "Read Path\n";
00239     ReadPath();      
00240     break;
00241   case 50: cout << "Open Settings Menu\n";
00242     m.double_item("DeltaT",rrt->DeltaT);
00243     m.int_item("NumNodes",rrt->NumNodes);
00244     m.double_item("GapDist",rrt->GapDist);
00245     m.int_item("DrawIndexX",DrawIndexX);
00246     m.int_item("DrawIndexY",DrawIndexY);
00247     //m.double_item("GifSize",GifSize);
00248     //m.double_item("LineWidth",LineWidth);
00249     //m.double_item("PSLineWidth",PSLineWidth);
00250     //m.bool_item("Include RRTs in PS Path",DumpTrees);
00251     m.button("Done",51);
00252     m.display();
00253     m.read_mouse();
00254     m.close();
00255     if ((DrawIndexX < 0) || (DrawIndexX >= rrt->P->StateDim))
00256       DrawIndexX = 0;
00257     if ((DrawIndexY < 0) || (DrawIndexY >= rrt->P->StateDim))
00258       DrawIndexX = 1;
00259     break;
00260   case 51: cout << "Done\n";
00261     break;
00262 
00263   case 201: cout << "Switch to RRT Planner\n";
00264     ResetRRT();
00265     rrt = new RRT(rrt->P); // Keep the old model, change the planner
00266     break;
00267   case 202: cout << "Switch to RRTGoalBias Planner\n";
00268     ResetRRT();
00269     rrt = new RRTGoalBias(rrt->P); // Keep the old model, change the planner
00270     break;
00271   case 203: cout << "Switch to RRTGoalPull Planner\n";
00272     ResetRRT();
00273     rrt = new RRTGoalPull(rrt->P); // Keep the old model, change the planner
00274     break;
00275   case 205: cout << "Switch to RRTDual Planner\n";
00276     ResetRRT();
00277     rrt = new RRTDual(rrt->P); // Keep the old model, change the planner
00278     break;
00279   case 206: cout << "Switch to RRTExtExt Planner\n";
00280     ResetRRT();
00281     rrt = new RRTExtExt(rrt->P); // Keep the old model, change the planner
00282     break;
00283   case 207: cout << "Switch to RRTGoalZoom Planner\n";
00284     ResetRRT();
00285     rrt = new RRTGoalZoom(rrt->P); // Keep the old model, change the planner
00286     break;
00287   case 208: cout << "Switch to RRTPolar Planner\n";
00288     ResetRRT();
00289     rrt = new RRTPolar(rrt->P); // Keep the old model, change the planner
00290     break;
00291   case 209: cout << "Switch to RRTHull Planner\n";
00292     ResetRRT();
00293     rrt = new RRTHull(rrt->P);
00294     break;
00295   case 210: cout << "Switch to RRTExtCon (RRT-Connect) Planner\n";
00296     ResetRRT();
00297     rrt = new RRTExtCon(rrt->P);
00298     break;
00299   case 211: cout << "Switch to RRTConCon Planner\n";
00300     ResetRRT();
00301     rrt = new RRTConCon(rrt->P);
00302     break;
00303   case 212: cout << "Switch to RRTStar Planner\n";
00304     ResetRRT();
00305     rrt = new RRTStar(rrt->P);
00306     break;
00307   case 213: cout << "Switch to RRTHsu Planner\n";
00308     ResetRRT();
00309     rrt = new RRTHsu(rrt->P);
00310     break;
00311   case 214: cout << "Switch to RRTNaive Planner\n";
00312     ResetRRT();
00313     rrt = new RRTNaive(rrt->P);
00314     break;
00315 
00316   default: cout << "Function not implemented\n";
00317              break;
00318   }
00319 }
00320 
00321 
00322 
00323 void GuiRRT::WriteRRT()
00324 {
00325   string fname;
00326   fname = FilePath + "rrt";
00327   window m(200,63);
00328   m.string_item("Filename",fname);
00329   m.button("Done",51);
00330   m.display();
00331   m.read_mouse();
00332   m.close();
00333 
00334   ofstream fout(fname);
00335   rrt->G.write(fout);
00336   rrt->G2.write(fout);
00337 }
00338 
00339 
00340 
00341 void GuiRRT::ReadRRT()
00342 {
00343   string fname;
00344   fname = FilePath + "rrt";
00345   window m(200,63);
00346   m.string_item("Filename",fname);
00347   m.button("Done",51);
00348   m.display();
00349   m.read_mouse();
00350   m.close();
00351 
00352   ifstream fin(fname);
00353   rrt->G.read(fin);
00354   rrt->G2.read(fin);
00355   cout << "RRT from initial state has " << rrt->G.number_of_nodes() 
00356        << " nodes\n";
00357   cout << "RRT from goal state has " << rrt->G2.number_of_nodes() 
00358        << " nodes\n";
00359 }
00360 
00361 
00362 
00363 void GuiRRT::WriteAnimationFrames()
00364 {
00365   string fname;
00366   fname = FilePath + "frames";
00367   window m(200,63);
00368   m.string_item("Filename",fname);
00369   m.button("Done",51);
00370   m.display();
00371   m.read_mouse();
00372   m.close();
00373 
00374   R->Animate(Path,rrt->TimeList); 
00375   ofstream fout(fname);
00376   fout << R->FrameList;
00377   fout.close();
00378   ofstream fout2(string(fname+".times"));
00379   fout2 << R->TimeList;
00380   fout2.close();
00381 }
00382 
00383 
00384 
00385 void GuiRRT::ReadAnimationFrames()
00386 {
00387   string fname;
00388   fname = FilePath + "frames";
00389   window m(200,63);
00390   m.string_item("Filename",fname);
00391   m.button("Done",51);
00392   m.display();
00393   m.read_mouse();
00394   m.close();
00395 
00396   ifstream fin(fname);
00397   fin >> R->FrameList;
00398   fin.close();
00399   ifstream fin2(string(fname+".times"));
00400   fin2 >> R->TimeList;
00401   fin2.close();
00402 
00403   R->Animated = true;
00404 }
00405 
00406 
00407 void GuiRRT::WritePath()
00408 {
00409   string fname;
00410   fname = FilePath + "path";
00411   window m(200,63);
00412   m.string_item("Filename",fname);
00413   m.button("Done",51);
00414   m.display();
00415   m.read_mouse();
00416   m.close();
00417 
00418   ofstream fout(fname);
00419   fout << Path;
00420   fout.close();
00421   ofstream fout2(string(fname+".times"));
00422   fout2 << rrt->TimeList;
00423   fout2.close();
00424 }
00425 
00426 
00427 
00428 void GuiRRT::ReadPath()
00429 {
00430   string fname;
00431   fname = FilePath + "path";
00432   window m(200,63);
00433   m.string_item("Filename",fname);
00434   m.button("Done",51);
00435   m.display();
00436   m.read_mouse();
00437   m.close();
00438 
00439   ifstream fin(fname);
00440   fin >> Path;
00441   fin.close();
00442   ifstream fin2(string(fname+".times"));
00443   fin2 >> rrt->TimeList;
00444   fin2.close();
00445 
00446   R->Animated = true;
00447 }
00448 
00449 
00450 
00451 
00452 void GuiRRT::DrawRRT()
00453 {
00454   edge e;
00455   vector x1,x2;
00456   double tranx,trany,scalex,scaley;
00457 
00458   window dw(600,600,string("RRT Display Window:     x[%d] versus x[%d]",
00459                             DrawIndexY,DrawIndexX));
00460 
00461   dw.display();
00462   dw.set_line_width(LineWidth);
00463 
00464   // The default window coords are [0,100]x[0,100]
00465   scalex = 100.0/(rrt->P->UpperState[DrawIndexX] - 
00466                   rrt->P->LowerState[DrawIndexX]); 
00467   scaley = 100.0/(rrt->P->UpperState[DrawIndexY] - 
00468                   rrt->P->LowerState[DrawIndexY]); 
00469   tranx = -1.0 * scalex * rrt->P->LowerState[DrawIndexX];
00470   trany = -1.0 * scaley * rrt->P->LowerState[DrawIndexY];
00471 
00472   // Show first tree
00473   if (rrt->G.number_of_nodes() > 0) {
00474     x1 = rrt->G.inf(rrt->G.first_node());
00475     dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00476                  0.5,blue);
00477     forall_edges(e,rrt->G) {
00478       x1 = rrt->G.inf(rrt->G.source(e));
00479       x2 = rrt->G.inf(rrt->G.target(e));
00480       dw.draw_segment(x1[DrawIndexX]*scalex+tranx,
00481                       x1[DrawIndexY]*scaley+trany,
00482                       x2[DrawIndexX]*scalex+tranx,
00483                       x2[DrawIndexY]*scaley+trany,blue);
00484     }
00485   }
00486   // Show second tree (if it exists)
00487   if (rrt->G2.number_of_nodes() > 0) {
00488     x1 = rrt->G2.inf(rrt->G2.first_node());
00489     dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00490                  0.5,red);
00491     forall_edges(e,rrt->G2) {
00492       x1 = rrt->G2.inf(rrt->G2.source(e));
00493       x2 = rrt->G2.inf(rrt->G2.target(e));
00494       dw.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00495                       x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
00496                       red);
00497     }
00498   }
00499 
00500   dw.read_mouse();
00501   dw.close();
00502 }
00503 
00504 
00505 
00506 void GuiRRT::PSDrawRRT()
00507 {
00508   edge e;
00509   vector x1,x2;
00510   double tranx,trany,scalex,scaley;
00511   string fname;
00512 
00513   fname = FilePath + string("rrtedges.%d.%d.ps",DrawIndexY,DrawIndexX);
00514   window m(200,63);
00515   m.string_item("Filename",fname);
00516   m.button("Done",51);
00517   m.display();
00518   m.read_mouse();
00519   m.close();
00520 
00521   ps_file f(15.0,15.0,fname);
00522   //f.set_draw_bb(false);
00523   f.set_line_width(PSLineWidth);
00524 
00525   // The default window coords are [0,100]x[0,100]
00526   scalex = 100.0/(rrt->P->UpperState[DrawIndexX] - 
00527                   rrt->P->LowerState[DrawIndexX]); 
00528   scaley = 100.0/(rrt->P->UpperState[DrawIndexY] - 
00529                   rrt->P->LowerState[DrawIndexY]); 
00530   tranx = -1.0 * scalex * rrt->P->LowerState[DrawIndexX];
00531   trany = -1.0 * scaley * rrt->P->LowerState[DrawIndexY];
00532 
00533   // Show first tree
00534   if (rrt->G.number_of_nodes() > 0) {
00535     x1 = rrt->G.inf(rrt->G.first_node());
00536     f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00537                  0.5,blue);
00538     forall_edges(e,rrt->G) {
00539       x1 = rrt->G.inf(rrt->G.source(e));
00540       x2 = rrt->G.inf(rrt->G.target(e));
00541       f.draw_segment(x1[DrawIndexX]*scalex+tranx,
00542                      x1[DrawIndexY]*scaley+trany,
00543                      x2[DrawIndexX]*scalex+tranx,
00544                      x2[DrawIndexY]*scaley+trany,blue);
00545     }
00546   }
00547   // Show second tree (if it exists)
00548   if (rrt->G2.number_of_nodes() > 0) {
00549     x1 = rrt->G2.inf(rrt->G2.first_node());
00550     f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00551                 0.5,red);
00552     forall_edges(e,rrt->G2) {
00553       x1 = rrt->G2.inf(rrt->G2.source(e));
00554       x2 = rrt->G2.inf(rrt->G2.target(e));
00555       f.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00556                      x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
00557                       red);
00558     }
00559   }
00560 
00561   f.close();
00562 }
00563 
00564 
00565 
00566 #include <LEDA/UNDEFINE_NAMES.h>
00567 
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.