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 "guiplanner.h"
00020 
00021 
00022 // *********************************************************************
00023 // *********************************************************************
00024 // CLASS:     MSLPlannerWindow
00025 // 
00026 // *********************************************************************
00027 // *********************************************************************
00028 
00029 // Map
00030 FXDEFMAP(MSLPlannerWindow) MSLPlannerWindowMap[]={
00031   FXMAPFUNC(SEL_TIMEOUT,FXMainWindow::ID_LAST,MSLPlannerWindow::onCmdTimer),
00032   FXMAPFUNCS(SEL_COMMAND, 20, 300, MSLPlannerWindow::GeneralHandler)
00033 };
00034 
00035 
00036 // Object implementation
00037 FXIMPLEMENT(MSLPlannerWindow,FXMainWindow,MSLPlannerWindowMap,ARRAYNUMBER(MSLPlannerWindowMap))
00038   
00039 // Make some windows
00040 MSLPlannerWindow::MSLPlannerWindow(GuiPlanner* gp):FXMainWindow(gp,"Motion Strategy Library",NULL,NULL,DECOR_ALL,20,20,500,260) {
00041 
00042   GP = gp;
00043   
00044   // Make the target connections
00045   Restart();
00046 
00047   // Menubar
00048   menubar=new FXMenubar(this,LAYOUT_SIDE_TOP|LAYOUT_FILL_X);
00049   plannermenu = new FXMenuPane(this);
00050     new FXMenuCommand(plannermenu,"RRT",NULL,this,201);
00051     new FXMenuCommand(plannermenu,"RRTGoalBias",NULL,this,202);
00052     new FXMenuCommand(plannermenu,"RRTCon",NULL,this,204);
00053     new FXMenuCommand(plannermenu,"RRTDual",NULL,this,205);
00054     new FXMenuCommand(plannermenu,"RRTExtExt",NULL,this,206);
00055     new FXMenuCommand(plannermenu,"RRTExtCon",NULL,this,210);
00056     new FXMenuCommand(plannermenu,"RRTConCon",NULL,this,211);
00057     new FXMenuCommand(plannermenu,"PRM",NULL,this,230);
00058     new FXMenuCommand(plannermenu,"FDP",NULL,this,240);
00059     new FXMenuCommand(plannermenu,"FDPStar",NULL,this,241);
00060     new FXMenuCommand(plannermenu,"FDPBestFirst",NULL,this,242);
00061     new FXMenuCommand(plannermenu,"FDPBi",NULL,this,243);
00062   new FXMenuTitle(menubar,"&Planner",NULL,plannermenu);
00063 
00064   loadmenu=new FXMenuPane(this);
00065     new FXMenuCommand(loadmenu,"Load Graph(s)",NULL,this,45,0);
00066     new FXMenuCommand(loadmenu,"Load Path",NULL,this,49,0);
00067     new FXMenuCommand(loadmenu,"Load Animation Frames",NULL,this,47,0);
00068   new FXMenuTitle(menubar,"&Load",NULL,loadmenu);
00069 
00070   savemenu=new FXMenuPane(this);
00071     new FXMenuCommand(savemenu,"Save Graph(s)",NULL,this,44,0);
00072     new FXMenuCommand(savemenu,"Save Path",NULL,this,48,0);
00073     new FXMenuCommand(savemenu,"Save Animation Frames",NULL,this,46,0);
00074   new FXMenuTitle(menubar,"&Save",NULL,savemenu);
00075 
00076   plotmenu=new FXMenuPane(this);
00077     new FXMenuCommand(plotmenu,"Draw 2D Graphs",NULL,this,31,0);
00078   new FXMenuTitle(menubar,"Plo&t",NULL,plotmenu);
00079 
00080   new FXHorizontalSeparator(this,LAYOUT_SIDE_TOP|SEPARATOR_GROOVE|LAYOUT_FILL_X);
00081 
00082   // Buttons
00083   buttonbar=new FXMenubar(this,LAYOUT_SIDE_TOP|LAYOUT_FILL_X);
00084   new FXButton(buttonbar,"&Construct",NULL,this,22,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00085   new FXButton(buttonbar,"P&lan",NULL,this,23,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00086   new FXButton(buttonbar,"C&lear",NULL,this,24,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00087   new FXButton(buttonbar,"&Quit",NULL,this,51,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00088 
00089   new FXHorizontalSeparator(this,LAYOUT_SIDE_TOP|SEPARATOR_GROOVE|LAYOUT_FILL_X);
00090   
00091   new FXLabel(this," Planner Controls                                    Animation Controls",
00092     NULL,LAYOUT_SIDE_TOP|LAYOUT_FILL_X|JUSTIFY_LEFT);
00093     
00094   new FXHorizontalSeparator(this,LAYOUT_SIDE_TOP|SEPARATOR_GROOVE|LAYOUT_FILL_X);
00095 
00096   vcrbar=new FXMenubar(this,LAYOUT_SIDE_BOTTOM|LAYOUT_FILL_X);
00097   new FXLabel(vcrbar,"Movie Buttons",NULL,LAYOUT_SIDE_TOP|LAYOUT_FILL_X|JUSTIFY_LEFT);
00098   new FXButton(vcrbar,"Stop",NULL,this,71,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00099   new FXButton(vcrbar,"Last",NULL,this,76,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00100   new FXButton(vcrbar,"Pause",NULL,this,72,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00101   new FXButton(vcrbar,"Next",NULL,this,77,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00102   new FXButton(vcrbar,"Slower",NULL,this,73,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00103   new FXButton(vcrbar,"Play",NULL,this,70,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00104   new FXButton(vcrbar,"Faster",NULL,this,74,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00105   new FXButton(vcrbar,"Reset",NULL,this,75,FRAME_RAISED|FRAME_THICK|LAYOUT_FILL_X);
00106 
00107   // Arange nicely
00108   matrix=new FXMatrix(this,5,MATRIX_BY_COLUMNS|LAYOUT_SIDE_TOP|LAYOUT_FILL_X|LAYOUT_FILL_Y);
00109   
00110   new FXLabel(matrix,"PlannerDeltaT",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00111   new FXTextField(matrix,10,&plannerdeltat_target,FXDataTarget::ID_VALUE,TEXTFIELD_REAL|JUSTIFY_RIGHT|LAYOUT_CENTER_Y|LAYOUT_CENTER_X|FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
00112   new FXFrame(matrix,LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW);
00113   new FXLabel(matrix,"AnimationTimeScale",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00114   FXSlider* fsl = new FXSlider(matrix,&animationtimescale_target,FXDataTarget::ID_VALUE,LAYOUT_CENTER_Y|LAYOUT_FILL_ROW|LAYOUT_FIX_WIDTH,0,0,100);
00115   fsl->setRange(1,100);
00116 
00117   new FXLabel(matrix,"NumNodes",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00118   new FXTextField(matrix,10,&numnodes_target,FXDataTarget::ID_VALUE,TEXTFIELD_INTEGER|JUSTIFY_RIGHT|LAYOUT_CENTER_Y|LAYOUT_CENTER_X|FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
00119   new FXFrame(matrix,LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW);
00120   new FXFrame(matrix,LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW);
00121   //new FXLabel(matrix,"AmbientLight",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00122   new FXTextField(matrix,10,&animationtimescale_target,FXDataTarget::ID_VALUE,TEXTFIELD_INTEGER|JUSTIFY_RIGHT|LAYOUT_CENTER_Y|LAYOUT_CENTER_X|FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
00123   //new FXSlider(matrix,&ambientlight_target,FXDataTarget::ID_VALUE,LAYOUT_CENTER_Y|LAYOUT_FILL_ROW|LAYOUT_FIX_WIDTH,0,0,100);
00124 
00125   new FXLabel(matrix,"DrawIndexX",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00126   new FXTextField(matrix,10,&drawindexx_target,FXDataTarget::ID_VALUE,TEXTFIELD_INTEGER|JUSTIFY_RIGHT|LAYOUT_CENTER_Y|LAYOUT_CENTER_X|FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
00127   //  new FXSpinner(matrix,5,&drawindexx_target,0,SPIN_CYCLIC|FRAME_SUNKEN|FRAME_THICK|LAYOUT_CENTER_Y|LAYOUT_FILL_ROW);
00128   new FXFrame(matrix,LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW);
00129 
00130   new FXToggleButton(matrix,"Bounding Box OFF","Bounding Box ON",NULL,NULL,this,83,LAYOUT_CENTER_Y|LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW|ICON_BEFORE_TEXT);
00131   new FXToggleButton(matrix,"Multiple Views OFF","Multiple Views ON",NULL,NULL,this,82,LAYOUT_CENTER_Y|LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW|ICON_BEFORE_TEXT);
00132 
00133   new FXLabel(matrix,"DrawIndexY",NULL,LAYOUT_CENTER_Y|LAYOUT_CENTER_X|JUSTIFY_RIGHT|LAYOUT_FILL_ROW);
00134   new FXTextField(matrix,10,&drawindexy_target,FXDataTarget::ID_VALUE,TEXTFIELD_INTEGER|JUSTIFY_RIGHT|LAYOUT_CENTER_Y|LAYOUT_CENTER_X|FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_ROW);
00135   //new FXSpinner(matrix,5,&drawindexy_target,1,SPIN_CYCLIC|FRAME_SUNKEN|FRAME_THICK|LAYOUT_CENTER_Y|LAYOUT_FILL_ROW);
00136   new FXFrame(matrix,LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW);
00137   new FXToggleButton(matrix,"Attached Camera OFF","Attached Camera ON",NULL,NULL,this,81,LAYOUT_CENTER_Y|LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW|ICON_BEFORE_TEXT);
00138   new FXToggleButton(matrix,"Show Path OFF","Show Path ON",NULL,NULL,this,80,LAYOUT_CENTER_Y|LAYOUT_FILL_COLUMN|LAYOUT_FILL_ROW|ICON_BEFORE_TEXT);
00139 
00140   new FXHorizontalSeparator(this,LAYOUT_SIDE_TOP|SEPARATOR_GROOVE|LAYOUT_FILL_X);
00141 
00142   // Install an accelerator
00143   getAccelTable()->addAccel(fxparseaccel("Ctl-Q"),getApp(),MKUINT(-1,SEL_COMMAND));
00144   }
00145 
00146 
00147 // Clean up
00148 MSLPlannerWindow::~MSLPlannerWindow(){
00149   delete loadmenu;
00150   delete savemenu;
00151   delete plotmenu;
00152   delete plannermenu;
00153   }
00154 
00155 
00156 // Timer  
00157 long MSLPlannerWindow::onCmdTimer(FXObject*,FXSelector,void*) {
00158   
00159   // Reset timer for next time
00160   getApp()->addTimeout(80,this,FXMainWindow::ID_LAST);
00161   return 1;
00162 }
00163 
00164 
00165   
00166 // Start
00167 void MSLPlannerWindow::create(){
00168   
00169   // Create windows
00170   FXMainWindow::create();
00171   
00172   // Kick off the timer
00173   getApp()->addTimeout(80,this,FXMainWindow::ID_LAST);
00174 
00175   // Show
00176   show(PLACEMENT_SCREEN);
00177 }
00178 
00179 
00180 
00181 void MSLPlannerWindow::Restart() {
00182   
00183   // Make the target connections
00184   plannerdeltat_target.connect(GP->Pl->PlannerDeltaT);
00185   numnodes_target.connect(GP->Pl->NumNodes);
00186   drawindexx_target.connect(GP->DrawIndexX);
00187   drawindexy_target.connect(GP->DrawIndexY);
00188   animationtimescale_target.connect(GP->R->AnimationTimeScale);
00189   ambientlight_target.connect(GP->R->AmbientLight);
00190 }
00191 
00192 
00193 // Timer  
00194 long MSLPlannerWindow::GeneralHandler(FXObject*,FXSelector sel,void*) {
00195   int i = SELID(sel);
00196   //cout << "General Handler  -- selection " << i << "\n";
00197 
00198   GP->ButtonHandle(i);
00199 
00200   return 1;
00201 }
00202 
00203 
00204 
00205 
00206 // *********************************************************************
00207 // *********************************************************************
00208 // CLASS:     MSLPlotWindow
00209 // 
00210 // *********************************************************************
00211 // *********************************************************************
00212 
00213 
00214 
00215 // Message Map for the Plot Window class
00216 FXDEFMAP(MSLPlotWindow) MSLPlotWindowMap[]={
00217 
00218   //________Message_Type_____________________ID____________Message_Handler_______
00219   FXMAPFUNC(SEL_PAINT,MSLPlotWindow::ID_CANVAS, MSLPlotWindow::onPaint),
00220   FXMAPFUNC(SEL_COMMAND,MSLPlotWindow::ID_PRINT, MSLPlotWindow::onCmdPrint),
00221 };
00222 
00223 
00224 
00225 // Macro for the PlotApp class hierarchy implementation
00226 FXIMPLEMENT(MSLPlotWindow,FXDialogBox,MSLPlotWindowMap,ARRAYNUMBER(MSLPlotWindowMap))
00227 
00228 
00229 
00230 // Construct a MSLPlotWindow
00231 MSLPlotWindow::MSLPlotWindow(MSLPlannerWindow* owner):
00232   FXDialogBox(owner,"Plot Window",DECOR_ALL,0,0,500,500) {
00233 
00234   Owner = owner;
00235   GP = owner->GP;
00236   indexx = GP->DrawIndexX;
00237   indexy = GP->DrawIndexY;
00238 
00239   if ((indexx >= GP->Pl->P->StateDim)||(indexx < 0))
00240     indexx = 0;
00241   if ((indexy >= GP->Pl->P->StateDim)||(indexy < 0))
00242     indexy = 1;
00243 
00244   char* title = new char[80];
00245   sprintf(title,"Search Graph(s):   x[%d] vs. x[%d]",indexx,indexy);
00246 
00247   contents=new FXHorizontalFrame(this,LAYOUT_SIDE_TOP|LAYOUT_FILL_X|LAYOUT_FILL_Y,0,0,0,0, 0,0,0,0);
00248   
00249   // LEFT pane to contain the canvas
00250   canvasFrame=new FXVerticalFrame(contents,FRAME_SUNKEN|LAYOUT_FILL_X|LAYOUT_FILL_Y|LAYOUT_TOP|LAYOUT_LEFT,0,0,0,0,10,10,10,10);
00251   
00252     // Label above the canvas               
00253   new FXLabel(canvasFrame,title,NULL,JUSTIFY_CENTER_X|LAYOUT_FILL_X);
00254   
00255     // Horizontal divider line
00256   new FXHorizontalSeparator(canvasFrame,SEPARATOR_GROOVE|LAYOUT_FILL_X);
00257 
00258     // Drawing canvas
00259   canvas=new FXCanvas(canvasFrame,this,ID_CANVAS,FRAME_SUNKEN|FRAME_THICK|LAYOUT_FILL_X|LAYOUT_FILL_Y|LAYOUT_TOP|LAYOUT_LEFT);
00260 
00261   // RIGHT pane for the buttons
00262   buttonFrame=new FXVerticalFrame(contents,FRAME_SUNKEN|LAYOUT_FILL_Y|LAYOUT_TOP|LAYOUT_LEFT,0,0,0,0,10,10,10,10);
00263 
00264     // Label above the buttons  
00265     new FXLabel(buttonFrame,"Options",NULL,JUSTIFY_CENTER_X|LAYOUT_FILL_X);
00266     
00267     // Horizontal divider line
00268     new FXHorizontalSeparator(buttonFrame,SEPARATOR_RIDGE|LAYOUT_FILL_X);
00269 
00270     // Button to print
00271     new FXButton(buttonFrame,"&Print",NULL,this,ID_PRINT,FRAME_THICK|FRAME_RAISED|LAYOUT_FILL_X|LAYOUT_TOP|LAYOUT_LEFT,0,0,0,0,10,10,5,5);
00272 
00273     // Exit button
00274     new FXButton(buttonFrame,"&Exit",NULL,this,ID_HIDE,FRAME_THICK|FRAME_RAISED|LAYOUT_FILL_X|LAYOUT_TOP|LAYOUT_LEFT,0,0,0,0,10,10,5,5);
00275 
00276 }
00277     
00278  
00279 
00280 // Paint the canvas
00281 long MSLPlotWindow::onPaint(FXObject*,FXSelector,void* ptr){
00282   FXEvent *ev=(FXEvent*)ptr;
00283   FXDCWindow dc(canvas,ev);
00284   dc.setForeground(canvas->getBackColor());
00285   dc.fillRectangle(ev->rect.x,ev->rect.y,ev->rect.w,ev->rect.h);
00286   drawPage(dc,canvas->getWidth(),canvas->getHeight());
00287   return 1;
00288 }
00289 
00290 
00291 long MSLPlotWindow::onCmdPrint(FXObject*,FXSelector,void*){
00292   FXPrintDialog dlg(this,"Print Graphics");
00293   FXPrinter printer;
00294   if(dlg.execute()){
00295     dlg.getPrinter(printer);
00296     FXTRACE((100,"Printer = %s\n",printer.name.text()));
00297     FXDCPrint pdc(getApp());
00298     if(!pdc.beginPrint(printer)) {
00299       FXMessageBox::error(this,MBOX_OK,"Printer Error","Unable to print");
00300       return 1;
00301     }
00302     pdc.beginPage(1);
00303     pdc.setLineWidth(1);
00304     drawPage(pdc,400,400,100,100);
00305     pdc.endPage();
00306     pdc.endPrint();
00307     }
00308   return 1;
00309 }
00310 
00311 
00312 
00313 // This is the WYSIWYG routine, it takes a DC and renders 
00314 // into it; it does not know if the DC is a printer or screen.
00315 void MSLPlotWindow::drawPage(FXDC& dc,FXint w,FXint h,
00316                              FXint tx = 0,FXint ty = 0) {
00317   list<MSLNode*> nlist;
00318   list<MSLNode*>::iterator ni;
00319   list<MSLEdge*> elist;
00320   list<MSLEdge*>::iterator ei;
00321   MSLVector x1,x2;
00322   double tranx,trany,scalex,scaley;
00323   int lw;
00324 
00325   //dc.setForeground(erasecolor);
00326   //dc.fillRectangle(0,0,w,h);
00327 
00328   //dc.setForeground(forecolor);
00329   //dc.setBackground(FXRGB(0,0,0));
00330   
00331   //dc.setLineStyle(lineStyle);
00332   //dc.setLineCap(capStyle);
00333   //dc.setLineJoin(joinStyle);
00334   //dc.setFunction(function);
00335   
00336   //dc.setStipple(stipple);
00337   //dc.setFillStyle(fillStyle);
00338   //dc.setLineWidth(lineWidthSpinner->getValue());
00339   lw = dc.getLineWidth();
00340   dc.setLineWidth(0);
00341 
00342   dc.setForeground(FXRGB(0,0,0));
00343   dc.drawRectangle(tx,ty,w,h);
00344 
00345   scalex = w/(GP->Pl->P->UpperState[indexx] - 
00346               GP->Pl->P->LowerState[indexx]); 
00347   scaley = -h/(GP->Pl->P->UpperState[indexy] - 
00348               GP->Pl->P->LowerState[indexy]); 
00349   tranx = tx-1.0 * scalex * GP->Pl->P->LowerState[indexx];
00350   trany = ty+h-1.0 * scaley * GP->Pl->P->LowerState[indexy];
00351 
00352   // Show path (if it exists)
00353   dc.setForeground(FXRGB(200,0,0));
00354   dc.setLineWidth(2);
00355   list<MSLVector> path;
00356   list<MSLVector>::iterator pi;
00357   path = GP->Pl->Path;
00358   if (path.size() >= 2) {
00359     x1 = path.front();
00360     pi = path.begin(); 
00361     pi++;
00362     while (pi != path.end()) {
00363       x2 = *pi;
00364       dc.drawLine(x1[indexx]*scalex+tranx,
00365                   x1[indexy]*scaley+trany,
00366                   x2[indexx]*scalex+tranx,
00367                   x2[indexy]*scaley+trany);
00368       x1 = x2;
00369       pi++;
00370     }
00371   }
00372 
00373   // Show first tree (if it exists)
00374   dc.setLineWidth(lw);
00375   dc.setForeground(FXRGB(0,0,255));
00376   if (GP->Pl->T) {
00377     nlist = GP->Pl->T->Nodes();
00378     forall(ni,nlist) {
00379       if ((*ni)->Parent()) {  // Make sure it has a parent to connect to!
00380         x1 = (*ni)->State();
00381         x2 = (*ni)->Parent()->State();
00382         dc.drawLine(x1[indexx]*scalex+tranx,
00383                     x1[indexy]*scaley+trany,
00384                     x2[indexx]*scalex+tranx,
00385                     x2[indexy]*scaley+trany);
00386       }
00387     }
00388   }
00389   // Show second tree (if it exists)
00390   dc.setForeground(FXRGB(255,0,0));
00391   if (GP->Pl->T2) {
00392     nlist = GP->Pl->T2->Nodes();
00393     forall(ni,nlist) {
00394       if ((*ni)->Parent()) {  // Make sure it has a parent to connect to!
00395         x1 = (*ni)->State();
00396         x2 = (*ni)->Parent()->State();
00397         dc.drawLine(x1[indexx]*scalex+tranx,
00398                     x1[indexy]*scaley+trany,
00399                     x2[indexx]*scalex+tranx,
00400                     x2[indexy]*scaley+trany);
00401       }
00402     }
00403   }
00404 
00405   // Show roadmap (if it exists)
00406   dc.setForeground(FXRGB(0,150,0));
00407   if (GP->Pl->Roadmap) {
00408     elist = GP->Pl->Roadmap->Edges();
00409     forall(ei,elist) {
00410       x1 = (*ei)->Source()->State();
00411       x2 = (*ei)->Target()->State();
00412       dc.drawLine(x1[indexx]*scalex+tranx,
00413                   x1[indexy]*scaley+trany,
00414                   x2[indexx]*scalex+tranx,
00415                   x2[indexy]*scaley+trany);
00416     }
00417   }
00418 
00419   // Restore original values
00420   dc.setLineWidth(lw);
00421 }
00422 
00423 
00424 
00425 // *********************************************************************
00426 // *********************************************************************
00427 // CLASS:     GuiPlanner
00428 // 
00429 // *********************************************************************
00430 // *********************************************************************
00431 
00432 GuiPlanner::GuiPlanner(Render *render, Planner *planner):Gui(render) {
00433   Pl = planner;
00434 
00435   LineWidth = 1.0;
00436   PSLineWidth = 1.0;
00437   DrawIndexX = 0;
00438   DrawIndexY = 1;
00439 
00440   if (!render) 
00441     cout << "ERROR: Renderer no defined\n";
00442 
00443   FilePath = Pl->P->FilePath;
00444 
00445   CreateMenuWindow();
00446 }
00447 
00448 
00449 
00450 void GuiPlanner::Init() {
00451   list<MSLVector> tpath;
00452   
00453   // Read the planner type, if exists
00454   if (is_file(Pl->P->FilePath + "RRT"))
00455     ButtonHandle(201);
00456   if (is_file(Pl->P->FilePath + "RRTGoalBias"))
00457     ButtonHandle(202);
00458   if (is_file(Pl->P->FilePath + "RRTGoalPull"))
00459     ButtonHandle(203);
00460   if (is_file(Pl->P->FilePath + "RRTCon"))
00461     ButtonHandle(204);
00462   if (is_file(Pl->P->FilePath + "RRTPolar"))
00463     ButtonHandle(208);
00464   if (is_file(Pl->P->FilePath + "RRTHull"))
00465     ButtonHandle(209);
00466   if (is_file(Pl->P->FilePath + "RRTStar"))
00467     ButtonHandle(212);
00468   if (is_file(Pl->P->FilePath + "RRTGoalZoom"))
00469     ButtonHandle(207);
00470   if (is_file(Pl->P->FilePath + "RRTDual")) 
00471     ButtonHandle(205);
00472   if (is_file(Pl->P->FilePath + "RRTExtExt"))
00473     ButtonHandle(206);
00474   if (is_file(Pl->P->FilePath + "RRTExtCon"))
00475     ButtonHandle(210);
00476   if (is_file(Pl->P->FilePath + "RRTConCon"))
00477     ButtonHandle(211);
00478   if (is_file(Pl->P->FilePath + "PlannerHsu"))
00479     ButtonHandle(213);
00480   if (is_file(Pl->P->FilePath + "RRTNaive"))
00481     ButtonHandle(214);    
00482   if (is_file(Pl->P->FilePath + "PRM"))
00483     ButtonHandle(230);    
00484   if (is_file(Pl->P->FilePath + "FDP"))
00485     ButtonHandle(240);    
00486   if (is_file(Pl->P->FilePath + "FDPStar"))
00487     ButtonHandle(241);    
00488   if (is_file(Pl->P->FilePath + "FDPBestFirst"))
00489     ButtonHandle(242);    
00490   if (is_file(Pl->P->FilePath + "FDPBi"))
00491     ButtonHandle(243);    
00492   
00493   Gui::Init();
00494 
00495   // Set up to animate a single frame initially
00496   tpath.clear();
00497   //tpath.push(Pl->P->GoalState);
00498   tpath.push_back(Pl->P->InitialState);
00499   R->MakeAnimationFrames(tpath, 1.0);   
00500   R->AnimationActive = true;
00501 }
00502 
00503 
00504 void GuiPlanner::CreateMenuWindow() {
00505   int argc;
00506   char **argv;
00507 
00508   // Make application
00509   //FXApp application("DataTarget","FoxTest");
00510   
00511   // Open display (multiple inheritance from FXApp)
00512   init(argc,argv);
00513   
00514   // Main window
00515   Window=new MSLPlannerWindow(this);
00516   
00517   // Handle interrupt to save stuff nicely (from FXApp)
00518   //addSignal(SIGINT,Window,FXApp::ID_QUIT);
00519 
00520   // Create app (from FXApp)
00521   create();
00522   
00523   // Run (from FXApp)
00524   //run();
00525 
00526 }
00527 
00528 
00529 void GuiPlanner::ResetPlanner()
00530 {
00531   R->SetScene(new Scene(Pl->P,FilePath));
00532   Pl->Reset();
00533   Window->Restart();
00534 }
00535 
00536 
00537 void GuiPlanner::ButtonHandle(int b){
00538   //cout << "button " << b << "\n";
00539   //char *icon;
00540   if ((b >= 70) && (b <= 90))
00541     R->ButtonHandle(b);
00542   else {
00543     switch (b) {
00544     case 22: cout << "Construct\n";
00545       Pl->Construct();
00546       //U.DrawGraphs();
00547       break;
00548     case 23: cout << "Plan\n";
00549       if (Pl->Plan()) {
00550         cout << "Making Animation Frames\n";
00551         // Make the animation frames
00552         if ((Pl->Path.size() >= 2)||
00553             (R->FrameList.size() > 0))
00554           {
00555           R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
00556           }
00557       }
00558       //U.DrawPath();
00559       //U.DrawGraphs();
00560       break;
00561     case 24: cout << "Clear Graphs\n";
00562       ResetPlanner();
00563       break;
00564     case 31: cout << "2D Graph Projection\n";
00565       DrawGraphs();
00566       break;
00567     case 32: cout << "3D Graph Projection\n";
00568       //DrawRRTNodes();
00569       break;
00570     case 33: cout << "Draw Voronoi Diagram\n";
00571       //DrawVoronoi();
00572       break;
00573     case 41: cout << "Write 2D Graph Projection to PS\n";
00574       //PSDrawGraphs();
00575       break;
00576     case 42: cout << "Write Voronoi Edges to PS\n";
00577       //PSDrawVoronoi();
00578       break;
00579     case 43: cout << "Write Path to PS\n";
00580       //PSDrawPath();
00581       break;
00582     case 44: cout << "Write Graph(s)\n";
00583       WriteGraphs();
00584       break;
00585     case 45: cout << "Read Graph(s)\n";
00586       ReadGraphs();
00587       break;
00588     case 46: cout << "Write Animation Frames\n";
00589       WriteAnimationFrames();
00590       break;
00591     case 47: cout << "Read Animation Frames\n";
00592       ReadAnimationFrames();      
00593       break;
00594     case 48: cout << "Write Path\n";
00595       WritePath();
00596       break;
00597     case 49: cout << "Read Path\n";
00598       ReadPath();      
00599       break;
00600     case 51: cout << "Done\n";
00601       Finished = true;  // This should trigger leaving (for most renderers)
00602       break;
00603     case 80: cout << "Toggle Show Path\n";
00604       R->ShowPathOn = !R->ShowPathOn; 
00605       break;
00606     case 201: cout << "Switch to RRT Planner\n";
00607       ResetPlanner();
00608       Pl = new RRT(Pl->P); // Keep the old model, change the planner
00609       break;
00610     case 202: cout << "Switch to RRTGoalBias Planner\n";
00611       ResetPlanner();
00612       Pl = new RRTGoalBias(Pl->P); // Keep the old model, change the planner
00613       break;
00614     case 203: cout << "Switch to RRTGoalPull Planner\n";
00615       ResetPlanner();
00616       //Pl = new RRTGoalPull(Pl->P); // Keep the old model, change the planner
00617       break;
00618     case 204: cout << "Switch to RRTCon Planner\n";
00619       ResetPlanner();
00620       Pl = new RRTCon(Pl->P); // Keep the old model, change the planner
00621       break;
00622     case 205: cout << "Switch to RRTDual Planner\n";
00623       ResetPlanner();
00624       Pl = new RRTDual(Pl->P); // Keep the old model, change the planner
00625       break;
00626     case 206: cout << "Switch to RRTExtExt Planner\n";
00627       ResetPlanner();
00628       Pl = new RRTExtExt(Pl->P); // Keep the old model, change the planner
00629       break;
00630     case 207: cout << "Switch to RRTGoalZoom Planner\n";
00631       ResetPlanner();
00632       //Pl = new RRTGoalZoom(Pl->P); // Keep the old model, change the planner
00633       break;
00634     case 208: cout << "Switch to RRTPolar Planner\n";
00635       ResetPlanner();
00636       //Pl = new RRTPolar(Pl->P); // Keep the old model, change the planner
00637       break;
00638     case 209: cout << "Switch to RRTHull Planner\n";
00639       ResetPlanner();
00640       //Pl = new RRTHull(Pl->P);
00641       break;
00642     case 210: cout << "Switch to RRTExtCon (RRT-Connect) Planner\n";
00643       ResetPlanner();
00644       Pl = new RRTExtCon(Pl->P);
00645       break;
00646     case 211: cout << "Switch to RRTConCon Planner\n";
00647       ResetPlanner();
00648       Pl = new RRTConCon(Pl->P);
00649       break;
00650     case 212: cout << "Switch to RRTStar Planner\n";
00651       ResetPlanner();
00652       //Pl = new RRTStar(Pl->P);
00653       break;
00654     case 213: cout << "Switch to PlannerHsu Planner\n";
00655       ResetPlanner();
00656       //Pl = new PlannerHsu(Pl->P);
00657       break;
00658     case 214: cout << "Switch to RandomTree Planner\n";
00659       ResetPlanner();
00660       //Pl = new RandomTree(Pl->P);
00661       break;
00662     case 230: cout << "Switch to PRM Planner\n";
00663       ResetPlanner();
00664       Pl = new PRM(Pl->P);
00665       break;
00666     case 240: cout << "Switch to FDP Planner\n";
00667       ResetPlanner();
00668       Pl = new FDP(Pl->P);
00669       break;
00670     case 241: cout << "Switch to FDPStar Planner\n";
00671       ResetPlanner();
00672       Pl = new FDPStar(Pl->P);
00673       break;
00674     case 242: cout << "Switch to FDPBestFirst Planner\n";
00675       ResetPlanner();
00676       Pl = new FDPBestFirst(Pl->P);
00677       break;
00678     case 243: cout << "Switch to FDPBi Planner\n";
00679       ResetPlanner();
00680       Pl = new FDPBi(Pl->P);
00681       break;
00682       
00683     default: cout << "Option " << b << " not implemented\n";
00684       break;
00685     }
00686   }
00687 }
00688 
00689 
00690 
00691 void GuiPlanner::HandleEvents() {
00692   runOneEvent();  // Run a FOX event (came from FXApp parent class)
00693 }
00694 
00695 
00696 
00697 void GuiPlanner::WriteGraphs()
00698 {
00699   FXFileDialog dialog(Window,"Write Solution Path");
00700   dialog.setDirectory(("./"+FilePath).c_str());
00701   dialog.setFilename("graphs");
00702   if (dialog.execute()) {
00703     std::ofstream outfile(dialog.getFilename().text()); 
00704     if (outfile) {   
00705       Pl->WriteGraphs(outfile);
00706       outfile.close();
00707     }
00708   }
00709 }
00710 
00711 
00712 
00713 void GuiPlanner::ReadGraphs()
00714 {
00715   FXFileDialog dialog(Window,"Read Animation Frames");
00716   dialog.setDirectory(("./"+FilePath).c_str());
00717   dialog.setFilename("graphs");
00718   if (dialog.execute()) {
00719     std::ifstream infile(dialog.getFilename().text()); 
00720     if (infile) {   
00721       Pl->ReadGraphs(infile);
00722       infile.close();
00723     }
00724   }
00725 }
00726 
00727 
00728 
00729 void GuiPlanner::WriteAnimationFrames()
00730 {
00731   FXFileDialog dialog(Window,"Write Animation Frames");
00732   dialog.setDirectory(("./"+FilePath).c_str());
00733   dialog.setFilename("frames");
00734   if (dialog.execute()) {
00735     std::ofstream outfile(dialog.getFilename().text()); 
00736     if (outfile) {   
00737       outfile << R->FrameList;
00738       outfile.close();
00739     }
00740   }
00741 }
00742 
00743 
00744 
00745 void GuiPlanner::ReadAnimationFrames()
00746 {
00747   FXFileDialog dialog(Window,"Read Animation Frames");
00748   dialog.setDirectory(("./"+FilePath).c_str());
00749   dialog.setFilename("frames");
00750   if (dialog.execute()) {
00751     std::ifstream infile(dialog.getFilename().text()); 
00752     if (infile) {   
00753       infile >> R->FrameList;
00754       infile.close();
00755     }
00756   }
00757 
00758   R->AnimationActive = true;
00759 }
00760 
00761 
00762 void GuiPlanner::WritePath()
00763 {
00764   FXFileDialog dialog(Window,"Write Solution Path");
00765   dialog.setDirectory(("./"+FilePath).c_str());
00766   dialog.setFilename("path");
00767   if (dialog.execute()) {
00768     std::ofstream outfile(dialog.getFilename().text()); 
00769     if (outfile) {   
00770       outfile << Pl->Path;
00771       outfile.close();
00772     }
00773   }
00774 }
00775 
00776 
00777 
00778 void GuiPlanner::ReadPath()
00779 {
00780   FXFileDialog dialog(Window,"Read Animation Frames");
00781   dialog.setDirectory(("./"+FilePath).c_str());
00782   dialog.setFilename("path");
00783   if (dialog.execute()) {
00784     std::ifstream infile(dialog.getFilename().text()); 
00785     if (infile) {   
00786       infile >> Pl->Path;
00787       infile.close();
00788     }
00789   }
00790 
00791   R->AnimationActive = true;
00792   R->MakeAnimationFrames(Pl->Path,Pl->TimeList); 
00793 }
00794 
00795 
00796 void GuiPlanner::DrawGraphs()
00797 {
00798   MSLPlotWindow *pw = new MSLPlotWindow(Window);
00799 
00800   //pw->execute();  This one is modal
00801   pw->create();
00802   pw->show();
00803 
00804   cout << "Draw Graphs\n";
00805 }
00806 
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.