pathplan: Fixed removing of safety margin

If there was a safety margin flag at goal position, it was never removed
causing a robot movement deadlock. The reason is that safety margin was
updated (and removed) inside aalgorithm() but the goal is checked for
obstacles in path_planner().

This problem was solved by moving safety margin update to path_planner().

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 05eec6436535ddc5bfd52c2212dbbbbec2bf9869..6c2374f0e154408e69c23aa723d083f8cbf247f4 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -147,31 +147,7 @@ int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double
         pushNodeInOrder(start);
 
     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
-        const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
-                {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
-
-
-    // Adds safety zone around the obstacle.
-    // This zone contains one neighbour cell (to avoid frequent recalculation).
-    int xx, yy, ii;
-    for (yy=0;yy<MAP_HEIGHT;yy++) {
-        for (xx=0;xx<MAP_WIDTH;xx++) {
-            // check obstacles around this cell to determine safety zone flag
-            map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN;  // delete margin status
-            for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
-            {
-                if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y) 
-                    && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST) 
-                        || (map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).detected_obstacle > 0 ))
-                {
-                    // creates safety status
-                    map->cells[yy][xx].flags |= MAP_FLAG_PLAN_MARGIN;
-                    //cntr ++;
-                    break;
-                }
-            }
-        }
-    }
+
     //printf("*cntr %d \n", cntr);
 
        /// <li> REPEAT until the queue is empty.

diff --git a/src/pathplan/path_planner.c b/src/pathplan/path_planner.c
index 599fdabebdc892918799a9c0a5481324a1006543..e9a857ec7f093ab2dde1dc3e70a8906dab6a9e0d 100644 (file)
--- a/src/pathplan/path_planner.c
+++ b/src/pathplan/path_planner.c
@@ -10,6 +10,35 @@
 #include <pthread.h>
 #define DBG printf
 
+static void add_safety_margin()
+{
+    // Adds safety zone around the obstacle.
+    // This zone contains one neighbour cell (to avoid frequent recalculation).
+    int xx, yy, ii;
+    struct map *map = ShmapIsMapInit();
+    const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
+                                              {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
+    
+    for (yy=0;yy<MAP_HEIGHT;yy++) {
+        for (xx=0;xx<MAP_WIDTH;xx++) {
+            // check obstacles around this cell to determine safety zone flag
+            map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN;  // delete margin status
+            for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
+            {
+                if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y) 
+                    && (((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST) 
+                        || (map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).detected_obstacle > 0 ))
+                {
+                    // creates safety status
+                    map->cells[yy][xx].flags |= MAP_FLAG_PLAN_MARGIN;
+                    //cntr ++;
+                    break;
+                }
+            }
+        }
+    }
+}
+
 
 /** 
  * Returns the number of points of the path on success or an error code.
@@ -55,6 +84,8 @@ int path_planner(double xstart_real,double ystart_real, double xgoal_real, doubl
                return PP_ERROR_MAP_NOT_INIT;
        }
 
+       add_safety_margin();
+
        // If the goal map is not free, abort.
        if (!ShmapIsFreePoint(xgoal_real, ygoal_real)) {
                DBG("ERROR: the goal cell is an obstacle.\n");