00001 #include <math.h> 00002 #include <stdlib.h> 00003 00004 #include <LEDA/window.h> 00005 00006 #include <MSL/rrt.h> 00007 #include <MSL/model2d.h> 00008 00009 #include <LEDA/REDEFINE_NAMES.h> 00010 00011 00012 00013 void DrawPath(const list<vector> &path, Planner *Pl, window *w) 00014 { 00015 vector x1,x2; 00016 double tranx,trany,scalex,scaley; 00017 double lw; 00018 00019 lw = w->get_line_width(); 00020 w->set_line_width(5.0); 00021 00022 // The default window coords are [0,100]x[0,100] 00023 scalex = 100.0/(Pl->P->UpperState[0] - 00024 Pl->P->LowerState[0]); 00025 scaley = 100.0/(Pl->P->UpperState[1] - 00026 Pl->P->LowerState[1]); 00027 tranx = -1.0 * scalex * Pl->P->LowerState[0]; 00028 trany = -1.0 * scaley * Pl->P->LowerState[1]; 00029 00030 x1 = path.head(); 00031 forall(x2,path) { 00032 w->draw_segment(x1[0]*scalex+tranx, 00033 x1[1]*scaley+trany, 00034 x2[0]*scalex+tranx, 00035 x2[1]*scaley+trany,violet); 00036 x1 = x2; 00037 } 00038 00039 w->set_line_width(lw); 00040 } 00041 00042 00043 00044 00045 void DrawRRT(Planner *Pl, window *w) 00046 { 00047 edge e; 00048 vector x1,x2; 00049 double tranx,trany,scalex,scaley; 00050 00051 // The default window coords are [0,100]x[0,100] 00052 scalex = 100.0/(Pl->P->UpperState[0] - 00053 Pl->P->LowerState[0]); 00054 scaley = 100.0/(Pl->P->UpperState[1] - 00055 Pl->P->LowerState[1]); 00056 tranx = -1.0 * scalex * Pl->P->LowerState[0]; 00057 trany = -1.0 * scaley * Pl->P->LowerState[1]; 00058 00059 // Show first tree 00060 if (Pl->G.number_of_nodes() > 0) { 00061 forall_edges(e,Pl->G) { 00062 x1 = Pl->G.inf(Pl->G.source(e)); 00063 x2 = Pl->G.inf(Pl->G.target(e)); 00064 w->draw_segment(x1[0]*scalex+tranx, 00065 x1[1]*scaley+trany, 00066 x2[0]*scalex+tranx, 00067 x2[1]*scaley+trany,blue); 00068 } 00069 } 00070 // Show second tree (if it exists) 00071 if (Pl->G2.number_of_nodes() > 0) { 00072 forall_edges(e,Pl->G2) { 00073 x1 = Pl->G2.inf(Pl->G2.source(e)); 00074 x2 = Pl->G2.inf(Pl->G2.target(e)); 00075 w->draw_segment(x1[0]*scalex+tranx,x1[1]*scaley+trany, 00076 x2[0]*scalex+tranx,x2[1]*scaley+trany, 00077 red); 00078 } 00079 } 00080 } 00081 00082 00083 00084 00085 int main(int argc, char *argv[]) 00086 { 00087 string path = ""; 00088 window *w; 00089 Model *m = NULL; 00090 Geom *g = NULL; 00091 Problem *prob; 00092 RRT *rrt; 00093 int i; 00094 list<vector> solution; 00095 00096 // Initialize the window 00097 w = new window(600,600,"Rapidly-Exploring Random Trees"); 00098 w->set_line_width(2.0); 00099 w->display(); 00100 w->draw_rectangle(0.0,0.0,100.0,100.0,red); 00101 00102 00103 leda_wait(2.0); 00104 00105 cout << "Draw an RRT starting in the center of a box\n"; 00106 m = new Model2DPoint(path); // Define the differential model 00107 g = new GeomLedaPolygonal(path); 00108 prob = new Problem(g,m,path); // This is the input to a planner 00109 prob->InitialState = vector(50.0,50.0); 00110 prob->GoalState = vector(100.0,100.0); 00111 rrt = new RRT(prob); 00112 rrt->NumNodes = 50; 00113 rrt->PlannerDeltaT = 1.0; 00114 00115 for (i = 0; i < 15; i++) { 00116 rrt->Construct(); 00117 DrawRRT(rrt,w); 00118 leda_wait(0.2); 00119 } 00120 for (i = 0; i < 30; i++) { 00121 rrt->Construct(); 00122 DrawRRT(rrt,w); 00123 leda_wait(0.05); 00124 } 00125 00126 leda_wait(2.0); 00127 00128 cout << "Draw an RRT starting in the corner\n"; 00129 w->clear(); 00130 w->draw_rectangle(0.0,0.0,100.0,100.0,red); 00131 rrt->Reset(); 00132 rrt->NumNodes = 50; 00133 rrt->PlannerDeltaT = 1.0; 00134 prob->InitialState = vector(1.0,1.0); 00135 for (i = 0; i < 15; i++) { 00136 rrt->Construct(); 00137 DrawRRT(rrt,w); 00138 leda_wait(0.1); 00139 } 00140 for (i = 0; i < 30; i++) { 00141 rrt->Construct(); 00142 DrawRRT(rrt,w); 00143 leda_wait(0.05); 00144 } 00145 00146 leda_wait(2.0); 00147 00148 cout << "Draw an RRT for the car\n"; 00149 w->clear(); 00150 w->draw_rectangle(0.0,0.0,100.0,100.0,red); 00151 rrt->Reset(); 00152 rrt->NumNodes = 50; 00153 rrt->PlannerDeltaT = 1.0; 00154 m = new Model2DRigidCar(path); // Define the differential model 00155 prob->SetModel(m); 00156 prob->InitialState = vector(50.0,50.0,0.0); 00157 prob->GoalState = vector(100.0,50.0,0.0); 00158 for (i = 0; i < 50; i++) { 00159 rrt->Construct(); 00160 DrawRRT(rrt,w); 00161 leda_wait(0.05); 00162 } 00163 00164 leda_wait(2.0); 00165 00166 cout << "Draw an RRT for the forward-only car\n"; 00167 w->clear(); 00168 w->draw_rectangle(0.0,0.0,100.0,100.0,red); 00169 rrt->Reset(); 00170 rrt->NumNodes = 50; 00171 rrt->PlannerDeltaT = 1.0; 00172 m = new Model2DRigidCarForward(path); // Define the differential model 00173 prob->SetModel(m); 00174 prob->InitialState = vector(50.0,50.0,1.732); 00175 prob->GoalState = vector(100.0,50.0,1.732); 00176 for (i = 0; i < 50; i++) { 00177 rrt->Construct(); 00178 DrawRRT(rrt,w); 00179 leda_wait(0.05); 00180 } 00181 00182 // Finished 00183 cout << "Click a mouse button to terminate\n"; 00184 w->read_mouse(); 00185 return 0; 00186 } 00187 00188 00189 #include <LEDA/UNDEFINE_NAMES.h>