src/pathplan/path_planner.c

   1 /**
   2  * @file        path_planner.c
   3  * @brief       Main program of Path Planner
   4  * @author      Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>, (C) 2007
   5  * @warning     This library needs map library to work!
   6 */
   7 /** @ingroup pp */
   8 /**@{*/
   9 #include "path_planner.h"
  10 #include <pthread.h>
  11 #define DBG printf
  12
  13 static void add_safety_margin()
  14 {
  15     // Adds safety zone around the obstacle.
  16     // This zone contains one neighbour cell (to avoid frequent recalculation).
  17     int xx, yy, ii;
  18     struct map *map = ShmapIsMapInit();
  19     const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
  20                                                {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
  21
  22     for (yy=0;yy<MAP_HEIGHT;yy++) {
  23         for (xx=0;xx<MAP_WIDTH;xx++) {
  24             // check obstacles around this cell to determine safety zone flag
  25             map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN;  // delete margin status
  26             for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
  27             {
  28                 if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y)
  29                     && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST)
  30                         || (map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).detected_obstacle > 0 ))
  31                 {
  32                     // creates safety status
  33                     map->cells[yy][xx].flags |= MAP_FLAG_PLAN_MARGIN;
  34                     //cntr ++;
  35                     break;
  36                 }
  37             }
  38         }
  39     }
  40 }
  41
  42
  43 /**
  44  * Returns the number of points of the path on success or an error code.
  45  * @param       xstart_real     X coordinate in meters
  46  * @param       ystart_real     Y coordinate in meters
  47  * @param       xgoal_real      X coordinate in meters
  48  * @param       ygoal_real      Y coordinate in meters
  49  * @param       simple_path     A pointer to where the simple path will be stocked
  50  * @param       angle           A pointer to a double variable where the final angle of the trajectory will be stocked.
  51  * @return 1 if a path is founded, 0 if not
  52  * @warning     The Shared Map Memory must be init before.
  53  * @warning     Do not forget to free allocated memory with freePathMemory().
  54  *
  55  * It takes parameters of start and goal points. It returns the number
  56  * of points of the trajectory and the caller must specify pointers to
  57  * path and angle.
  58  *
  59  * The result of the algorithm is stored in a list of ::PathPoint,
  60  * terminated by NULL, containing the points of the path. This memory
  61  * is allocated dynamicaly and its address will be returned as
  62  * a double pointer to the path (simple_path) passed as
  63  * argument. After use the path, memory should be free with
  64  * freePathMemory(). A pointer of the final angle must be also passed
  65  * as argument.
  66  *
  67  * The map memory must be init with ShmapInit() before calling
  68  * path_planner(). Do not forget to free this map memory with
  69  * ShmapFree().
  70  */
  71
  72 int path_planner(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real , PathPoint ** simple_path, double * angle)
  73 {
  74         int nbpoints, count;
  75         int ret;
  76         GraphMapCell *astar_path;
  77         PathPoint *original_path;
  78         PathPoint * tmp;
  79         static pthread_mutex_t plan_mutex = PTHREAD_MUTEX_INITIALIZER;
  80
  81         // Init the map
  82         if (! ShmapIsMapInit()) {
  83                 DBG("ERROR: shared map memory is not init.\n");
  84                 return PP_ERROR_MAP_NOT_INIT;
  85         }
  86
  87         //add_safety_margin();
  88
  89         // If the goal map is not free, abort.
  90         if (!ShmapIsFreePoint(xgoal_real, ygoal_real)) {
  91                 DBG("ERROR: the goal cell is an obstacle (point coords [%.3f, %.3f]).\n", xgoal_real, ygoal_real);
  92                 return PP_ERROR_GOAL_IS_OBSTACLE;
  93         }
  94
  95         pthread_mutex_lock(&plan_mutex);
  96
  97         // Call to A* algorithm
  98         nbpoints = aAlgorithm(xstart_real,ystart_real, xgoal_real, ygoal_real, &astar_path);
  99         if (nbpoints < 0) {
 100                 ret = PP_ERROR_GOAL_NOT_REACHABLE;
 101                 goto out;
 102         }
 103
 104         original_path = malloc(nbpoints * sizeof(PathPoint));
 105         tmp = original_path;
 106         while (astar_path) {
 107             int x, y;
 108             GraphCell2XY(astar_path, &x, &y);
 109             if (tmp != original_path) {
 110                     ShmapCell2Point(x, y, &tmp->x, &tmp->y);
 111             } else {
 112                     tmp->x = xstart_real;
 113                     tmp->y = ystart_real;
 114             }
 115             astar_path = astar_path->next;
 116             tmp++;
 117         }
 118         tmp--;
 119         tmp->x = xgoal_real;
 120         tmp->y = ygoal_real;
 121
 122
 123
 124         // DBG("Simplifing the path\n");
 125         // Calc a simplest path
 126         (*simple_path) = (PathPoint *) malloc(sizeof(PathPoint));
 127         count = path_simplifier(original_path, nbpoints, *simple_path, angle);
 128         // Do not send the first point
 129         tmp=(*simple_path);
 130         (*simple_path)= (*simple_path)->next;
 131         free(tmp);
 132         free(original_path);
 133
 134         // All is OK
 135         ret = 1;
 136 out:
 137         pthread_mutex_unlock(&plan_mutex);
 138         return ret;
 139
 140
 141 }
 142 /**@}*/
 143