/**
- * @file map.c
- * @brief Useful functions related map
- * @author Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
+ * @file map_2_png.c
+ * @brief Transform map to .png file
+ * @author Matous Pokorny <pokormat@fel.cvut.cz>
*
- * This file contains functions to manage map.
- * The map should be accessed only by this library.
- *
- * @todo Lock the map! It necessary in multi-thread programs. If while
- * we are doing something in the map, somewhere the function
- * ShmapFree() is called, segfault occurs.
+ * This file contains function to transform map to picture (.png file). It is using libpng.
*/
#include <stdlib.h>
#include "map.h"
#include "map_2_png.h"
+#define NBR_COLOR_CHANNEL 3
+
+//#define MAP_2_PNG_DBG
+
+// RGB colors used for cells
+png_color LINE_COLOR = { // black
+ 0x00,
+ 0x00,
+ 0x00
+};
+
+png_color FREE_SPACE_COLOR = { // light grey
+ 0xCD,
+ 0xCD,
+ 0xCD
+};
+
+png_color PLAN_MASK_COLOR = { // dark grey
+ 0x99,
+ 0x99,
+ 0x99
+};
+
+png_color GOAL_COLOR = { // green
+ 0x00,
+ 0xFF,
+ 0x00
+};
+
+png_color START_COLOR = { // red
+ 0xFF,
+ 0x00,
+ 0x00
+};
+
+png_color PATH_COLOR = { // dark red
+ 0x8B,
+ 0x25,
+ 0x00
+};
+
+png_color SIMULATED_WALL_COLOR = { // yellow
+ 0xFF,
+ 0xFF,
+ 0x00
+};
+
+png_color INVALIDATE_AND_WALL_COLOR = { // dark yellow
+ 0xFF,
+ 0xB9,
+ 0x0F
+};
+
+png_color IGNORE_OBST_COLOR = { // dark green
+ 0x00,
+ 0x64,
+ 0x00
+};
+
+png_color MORE_THEN_ONE_FLAG = { // dark green
+ 0xFF,
+ 0x00,
+ 0xFF
+};
+
+png_color WALL_COLOR = { // navy blue
+ 0x19,
+ 0x19,
+ 0x70
+};
+
+png_color EXPANDED_CELLS_COLOR = { // turquoise
+ 0x40,
+ 0xE0,
+ 0xD0
+};
+
+png_color MARGIN_COLOR = { // orange
+ 0xFF,
+ 0x7D,
+ 0x40
+};
+
+/**
+ * @name Transform function
+ * @{
+ */
+
+/**
+ * @brief Fuction to transform map to picture (.png file)
+ * @return 1 file is not created , 0 if success.
+ *
+ * This is an implementation of library for extended A* Algorithm
+ * diploma thesis Collison-free Pathplannig for Mobile Robots, CTU
+ * Matous Pokorny 2012
+ */
+
bool map_2_png(struct map *map, char* file_name)
{
- int width = NBR_COLOR_CHANNEL * (CELL_DIMENSION + LINE_THICK) * MAP_WIDTH;
+ int width = (CELL_DIMENSION + LINE_THICK) * MAP_WIDTH;
int height = (CELL_DIMENSION + LINE_THICK) * MAP_HEIGHT;
- png_bytep * row_pointers;
int x, y;
/* create file */
FILE *fp = fopen(file_name, "wb");
if (!fp) {
- printf("PATHPLAN: map_2_png: png file could not open!");
+ printf("MAP_2_PNG: map_2_png: png file could not open!");
return true;
}
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: file %s opened\n", file_name);
+#endif
/* initialize stuff */
png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
if (!png_ptr) {
- printf("PATHPLAN: map_2_png: png stuff is not initialized!");
+ printf("MAP_2_PNG: map_2_png: png stuff is not initialized!");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return true;
}
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: write struct created\n");
+#endif
+
info_ptr = png_create_info_struct(png_ptr);
if (!info_ptr) {
- printf("PATHPLAN: map_2_png: png info structure is not created");
+ printf("MAP_2_PNG: map_2_png: png info structure is not created");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return false;
}
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: info struct created\n");
+#endif
+
if (setjmp(png_jmpbuf(png_ptr))) {
- printf("PATHPLAN: map_2_png: error during png io");
+ printf("MAP_2_PNG: map_2_png: error during png io");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return true;
}
png_init_io(png_ptr, fp);
-
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: png file inited\n");
+#endif
/* write header */
if (setjmp(png_jmpbuf(png_ptr))) {
- printf("PATHPLAN: map_2_png: error during write png header");
+ printf("MAP_2_PNG: map_2_png: error during write png header");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return true;
}
png_write_info(png_ptr, info_ptr);
- row_pointers = (png_bytep*) malloc(height * sizeof(png_bytep));
- for(y=0; y<height; y++) {
- row_pointers[y] = (png_byte*) malloc(width * sizeof(png_byte));
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: info struct written in the file\n");
+#endif
+
+ png_byte ** row_pointers = NULL;
+ row_pointers = png_malloc (png_ptr, height * sizeof(png_byte*));
+
+ if(!row_pointers) {
+ printf("MAP_2_PNG: map_2_png: columns of image are not created!\n");
+ return true;
}
- // fill the picture
- png_byte *pixel = NULL;
- png_byte *p_row_pointer = NULL;
- png_byte *p_cell_pointer = NULL;
- int y_i, x_j;
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: column of the image is created\n");
+#endif
+ png_color pixel;
+ bool free_space_color;
+ int x_cell, y_cell;
+ map_cell_flag_t flags;
+ for (y = 0; y < height; y++) {
+ png_byte *row = png_malloc (png_ptr, sizeof(png_byte) * width * NBR_COLOR_CHANNEL);
+ row_pointers[y] = row;
+
+ if(!row) {
+ printf("MAP_2_PNG: map_2_png: row of image not created!\n");
+ return true;
+ }
- for(y = 0; y < MAP_HEIGHT; y++) {
- for(x = 0; x < MAP_WIDTH; x++) {
+ y_cell = (int) (y / (CELL_DIMENSION + LINE_THICK));
+ for (x = 0; x < width; x++) {
+ x_cell = (int) (x / (CELL_DIMENSION + LINE_THICK));
+
+ flags = map->cells[y_cell][x_cell].flags;
pixel = FREE_SPACE_COLOR; // light grey color
- if (map->cells[y][x].flags & MAP_FLAG_PLAN_MASK) {
- pixel = (pixel == FREE_SPACE_COLOR) ? PLAN_MASK_COLOR : MORE_THEN_ONE_FLAG;
- }
- if (map->cells[y][x].flags & MAP_FLAG_GOAL) {
- pixel = (pixel == FREE_SPACE_COLOR) ? GOAL_COLOR : MORE_THEN_ONE_FLAG;
- }
- if (map->cells[y][x].flags & MAP_FLAG_START) {
- pixel = (pixel == FREE_SPACE_COLOR) ? START_COLOR : MORE_THEN_ONE_FLAG;
- }
- if (map->cells[y][x].flags & MAP_FLAG_PATH) {
- pixel = (pixel == FREE_SPACE_COLOR) ? PATH_COLOR : MORE_THEN_ONE_FLAG;
- }
- if (map->cells[y][x].flags & MAP_FLAG_SIMULATED_WALL) {
- pixel = (pixel == FREE_SPACE_COLOR) ? SIMULATED_WALL_COLOR : MORE_THEN_ONE_FLAG;
- }
- if ((map->cells[y][x].flags & MAP_FLAG_WALL) && (map->cells[y][x].flags & MAP_FLAG_INVALIDATE_WALL) == 0) {
- pixel = (pixel == FREE_SPACE_COLOR) ? INVALIDATE_AND_WALL_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = true;
+
+ if(((y % (CELL_DIMENSION + LINE_THICK)) >= CELL_DIMENSION) || ((x % (CELL_DIMENSION + LINE_THICK)) >= CELL_DIMENSION)){
+ pixel = LINE_COLOR;
}
- if (map->cells[y][x].flags & MAP_FLAG_IGNORE_OBST) {
- pixel = (pixel == FREE_SPACE_COLOR) ? IGNORE_OBST_COLOR : MORE_THEN_ONE_FLAG;
+ else {
+ if (flags & MAP_FLAG_PLAN_MASK) {
+ pixel = (free_space_color) ? PLAN_MASK_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_GOAL) {
+ pixel = (free_space_color) ? GOAL_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_START) {
+ pixel = (free_space_color) ? START_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_PATH) {
+ pixel = (free_space_color) ? PATH_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_SIMULATED_WALL) {
+ pixel = (free_space_color) ? SIMULATED_WALL_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+// if (((flags & MAP_FLAG_WALL) && (flags & MAP_FLAG_INVALIDATE_WALL)) == 0) {
+// pixel = (free_space_color) ? INVALIDATE_AND_WALL_COLOR : MORE_THEN_ONE_FLAG;
+// free_space_color = false;
+// }
+ if (flags & MAP_FLAG_WALL) {
+ pixel = (free_space_color) ? WALL_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_IGNORE_OBST) {
+ pixel = (free_space_color) ? IGNORE_OBST_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_PLAN_EXPANDED_CELLS) {
+ pixel = (free_space_color) ? EXPANDED_CELLS_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
+ if (flags & MAP_FLAG_PLAN_MARGIN) {
+ pixel = (free_space_color) ? MARGIN_COLOR : MORE_THEN_ONE_FLAG;
+ free_space_color = false;
+ }
}
-
- for(y_i = 0; y_i < NBR_PIXELS_IN_CELL; y_i++) {
-
- // pointer to actuall row (png)
- p_row_pointer = row_pointers[y * NBR_PIXELS_IN_CELL] + y_i;
- // pointer to actuall cell (png)
- p_cell_pointer = p_row_pointer + x * NBR_PIXELS_IN_CELL * NBR_COLOR_CHANNEL;
-
- for(x_j = 0; x_j < NBR_PIXELS_IN_CELL; x_j++) {
-
- if((x_j >= CELL_DIMENSION) || (y_i >= CELL_DIMENSION)) {
- pixel = LINE_COLOR;
- }
-
- p_cell_pointer += x_j * NBR_COLOR_CHANNEL;
- *(p_cell_pointer + 1) = *(pixel + 0); // R
- *(p_cell_pointer + 1) = *(pixel + 1); // G
- *(p_cell_pointer + 1) = *(pixel + 2); // B
-
- }
- }
+/*#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: coordinates in map are %3d is smaller %d, %2d is smaller %d, color is (R, G, B) %X %X %X\n", x_cell, MAP_WIDTH, y_cell, MAP_HEIGHT, pixel.red, pixel.blue, pixel.green);
+#endif */
+ *row++ = pixel.red;
+ *row++ = pixel.green;
+ *row++ = pixel.blue;
}
- }
+ }
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: rows of the image are created\n");
+#endif
+
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: image is created\n");
+#endif
/* write bytes */
if (setjmp(png_jmpbuf(png_ptr))) {
- printf("PATHPLAN: map_2_png: error during write bytes");
+ printf("MAP_2_PNG: map_2_png: error during write bytes");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return true;
}
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: bytes are written\n");
+#endif
png_write_image(png_ptr, row_pointers);
-
+
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: image is written\n");
+#endif
/* end write */
if (setjmp(png_jmpbuf(png_ptr))){
- printf("PATHPLAN: map_2_png: error during write end of file");
+ printf("MAP_2_PNG: map_2_png: error during write end of file");
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
return true;
}
free(row_pointers[y]);
free(row_pointers);
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: memory is free\n");
+#endif
+
if(fclose(fp)) {
- printf("PATHPLAN: map_2_png: error during close the file");
+ printf("MAP_2_PNG: map_2_png: error during close the file");
}
+#ifdef MAP_2_PNG_DBG
+ printf("MAP_2_PNG: map_2_png: file is closed, return from function\n");
+#endif
return false;
}
projects / eurobot / public.git / blobdiff