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