pathplan: added map_2_png as debug view in aalgorithm

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 1f8eb3234edcdf8b9b52a693ee88f0718c6a83a4..bd985d9c616ee6d421606d2aa468d3dc45b37722 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -674,6 +674,18 @@ int aAlgorithm_shape(double xstart_real, double ystart_real, double xgoal_real,
                                 }
                         }
                         
+#ifdef AALGORITHM_DEBUG 
+                       char file_name[20];
+                       sprintf(file_name,"map_x%3d_y%3d_a%1d", coor_contig.x,coor_contig.y, angle_index);
+                        
+                       if(map_2_png(map, file_name)) {
+                               printf("PATHPLAN: aAlgorithm : PNG file sucessfull created");
+                       }
+                       else {
+                               printf("PATHPLAN: aAlgorithm : PNG file not created!");
+                       }
+#endif 
+                        
                        if ((!rot_left) && (!rot_right)) {
                                break;
                        }