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

guiplanner.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 "guiplanner.h"
00024 #include "defs.h"
00025 
00026 #include "pixmaps/msl.xpm"
00027 
00028 #include <LEDA/REDEFINE_NAMES.h>
00029 
00031 //
00032 //  GuiPlanner class
00033 //
00035 
00036 GuiPlanner::GuiPlanner(Render *render, Planner *planner):Gui(render) {
00037   Pl = planner;
00038 
00039   LineWidth = 1.0;
00040   PSLineWidth = 1.0;
00041   DrawIndexX = 0;
00042   DrawIndexY = 1;
00043 
00044   PlanCtlWindowOn = false;
00045 
00046   CreateMenuWindow();
00047 }
00048 
00049 
00050 
00051 void GuiPlanner::Init() {
00052   list<vector> tpath;
00053   
00054   // Read the planner type, if exists
00055   if (is_file(Pl->P->FilePath + "RRT"))
00056     ButtonHandle(201);
00057   if (is_file(Pl->P->FilePath + "RRTGoalBias"))
00058     ButtonHandle(202);
00059   if (is_file(Pl->P->FilePath + "RRTGoalPull"))
00060     ButtonHandle(203);
00061   if (is_file(Pl->P->FilePath + "RRTCon"))
00062     ButtonHandle(204);
00063   if (is_file(Pl->P->FilePath + "RRTPolar"))
00064     ButtonHandle(208);
00065   if (is_file(Pl->P->FilePath + "RRTHull"))
00066     ButtonHandle(209);
00067   if (is_file(Pl->P->FilePath + "RRTStar"))
00068     ButtonHandle(212);
00069   if (is_file(Pl->P->FilePath + "RRTGoalZoom"))
00070     ButtonHandle(207);
00071   if (is_file(Pl->P->FilePath + "RRTDual")) 
00072     ButtonHandle(205);
00073   if (is_file(Pl->P->FilePath + "RRTExtExt"))
00074     ButtonHandle(206);
00075   if (is_file(Pl->P->FilePath + "RRTExtCon"))
00076     ButtonHandle(210);
00077   if (is_file(Pl->P->FilePath + "RRTConCon"))
00078     ButtonHandle(211);
00079   if (is_file(Pl->P->FilePath + "PlannerHsu"))
00080     ButtonHandle(213);
00081   if (is_file(Pl->P->FilePath + "RRTNaive"))
00082     ButtonHandle(214);    
00083   if (is_file(Pl->P->FilePath + "PRM"))
00084     ButtonHandle(230);    
00085   if (is_file(Pl->P->FilePath + "FDP"))
00086     ButtonHandle(240);    
00087   if (is_file(Pl->P->FilePath + "FDPStar"))
00088     ButtonHandle(241);    
00089   if (is_file(Pl->P->FilePath + "FDPBestFirst"))
00090     ButtonHandle(242);    
00091   if (is_file(Pl->P->FilePath + "FDPBi"))
00092     ButtonHandle(243);    
00093   
00094   Gui::Init();
00095 
00096   // Set up to animate a single frame initially
00097   tpath.clear();
00098   //tpath.push(Pl->P->GoalState);
00099   tpath.push(Pl->P->InitialState);
00100   R->MakeAnimationFrames(tpath, 1.0);   
00101   R->AnimationActive = true;
00102 }
00103 
00104 
00105 void GuiPlanner::CreateMenuWindow() {
00106   menu *m2,*m3,*m4,*mm2;
00107  
00108   W = new window(500,62,"MSL Planner, Iowa State University, 1998-2001");
00109   W->set_icon_pixrect(W->create_pixrect(msl_xpm));
00110   W->buttons_per_line(5);
00111   m2 = new menu();
00112   m3 = new menu();
00113   m4 = new menu();
00114   mm2 = new menu();
00115 
00116   W->button("Construct",22);
00117   W->button("Plan",23);
00118   W->button("Clear",24);
00119   W->button("PlanCtl",50);
00120   W->button("RenderCtl",60);
00121 
00122   mm2->button("RRT",201);
00123   mm2->button("RRTGoalBias",202);
00124   mm2->button("RRTGoalPull",203);
00125   mm2->button("RRTCon",204);
00126   mm2->button("RRTPolar",208);
00127   mm2->button("RRTHull",209);
00128   mm2->button("RRTStar",212);
00129   mm2->button("RRTGoalZoom",207);
00130   mm2->button("RRTDual",205);
00131   mm2->button("RRTExtExt",206);
00132   mm2->button("RRTExtCon",210);
00133   mm2->button("RRTConCon",211);
00134   mm2->button("PlannerHsu",213);
00135   mm2->button("RRTNaive",214);
00136   mm2->button("PRM",230);
00137   mm2->button("FDP",240);
00138   mm2->button("FDPStar",241);
00139   mm2->button("FDPBestFirst",242);
00140   mm2->button("FDPBi",243);
00141   W->button("Planner",*mm2);
00142 
00143   m3->button("2D Graph Projection",31);
00144   //m3->button("3D Graph Projection",32);
00145   //m3->button("Voronoi Diagram",33);
00146   W->button("Display",*m3);
00147 
00148   m4->button("2D Graph Proj. to PS",41);
00149   //m4->button("Voronoi Diagram to PS",42);
00150   m4->button("Write Graph(s)",44);
00151   m4->button("Read Graph(s)",45);
00152   m4->button("Write Path",48);
00153   m4->button("Read Path",49);
00154   m4->button("Write Animation Frames",46);
00155   m4->button("Read Animation Frames",47);
00156   W->button("File",*m4);
00157 
00158   W->button("Exit",-1,exit);
00159 
00160   W->display();
00161 }
00162 
00163 
00164 void GuiPlanner::ResetPlanner()
00165 {
00166   R->SetScene(new Scene(Pl->P,FilePath));
00167   Pl->Reset();
00168 }
00169 
00170 
00171 void GuiPlanner::ButtonHandle(int b){
00172   //cout << "button " << b << "\n";
00173   char *icon;
00174 
00175   switch (b) {
00176   case 12: cout << "Read Inputs\n";
00177     //ReadInputs();
00178     break;
00179   case 13: cout << "Read Initial State\n";
00180     //ReadInitialState();
00181     break;
00182   case 14: cout << "Read Goal State\n";
00183     //ReadGoalState();
00184     break;
00185   case 15: cout << "Change File Path\n";
00186     //NewPathName();
00187     break;
00188   case 22: cout << "Construct\n";
00189     Pl->Construct();
00190     //U.DrawGraphs();
00191     break;
00192   case 23: cout << "Plan\n";
00193     if (Pl->Plan()) {
00194       cout << "Success\n";
00195       // Make the animation frames
00196       if ((Pl->Path.length() >= 2)||
00197           (R->FrameList.length() > 0))
00198         {
00199           R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
00200         }
00201     }
00202     //U.DrawPath();
00203     //U.DrawGraphs();
00204     break;
00205   case 24: cout << "Clear Graphs\n";
00206     ResetPlanner();
00207     break;
00208   case 31: cout << "2D Graph Projection\n";
00209     DrawGraphs();
00210     break;
00211   case 32: cout << "3D Graph Projection\n";
00212     //DrawRRTNodes();
00213     break;
00214   case 33: cout << "Draw Voronoi Diagram\n";
00215     //DrawVoronoi();
00216     break;
00217   case 41: cout << "Write 2D Graph Projection to PS\n";
00218     PSDrawGraphs();
00219     break;
00220   case 42: cout << "Write Voronoi Edges to PS\n";
00221     //PSDrawVoronoi();
00222     break;
00223   case 43: cout << "Write Path to PS\n";
00224     //PSDrawPath();
00225     break;
00226   case 44: cout << "Write Graph(s)\n";
00227     WriteGraphs();
00228     break;
00229   case 45: cout << "Read Graph(s)\n";
00230     ReadGraphs();
00231     break;
00232   case 46: cout << "Write Animation Frames\n";
00233     WriteAnimationFrames();
00234     break;
00235   case 47: cout << "Read Animation Frames\n";
00236     ReadAnimationFrames();      
00237     break;
00238   case 48: cout << "Write Path\n";
00239     WritePath();
00240     break;
00241   case 49: cout << "Read Path\n";
00242     ReadPath();      
00243     break;
00244   case 50:
00245     cout << "Toggle Planner Control Window\n";
00246     PlanCtlWindowOn = !PlanCtlWindowOn;
00247     if (PlanCtlWindowOn) {
00248       PlanCtlWindow = new window(200,155,"MSL: Planner Control Window");
00249       icon = PlanCtlWindow->create_pixrect(msl_xpm);
00250       PlanCtlWindow->set_icon_pixrect(icon);
00251       PlanCtlWindow->double_item("PlannerDeltaT",Pl->PlannerDeltaT);
00252       PlanCtlWindow->int_item("NumNodes",Pl->NumNodes);
00253       PlanCtlWindow->int_item("DrawIndexX",DrawIndexX);
00254       PlanCtlWindow->int_item("DrawIndexY",DrawIndexY);
00255       PlanCtlWindow->double_item("LineWidth",LineWidth);
00256       PlanCtlWindow->double_item("PSLineWidth",PSLineWidth);
00257       PlanCtlWindow->display();
00258     }
00259     else {
00260       PlanCtlWindow->close();
00261     }
00262     if ((DrawIndexX < 0) || (DrawIndexX >= Pl->P->StateDim))
00263       DrawIndexX = 0;
00264     if ((DrawIndexY < 0) || (DrawIndexY >= Pl->P->StateDim))
00265       DrawIndexX = 1;
00266     break;
00267   case 51: cout << "Done\n";
00268     Finished = true;  // This should trigger leaving (for most renderers)
00269     break;
00270   case 60: 
00271     cout << "Toggle Render Control Window\n";
00272     R->ToggleRenderCtlWindow();
00273     break;
00274   case 70: 
00275     cout << "Test";
00276     R->ToggleRenderCtlWindow();
00277     break;
00278   case 201: cout << "Switch to RRT Planner\n";
00279     ResetPlanner();
00280     Pl = new RRT(Pl->P); // Keep the old model, change the planner
00281     break;
00282   case 202: cout << "Switch to RRTGoalBias Planner\n";
00283     ResetPlanner();
00284     Pl = new RRTGoalBias(Pl->P); // Keep the old model, change the planner
00285     break;
00286   case 203: cout << "Switch to RRTGoalPull Planner\n";
00287     ResetPlanner();
00288     Pl = new RRTGoalPull(Pl->P); // Keep the old model, change the planner
00289     break;
00290   case 204: cout << "Switch to RRTCon Planner\n";
00291     ResetPlanner();
00292     Pl = new RRTCon(Pl->P); // Keep the old model, change the planner
00293     break;
00294   case 205: cout << "Switch to RRTDual Planner\n";
00295     ResetPlanner();
00296     Pl = new RRTDual(Pl->P); // Keep the old model, change the planner
00297     break;
00298   case 206: cout << "Switch to RRTExtExt Planner\n";
00299     ResetPlanner();
00300     Pl = new RRTExtExt(Pl->P); // Keep the old model, change the planner
00301     break;
00302   case 207: cout << "Switch to RRTGoalZoom Planner\n";
00303     ResetPlanner();
00304     Pl = new RRTGoalZoom(Pl->P); // Keep the old model, change the planner
00305     break;
00306   case 208: cout << "Switch to RRTPolar Planner\n";
00307     ResetPlanner();
00308     Pl = new RRTPolar(Pl->P); // Keep the old model, change the planner
00309     break;
00310   case 209: cout << "Switch to RRTHull Planner\n";
00311     ResetPlanner();
00312     Pl = new RRTHull(Pl->P);
00313     break;
00314   case 210: cout << "Switch to RRTExtCon (RRT-Connect) Planner\n";
00315     ResetPlanner();
00316     Pl = new RRTExtCon(Pl->P);
00317     break;
00318   case 211: cout << "Switch to RRTConCon Planner\n";
00319     ResetPlanner();
00320     Pl = new RRTConCon(Pl->P);
00321     break;
00322   case 212: cout << "Switch to RRTStar Planner\n";
00323     ResetPlanner();
00324     Pl = new RRTStar(Pl->P);
00325     break;
00326   case 213: cout << "Switch to PlannerHsu Planner\n";
00327     ResetPlanner();
00328     Pl = new PlannerHsu(Pl->P);
00329     break;
00330   case 214: cout << "Switch to RandomTree Planner\n";
00331     ResetPlanner();
00332     Pl = new RandomTree(Pl->P);
00333     break;
00334   case 230: cout << "Switch to PRM Planner\n";
00335     ResetPlanner();
00336     Pl = new PRM(Pl->P);
00337     break;
00338   case 240: cout << "Switch to FDP Planner\n";
00339     ResetPlanner();
00340     Pl = new FDP(Pl->P);
00341     break;
00342   case 241: cout << "Switch to FDPStar Planner\n";
00343     ResetPlanner();
00344     Pl = new FDPStar(Pl->P);
00345     break;
00346   case 242: cout << "Switch to FDPBestFirst Planner\n";
00347     ResetPlanner();
00348     Pl = new FDPBestFirst(Pl->P);
00349     break;
00350   case 243: cout << "Switch to FDPBi Planner\n";
00351     ResetPlanner();
00352     Pl = new FDPBi(Pl->P);
00353     break;
00354 
00355   default: cout << "Function not implemented\n";
00356     break;
00357   }
00358 }
00359 
00360 
00361 
00362 void GuiPlanner::WriteGraphs()
00363 {
00364   string fname;
00365   fname = FilePath + "graph";
00366   window m(200,63);
00367   m.string_item("Filename",fname);
00368   m.button("Done",51);
00369   m.display();
00370   m.read_mouse();
00371   m.close();
00372 
00373   Pl->WriteGraphs(fname);
00374 }
00375 
00376 
00377 
00378 void GuiPlanner::ReadGraphs()
00379 {
00380   string fname;
00381   fname = FilePath + "graph";
00382   window m(200,63);
00383   m.string_item("Filename",fname);
00384   m.button("Done",51);
00385   m.display();
00386   m.read_mouse();
00387   m.close();
00388 
00389   Pl->ReadGraphs(fname);
00390 }
00391 
00392 
00393 
00394 void GuiPlanner::WriteAnimationFrames()
00395 {
00396   string fname;
00397   fname = FilePath + "frames";
00398   window m(200,63);
00399   m.string_item("Filename",fname);
00400   m.button("Done",51);
00401   m.display();
00402   m.read_mouse();
00403   m.close();
00404 
00405   R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
00406   ofstream fout(fname);
00407   fout << R->FrameList;
00408   fout.close();
00409 }
00410 
00411 
00412 
00413 void GuiPlanner::ReadAnimationFrames()
00414 {
00415   string fname;
00416   fname = FilePath + "frames";
00417   window m(200,63);
00418   m.string_item("Filename",fname);
00419   m.button("Done",51);
00420   m.display();
00421   m.read_mouse();
00422   m.close();
00423 
00424   ifstream fin(fname);
00425   fin >> R->FrameList;
00426   fin.close();
00427 
00428   R->AnimationActive = true;
00429 }
00430 
00431 
00432 void GuiPlanner::WritePath()
00433 {
00434   string fname;
00435   fname = FilePath + "path";
00436   window m(200,63);
00437   m.string_item("Filename",fname);
00438   m.button("Done",51);
00439   m.display();
00440   m.read_mouse();
00441   m.close();
00442 
00443   ofstream fout(fname);
00444   fout << Pl->Path;
00445   fout.close();
00446   ofstream fout2(string(fname+".times"));
00447   fout2 << Pl->TimeList;
00448   fout2.close();
00449 }
00450 
00451 
00452 
00453 void GuiPlanner::ReadPath()
00454 {
00455   string fname;
00456   fname = FilePath + "path";
00457   window m(200,63);
00458   m.string_item("Filename",fname);
00459   m.button("Done",51);
00460   m.display();
00461   m.read_mouse();
00462   m.close();
00463 
00464   ifstream fin(fname);
00465   fin >> Pl->Path;
00466   fin.close();
00467   ifstream fin2(string(fname+".times"));
00468   fin2 >> Pl->TimeList;
00469   fin2.close();
00470 
00471   R->AnimationActive = true;
00472 }
00473 
00474 
00475 
00476 void GuiPlanner::DrawGraphs()
00477 {
00478   edge e;
00479   vector x1,x2;
00480   double tranx,trany,scalex,scaley;
00481 
00482   window dw(600,600,string("RRT Display Window:     x[%d] versus x[%d]",
00483                             DrawIndexY,DrawIndexX));
00484 
00485   dw.display();
00486   dw.set_line_width(LineWidth);
00487 
00488   // The default window coords are [0,100]x[0,100]
00489   scalex = 100.0/(Pl->P->UpperState[DrawIndexX] - 
00490                   Pl->P->LowerState[DrawIndexX]); 
00491   scaley = 100.0/(Pl->P->UpperState[DrawIndexY] - 
00492                   Pl->P->LowerState[DrawIndexY]); 
00493   tranx = -1.0 * scalex * Pl->P->LowerState[DrawIndexX];
00494   trany = -1.0 * scaley * Pl->P->LowerState[DrawIndexY];
00495 
00496   // Show first tree
00497   if (Pl->G.number_of_nodes() > 0) {
00498     x1 = Pl->G.inf(Pl->G.first_node());
00499     dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00500                  0.5,blue);
00501     forall_edges(e,Pl->G) {
00502       x1 = Pl->G.inf(Pl->G.source(e));
00503       x2 = Pl->G.inf(Pl->G.target(e));
00504       dw.draw_segment(x1[DrawIndexX]*scalex+tranx,
00505                       x1[DrawIndexY]*scaley+trany,
00506                       x2[DrawIndexX]*scalex+tranx,
00507                       x2[DrawIndexY]*scaley+trany,blue);
00508     }
00509   }
00510   // Show second tree (if it exists)
00511   if (Pl->G2.number_of_nodes() > 0) {
00512     x1 = Pl->G2.inf(Pl->G2.first_node());
00513     dw.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00514                  0.5,red);
00515     forall_edges(e,Pl->G2) {
00516       x1 = Pl->G2.inf(Pl->G2.source(e));
00517       x2 = Pl->G2.inf(Pl->G2.target(e));
00518       dw.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00519                       x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
00520                       red);
00521     }
00522   }
00523 
00524   dw.read_mouse();
00525   dw.close();
00526 }
00527 
00528 
00529 
00530 void GuiPlanner::PSDrawGraphs()
00531 {
00532   edge e;
00533   vector x1,x2;
00534   double tranx,trany,scalex,scaley;
00535   string fname;
00536 
00537   fname = FilePath + string("rrtedges.%d.%d.ps",DrawIndexY,DrawIndexX);
00538   window m(200,63);
00539   m.string_item("Filename",fname);
00540   m.button("Done",51);
00541   m.display();
00542   m.read_mouse();
00543   m.close();
00544 
00545   ps_file f(15.0,15.0,fname);
00546   //f.set_draw_bb(false);
00547   f.set_line_width(PSLineWidth);
00548 
00549   // The default window coords are [0,100]x[0,100]
00550   scalex = 100.0/(Pl->P->UpperState[DrawIndexX] - 
00551                   Pl->P->LowerState[DrawIndexX]); 
00552   scaley = 100.0/(Pl->P->UpperState[DrawIndexY] - 
00553                   Pl->P->LowerState[DrawIndexY]); 
00554   tranx = -1.0 * scalex * Pl->P->LowerState[DrawIndexX];
00555   trany = -1.0 * scaley * Pl->P->LowerState[DrawIndexY];
00556 
00557   // Show first tree
00558   if (Pl->G.number_of_nodes() > 0) {
00559     x1 = Pl->G.inf(Pl->G.first_node());
00560     f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00561                  0.5,blue);
00562     forall_edges(e,Pl->G) {
00563       x1 = Pl->G.inf(Pl->G.source(e));
00564       x2 = Pl->G.inf(Pl->G.target(e));
00565       f.draw_segment(x1[DrawIndexX]*scalex+tranx,
00566                      x1[DrawIndexY]*scaley+trany,
00567                      x2[DrawIndexX]*scalex+tranx,
00568                      x2[DrawIndexY]*scaley+trany,blue);
00569     }
00570   }
00571   // Show second tree (if it exists)
00572   if (Pl->G2.number_of_nodes() > 0) {
00573     x1 = Pl->G2.inf(Pl->G2.first_node());
00574     f.draw_disc(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00575                 0.5,red);
00576     forall_edges(e,Pl->G2) {
00577       x1 = Pl->G2.inf(Pl->G2.source(e));
00578       x2 = Pl->G2.inf(Pl->G2.target(e));
00579       f.draw_segment(x1[DrawIndexX]*scalex+tranx,x1[DrawIndexY]*scaley+trany,
00580                      x2[DrawIndexX]*scalex+tranx,x2[DrawIndexY]*scaley+trany,
00581                       red);
00582     }
00583   }
00584 
00585   f.close();
00586 }
00587 
00588 
00589 
00590 #include <LEDA/UNDEFINE_NAMES.h>
00591 
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: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.