pathplan:add map cell flag path shape. Flag shows cells, which was checked by astar...

Add function for setting flag in one map cell.

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 2ba831f7d0452414aba81b2a370296d2ecd9c05b..e763ee627b561be1c4e7bb16665979733694c508 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -114,6 +114,9 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
   
   for(x=x_0-bitmap_straight[angle].x_min; x<=x_0+bitmap_straight[angle].x_max;x++){
     for(y=y_0-bitmap_straight[angle].y_min; y<=y_0+bitmap_straight[angle].y_max;y++){
+      
+      ShmapSetCellFlag(x,y,MAP_FLAG_PATH_SHAPE);
+      
       if (!ShmapIsFreeCell(x,y)){      //1 no obstacle,-1 no in map, 0 otherwise
        return false;
       }
@@ -123,6 +126,9 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
   return true;
 }
 
+      
+       ShmapSetCellFlag(x,x+q,MAP_FLAG_PATH_SHAPE);
+      
 // int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
 //   
 //   double angle=0; 

diff --git a/src/pathplan/map.c b/src/pathplan/map.c
index e713441398bbd82f4a874b8e32b311d737c965ca..75f3bfdabe1b73276666084d12254a635c84f09d 100644 (file)
--- a/src/pathplan/map.c
+++ b/src/pathplan/map.c
@@ -208,6 +208,26 @@ int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags
        return 1;
 }
 
+/**
+ * @brief Set flag in the map cell
+ * @param x    Coordonate X (int) of the cell
+ * @param y    Coordonate Y (int) of the cell
+ * @param set_flags    Map cell falgs
+ * 
+ */
+int ShmapSetCellFlag(int x, int y, map_cell_flag_t set_flags)
+{
+  if (!ShmapIsCellInMap(x, y)){
+    return 0;
+  }
+          
+  map->cells[y][x].flags |= set_flags;
+  
+  return 1;
+}
+
+/** @} */
+
 /**
  * @brief Creates an obstacle in the map
  * @param x1   Coordonate X (in m) of the first point

diff --git a/src/pathplan/map.h b/src/pathplan/map.h
index 87b910dd11653527f2bb4689da5ebb4a7c7d5853..aa37f568c7c0586f48b556c9913e81e5f3301546 100644 (file)
--- a/src/pathplan/map.h
+++ b/src/pathplan/map.h
@@ -188,6 +188,7 @@
  * not during runtime obstacle avoidance. */
 #define MAP_FLAG_PLAN_MARGIN           128
 #define MAP_FLAG_INVALIDATE_WALL       256 /**< Area, where the wall should be forgotten */
+#define MAP_FLAG_PATH_SHAPE            512
 
 /** @}*/
 /** @name Shared Memory macros */
@@ -265,6 +266,8 @@ int ShmapIsFreePoint(double x_m, double y_m);
 int ShmapSetRectangleFlag(double x1, double y1, double x2, double y2, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
 int ShmapSetCircleFlag(double xs, double ys, double r, map_cell_flag_t set_flags, map_cell_flag_t clear_flags);
 
+int ShmapSetCellFlag(int x, int y, map_cell_flag_t set_flags);
+
 #ifdef __cplusplus
 }
 #endif 

diff --git a/src/pathplan/test/testastar.c b/src/pathplan/test/testastar.c
index 8ca7a4baa1c8f1a043046813612843339faad714..c2cdf089f465f95232d2ffae6a8a0d0556456c56 100644 (file)
--- a/src/pathplan/test/testastar.c
+++ b/src/pathplan/test/testastar.c
@@ -24,9 +24,13 @@ void print_map(char *label)
                        char c;
                        if (ShmapIsFreeCell(x, y)) {
                                c = '.';
+                               if (map->cells[y][x].flags & MAP_FLAG_PATH_SHAPE) {
+                                       c = 's';
+                               }
                                if (map->cells[y][x].flags & MAP_FLAG_PATH) {
                                        c = 'p';
                                }
+                               
                        } else
                                c = '#';
                        putchar(c);