map.cpp

00001 #include "map.h"
00002 
00003 map::map (Odometry* nodo) : Codo(nodo) {
00004 }
00005 
00006 void map::addObs (obsCarr nobs) {
00007     obs.push_back (nobs);
00008 }
00009 
00010 void map::FindWay (float depX, float depY, float arrX, float arrY) {
00011     point depart(depX, depY);
00012     point arrivee(arrX, arrY);
00013     FindWay(depart, arrivee);
00014 }
00015 
00016 void map::FindWay (point dep, point arr) {
00017     //logger.printf("On a cherche un chemin\n\r");
00018     nVector<pointParcours> open;
00019     nVector<pointParcours> close;
00020     points4 tmp;
00021     bool val[4] = {true,true,true,true};
00022     int os = obs.size ();
00023     int i, j;
00024     bool ended=false;   // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
00025     endedParc = false;
00026 
00027     path.clear();
00028     
00029     pointParcours depP (dep, NULL, arr);
00030     int indTMP1=0;  // Le point actuel
00031     int PointEnding = 0;    
00032     open.push_back (depP);
00033 
00034     while (!ended && !open.empty ()) {
00035         for (i = 0; i < open.size (); ++i) {
00036             if (open[i].getP2 () < open[indTMP1].getP2 ())
00037                 indTMP1 = i;
00038         }
00039 
00040         close.push_first (open[indTMP1]);
00041         open.erase (indTMP1);
00042         indTMP1 = 0;
00043 
00044         ended = true;
00045         for (i = 0; i < os; ++i) {
00046             if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
00047                 ended = false;
00048                 tmp = obs[i].getPoints ();
00049 
00050                 // On vérifie si le point croise un obstacle
00051                 for (j = 0; j < os; ++j)
00052                     if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
00053                         val[0] = false;
00054                 // On vérifie si le point existe déjà dans la liste ouverte
00055                 for (j = 0; j < open.size (); ++j) {
00056                     if (open[j] == tmp.p0)
00057                         val[0] = false;
00058                 }
00059                 // On vérifie si le point existe déjà dans la liste fermée
00060                 for (j = 0; j < close.size (); ++j) {
00061                     if (close[j] == tmp.p0)
00062                         val[0] = false;
00063                 }
00064                 if (val[0]) {
00065                     open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
00066                 }
00067 
00068                 // On repete l'operation pour le second point
00069                 for (j = 0; j < os; ++j)
00070                     if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
00071                         val[1] = false;
00072                 for (j = 0; j < open.size (); ++j) {
00073                     if (open[j] == tmp.p1)
00074                         val[1] = false;
00075                 }
00076                 for (j = 0; j < close.size (); ++j) {
00077                     if (close[j] == tmp.p1)
00078                         val[1] = false;
00079                 }
00080                 if (val[1]) {
00081                     open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
00082                 }
00083 
00084                 // On répète l'opération pour le troisième point
00085                 for (j = 0; j < os; ++j)
00086                     if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
00087                         val[2] = false;
00088                 for (j = 0; j < open.size (); ++j) {
00089                     if (open[j] == tmp.p2)
00090                         val[2] = false;
00091                 }
00092                 for (j = 0; j < close.size (); ++j) {
00093                     if (close[j] == tmp.p2)
00094                         val[2] = false;
00095                 }
00096                 if (val[2]) {
00097                     open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
00098                 }
00099 
00100                 // On répète l'opération pour le quatrieme point
00101                 for (j = 0; j < os; ++j)
00102                     if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
00103                         val[3] = false;
00104                 for (j = 0; j < open.size (); ++j) {
00105                     if (open[j] == tmp.p3)
00106                         val[3] = false;
00107                 }
00108                 for (j = 0; j < close.size (); ++j) {
00109                     if (close[j] == tmp.p3)
00110                         val[3] = false;
00111                 }
00112                 if (val[3]) {
00113                     open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
00114                 }
00115 
00116                 val[0] = true;
00117                 val[1] = true;
00118                 val[2] = true;
00119                 val[3] = true;
00120             }
00121         }
00122     }
00123 
00124     /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
00125     if (ended) {
00126         pointParcours* pente;
00127         pente = &close[0];
00128         while (pente != NULL) {
00129             path.push_first (*pente);
00130             pente = pente->getPere ();
00131         }
00132         path.push_back (pointParcours(arr, NULL, arr));
00133         path.erase(0);
00134         endedParc = true;
00135         /*
00136         if (path.size() > 1)
00137             path.erase(0);*/
00138     }
00139 }
00140 
00141 void map::Execute(float XObjectif, float YObjectif) {
00142     logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
00143     FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
00144     
00145     if (endedParc) {
00146         //logger.printf("\n\r");
00147         for (int i = 0; i < path.size (); i++) {
00148             logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
00149             //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
00150             Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
00151             logger.printf("Goto Fini\n\r");
00152         }
00153         //logger.printf("Chemin fini !\n\r");
00154     }
00155     else {
00156         logger.printf("Chemin pas trouve ...\n\r");
00157     }
00158     endedParc = false;
00159 }