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