src/pathplan/aalgorithm.c

   1 /**
   2  * @file        aalgorithm.c
   3  * @brief       Implementation of the A* algorithm. Main functions.
   4  * @author      Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
   5  *
   6  * @note The author asumes that the reader knows how A* algorithm
   7  *       works. In this documentation only the implementation is
   8  *       explained. More information "Principles of Robot Motion" by
   9  *       H. Choset&others (Algorithm 24, p531)
  10  *
  11 */
  12 /** @addtogroup ppi*/
  13 /**@{*/
  14 #define _ISOC99_SOURCE              /* To have INFINITY */
  15 #define _XOPEN_SOURCE               /* To have M_SQRT2 etc. */
  16 #include "aalgorithm.h"
  17 #include "pathqueue.h"
  18 #include "map.h"
  19 #include <math.h>
  20 #include <stdlib.h>
  21
  22 /**
  23  * @name A* Constants
  24  * @{
  25  */
  26 #define SQRT2 M_SQRT2           /**< @brief Root square of two.*/
  27 #define WALL_COST       1000 /**< Cost of jumping a wall. */
  28 /**@}*/
  29
  30 /**
  31  * @name A* Main Structure
  32  * @{
  33  */
  34 static GraphMapCell graph[MAP_HEIGHT][MAP_WIDTH]; /**< @brief Graph structure used in A*.  */
  35 /**@}*/
  36
  37 #ifndef CONFIG_PP_USES_RBTREE
  38 static GraphMapCell *graph_queue;   /**< Pointer to the first cell in the list */
  39 #else
  40 static struct rb_root graph_queue;   /**< rbtree root */
  41
  42 #define __compiler_offsetof(a,b) __builtin_offsetof(a,b) /* For GCC 4 */
  43 #ifdef __compiler_offsetof
  44 #define offsetof(TYPE,MEMBER) __compiler_offsetof(TYPE,MEMBER)
  45 #else
  46 #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
  47 #endif
  48
  49 /**
  50  * container_of - cast a member of a structure out to the containing structure
  51  * @ptr:        the pointer to the member.
  52  * @type:       the type of the container struct this is embedded in.
  53  * @member:     the name of the member within the struct.
  54  *
  55  */
  56 #define container_of(ptr, type, member) ({                      \
  57         const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
  58         (type *)( (char *)__mptr - offsetof(type,member) );})
  59
  60 #endif
  61
  62 static void pushNodeInOrder(GraphMapCell *cell);
  63 float cFunction(int x1, int y1, int x2, int y2);
  64 void calculateMapHeuristic(int xgoal, int ygoal);
  65 float calculateCost(int x1, int y1, int x2, int y2);
  66 void initGraphStructure(void);
  67 int getPathLength(int xstart, int ystart, int xgoal, int ygoal);
  68
  69 /**
  70  * Returns X and Y index of a graph cell identified by a pointer.
  71  *
  72  * @param[in] c Pointer to the cell in question.
  73  * @param[out] x Cell's X index.
  74  * @param[out] y Cell's Y index.
  75  */
  76 void GraphCell2XY(GraphMapCell *c, int *x, int *y)
  77 {
  78         *y = (c - &graph[0][0]) / MAP_WIDTH;
  79         *x = (c - &graph[0][0]) % MAP_WIDTH;
  80 }
  81
  82
  83 /**
  84  * @name A* Main function
  85  * @{
  86  */
  87
  88 /**
  89  * @brief Search the shortest path between two points
  90  * @param xstart_real   Coordonate X of the start point
  91  * @param ystart_real   Coordonate Y of the start point
  92  * @param xgoal_real    Coordonate X of the goal point
  93  * @param ygoal_real    Coordonate Y of the goal point
  94  * @param original_path [out] Pointer to the first map cell of the
  95  *                            path is stored here. Use
  96  *                            GraphMapCell.next to traverse the found
  97  *                            path.
  98  * @see graph
  99  * @return              -1 if the goal is not founded, the number of points to the goal is founded on success.
 100  *
 101  * This is an implementation of A* Algorithm (Algorithm 24, p531)
 102  * defined in the book "Principles of Robot Motion" by
 103  * H. Choset&others
 104  */
 105
 106 int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
 107 {
 108         /**
 109          * prioritySet : Set of nodes to process order by heuristic distance
 110          */
 111 #ifndef CONFIG_PP_USES_RBTREE
 112         graph_queue = NULL;
 113 #else
 114         graph_queue = RB_ROOT;
 115 #endif
 116         /**
 117          * nbest : Best node to reach the goal
 118          */
 119         GraphMapCell *nbest, *start, *goal, *contig;
 120         int i, x, y;
 121         int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
 122         struct map *map = ShmapIsMapInit();
 123         bool valid;
 124
 125         if (!map) return -1;
 126         // Transform real data in cell data
 127         ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
 128         if (!valid)
 129                 return -1;
 130         start = &graph[ystart][xstart];
 131         ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
 132         if (!valid)
 133                 return -1;
 134         goal = &graph[ygoal][xgoal];
 135
 136         /**@{*/
 137         /// <ol>
 138         /// <li> Init graph structure with default values.
 139         initGraphStructure();
 140         /// <li> Calculate Heuristic distances.
 141         calculateMapHeuristic(xgoal, ygoal);
 142
 143         /// <li> Initial values : put start node in queue.
 144         start->g = 0;
 145         start->f = start->g + start->h;
 146         start->backpointer = start;
 147         pushNodeInOrder(start);
 148
 149         /// <li> REPEAT until the queue is empty.
 150
 151         do {
 152                 /// <ol>
 153                 /// <li> Pick nbest from prioritySet and remove
 154 #ifndef CONFIG_PP_USES_RBTREE
 155                 nbest = graph_queue;
 156                 graph_queue = graph_queue->next;
 157 #else
 158                 nbest = container_of(rb_first(&graph_queue), GraphMapCell, node);
 159                 rb_erase(&nbest->node, &graph_queue);
 160 #endif
 161
 162                 GraphCell2XY(nbest, &x, &y);
 163 /*                 map->cells[y][x].detected_obstacle = 255; */
 164 /*                 usleep(100000); */
 165
 166                 /// <li> add to processed
 167                 nbest->processed = 1;
 168
 169                 ///  <li> IF nbest == goal ; EXIT
 170                 if (nbest==goal) {
 171                         break;
 172                 }
 173
 174                 ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
 175                 const struct { int x; int y; } neighbours[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
 176                                                           {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
 177                 for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
 178                         xcontig = x + neighbours[i].x;
 179                         ycontig = y + neighbours[i].y;
 180
 181                         if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
 182                                 double cost = cFunction(x, y, xcontig, ycontig);
 183                                 contig = &graph[ycontig][xcontig];
 184                                 /// <li> IF is not in Priorityset add to prioritySet.
 185                                 if (!contig->in_queue) {
 186                                         if(ShmapIsFreeCell(xcontig, ycontig)) {
 187                                                 //  add to prioritySet.
 188                                                 contig->g=nbest->g + cost;
 189                                                 contig->f = contig->g + contig->h;
 190                                                 contig->backpointer=nbest;
 191                                                 pushNodeInOrder(contig);
 192                                         }
 193                                 } else if (nbest->g + cost < contig->g) {
 194                                         /// <li>  ELSE update backpointer to point to nbest.
 195                                         contig->backpointer=nbest;
 196                                         contig->g = nbest->g + cost;
 197                                 }
 198                         }
 199                 }
 200                 /// </ol>
 201 #ifndef CONFIG_PP_USES_RBTREE
 202         } while (graph_queue);
 203 #else
 204         } while (!RB_EMPTY_ROOT(&graph_queue));
 205 #endif
 206
 207         if (nbest==goal) {
 208                 int nbpoints, x, y;
 209                 GraphMapCell *cell;
 210
 211                 // Clear previous path
 212                 for (y=0;y<MAP_HEIGHT;y++){
 213                         for(x=0;x<MAP_WIDTH;x++){
 214                                 map->cells[y][x].flags &= ~(MAP_FLAG_PATH | MAP_FLAG_START | MAP_FLAG_GOAL);
 215                         }
 216                 }
 217
 218
 219                 /* Construct forward list for the found path. */
 220                 cell = goal;
 221                 cell->next = NULL;
 222                 nbpoints = 1;
 223                 while (cell != start) {
 224                         cell->backpointer->next = cell;
 225                         cell = cell->backpointer;
 226                         nbpoints++;
 227                         GraphCell2XY(cell, &x, &y);
 228                         map->cells[y][x].flags |= MAP_FLAG_PATH;
 229                 }
 230
 231                 map->cells[ystart][xstart].flags |= MAP_FLAG_START | MAP_FLAG_PATH;
 232                 map->cells[ygoal][xgoal].flags |= MAP_FLAG_GOAL  | MAP_FLAG_PATH;
 233
 234                 *original_path = start;
 235                 return nbpoints;
 236         } else {
 237                 return -1;
 238         }
 239 }
 240
 241 /** @} */
 242 /**
 243  * @name A* related functions
 244  * This functions have a meaning in theorical A*.
 245  * @{
 246  */
 247
 248 /**
 249  * @brief Calculates the lenght of edge connecting two points
 250  * @param x1    Coordonate X of the first cell
 251  * @param y1    Coordonate Y of the first cell
 252  * @param x2    Coordonate X of the second cell
 253  * @param y2    Coordonate Y of the second cell
 254  * @return      Lenght of edge
 255 */
 256
 257 float cFunction(int x1, int y1, int x2, int y2)
 258 {
 259         float c;
 260         //if(GETMAPPOS(x2, y2)== MAP_WALL) c = WALL_COST;
 261         if(!ShmapIsFreeCell(x2,y2)) c = WALL_COST;
 262         else if( (x1==x2) || (y1 == y2) ) c=1.0;
 263         else c=SQRT2;
 264
 265         return c;
 266
 267 }
 268
 269 /**
 270  *
 271  * @brief Calculates Map Heuristic values.
 272  * @param xgoal Coordonate X of the goal
 273  * @param ygoal Coordonate Y of the goal
 274  *
 275  * This function calculates the map heuristic values (the distance to
 276  * the goal that is supoused to be the shortest).
 277  *
 278  * The chosen method has been the euclidean distance.  The distance
 279  * between a point of the grid \f$(x,y)\f$ and the
 280  * goal\f$(x_(goal),y_(goal))\f$ is
 281  * \f$\sqrt{(x_(goal)-x)^2+(y_(goal)-y)^2}\f$.
 282  *
 283  * Another method explained in the book is coded but not used.It is
 284  * called Brushfire algorithm. (section 4.3.2 of the book)
 285  *
 286  */
 287 void calculateMapHeuristic(int xgoal, int ygoal)
 288 {
 289         int i, j;
 290         for(i=0;i<MAP_HEIGHT;i++){
 291                 for (j=0;j<MAP_WIDTH;j++){
 292                         graph[i][j].h=sqrt(abs(xgoal-j)*abs(xgoal-j)+abs(ygoal-i)*abs(ygoal-i));
 293                 }
 294         }
 295 #if 0
 296 /*      This is another way to compute heuristic without SQRT but it don't work properly */
 297
 298         Node n;
 299         NodeQueue * toProcess = malloc(sizeof(NodeQueue));
 300         int xcontig, ycontig;
 301         float h; // Heuristic distance of cell
 302         float newh;                             // New Heuristic distance of cell
 303         newQueue(toProcess);
 304         graph[ygoal][xgoal].h=0;
 305         graph[ygoal][xgoal].processed=1;
 306
 307         pushNode(toProcess, xgoal, ygoal);
 308         while(!queueIsEmpty(toProcess)){
 309                 //printPrioritySet(toProcess);
 310                 // Pop the first node in the queue
 311                 popNode(toProcess,&n);
 312                 graph[n.y][n.x].processed=1;
 313                 // Take node's heuristic distance
 314                 h = graph[n.y][n.x].h;
 315
 316                 // Cross contiguous cells
 317                 for (xcontig = n.x-1 ; xcontig <= n.x + 1 ; xcontig++){
 318                         for (ycontig = n.y - 1 ; ycontig <= n.y + 1 ; ycontig++){
 319                                 if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed==0)) {
 320                                         //Add node to priority queue
 321                                         if ((!isInQueue(toProcess,xcontig,ycontig))) pushNode(toProcess,xcontig,ycontig);
 322                                         // Update heuristic if  [ (Heuristic not set) OR [oldHeuristic is higher than newer)]
 323                                         newh= h + calculateCost(n.x, n.y, xcontig, ycontig);
 324                                         if( ( (graph[ycontig][xcontig].h < 0) || (graph[ycontig][xcontig].h > newh )) )
 325                                                 graph[ycontig][xcontig].h=newh ;
 326                                 }
 327
 328                         }
 329                 }
 330         }
 331
 332         // Clear proccessed flag
 333         for(i=0;i<MAP_HEIGHT;i++){
 334                 for (j=0;j<MAP_WIDTH;j++){
 335                         graph[i][j].processed=0;
 336                 }
 337         }
 338 #endif
 339
 340 }
 341
 342 /**
 343  * @brief Calculates the cost of moving from the first cell to the second cell.
 344  * @param x1    Coordonate X of the first cell
 345  * @param y1    Coordonate Y of the first cell
 346  * @param x2    Coordonate X of the second cell
 347  * @param y2    Coordonate Y of the second cell
 348  * @return      Cross cost
 349  * @warning Cells must be contiguous
 350 */
 351 float calculateCost(int x1, int y1, int x2, int y2){
 352
 353         if( (x1==x2) || (y1 == y2) ) return 1.0;
 354         else return SQRT2;
 355
 356 }
 357
 358 /** @} */
 359
 360 /**
 361  * @name A* Implementation Functions
 362  * This functions have not a meaning in A* algorithm, but are needed for implementation.
 363  * @{
 364  */
 365
 366 /**
 367  * @brief Init Graph structure with default values
 368  * @see graph
 369 */
 370 void initGraphStructure(void){
 371         int i,j;
 372
 373         GraphMapCell g = {  .h = -1, .g = -1, .f = INFINITY, .backpointer = NULL,
 374                             .processed = false, .in_queue = false, .next = NULL };
 375
 376         //Initialisation of graph structure
 377         for(i=0;i<MAP_HEIGHT;i++){
 378                 for (j=0;j<MAP_WIDTH;j++){
 379                         graph[i][j] = g;
 380                 }
 381         }
 382 }
 383
 384 /**
 385  * @brief Push the cell c in the queue ordered by heuristic value of the cell
 386  *
 387  * @param c     The cell
 388  *
 389  * @todo Use some tree based data structure for this. -- Sojka
 390  */
 391
 392 void pushNodeInOrder(GraphMapCell *c)
 393 {
 394         c->in_queue = true;
 395
 396 #ifndef CONFIG_PP_USES_RBTREE
 397         GraphMapCell *current, *last;
 398         if (!graph_queue) {
 399                 graph_queue = c;
 400         } else {
 401                 // Put the node in right position
 402                 current=graph_queue;
 403                 last=NULL;
 404                 while (current!=NULL){
 405                         if (c->f < current->f ) {
 406                                 if (last==NULL) graph_queue = c;
 407                                 else last->next = c;
 408                                 c->next=current;
 409                                 return;
 410                         } else {
 411                                 last=current;
 412                                 current=current->next;
 413                         }
 414                 }
 415                 // the node goes in the last position
 416                last->next=c;
 417                c->next = NULL;
 418         }
 419 #else
 420         struct rb_node **new = &(graph_queue.rb_node), *parent = NULL;
 421         /* Figure out where to put new node */
 422         while (*new) {
 423                 GraphMapCell *this = container_of(*new, GraphMapCell, node);
 424
 425                 parent = *new;
 426                 if (c->f < this->f)
 427                         new = &((*new)->rb_left);
 428                 else
 429                         new = &((*new)->rb_right);
 430         }
 431
 432         /* Add new node and rebalance tree. */
 433         rb_link_node(&c->node, parent, new);
 434         rb_insert_color(&c->node, &graph_queue);
 435 #endif
 436 }
 437 /**@}*/
 438
 439 /** @} */
 440