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