Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * map_store.c: Global map storage functions 00004 * 00005 * Created: Thu May 24 18:48:43 2012 00006 * Copyright 2000 Brian Gerkey 00007 * 2000 Kasper Stoy 00008 * 2012 Tim Niemueller [www.niemueller.de] 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 /* From: 00025 * Player - One Hell of a Robot Server (LGPL) 00026 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00027 * gerkey@usc.edu kaspers@robotics.usc.edu 00028 */ 00029 /************************************************************************** 00030 * Desc: Global map storage functions 00031 * Author: Andrew Howard 00032 * Date: 6 Feb 2003 00033 **************************************************************************/ 00034 00035 #include <errno.h> 00036 #include <math.h> 00037 #include <stdio.h> 00038 #include <stdlib.h> 00039 #include <string.h> 00040 00041 #include "map.h" 00042 00043 /// @cond EXTERNAL 00044 00045 //////////////////////////////////////////////////////////////////////////// 00046 // Load an occupancy grid 00047 int map_load_occ(map_t *map, const char *filename, double scale, int negate) 00048 { 00049 FILE *file; 00050 char magic[3]; 00051 int i, j; 00052 int ch, occ; 00053 int width, height, depth; 00054 map_cell_t *cell; 00055 00056 // Open file 00057 file = fopen(filename, "r"); 00058 if (file == NULL) 00059 { 00060 fprintf(stderr, "%s: %s\n", strerror(errno), filename); 00061 return -1; 00062 } 00063 00064 // Read ppm header 00065 00066 if ((fscanf(file, "%10s \n", magic) != 1) || (strcmp(magic, "P5") != 0)) 00067 { 00068 fprintf(stderr, "incorrect image format; must be PGM/binary"); 00069 return -1; 00070 } 00071 00072 // Ignore comments 00073 while ((ch = fgetc(file)) == '#') 00074 while (fgetc(file) != '\n'); 00075 ungetc(ch, file); 00076 00077 // Read image dimensions 00078 if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3) 00079 { 00080 fprintf(stderr, "Failed ot read image dimensions"); 00081 return -1; 00082 } 00083 00084 // Allocate space in the map 00085 if (map->cells == NULL) 00086 { 00087 map->scale = scale; 00088 map->size_x = width; 00089 map->size_y = height; 00090 map->cells = calloc(width * height, sizeof(map->cells[0])); 00091 } 00092 else 00093 { 00094 if (width != map->size_x || height != map->size_y) 00095 { 00096 //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); 00097 return -1; 00098 } 00099 } 00100 00101 // Read in the image 00102 for (j = height - 1; j >= 0; j--) 00103 { 00104 for (i = 0; i < width; i++) 00105 { 00106 ch = fgetc(file); 00107 00108 // Black-on-white images 00109 if (!negate) 00110 { 00111 if (ch < depth / 4) 00112 occ = +1; 00113 else if (ch > 3 * depth / 4) 00114 occ = -1; 00115 else 00116 occ = 0; 00117 } 00118 00119 // White-on-black images 00120 else 00121 { 00122 if (ch < depth / 4) 00123 occ = -1; 00124 else if (ch > 3 * depth / 4) 00125 occ = +1; 00126 else 00127 occ = 0; 00128 } 00129 00130 if (!MAP_VALID(map, i, j)) 00131 continue; 00132 cell = map->cells + MAP_INDEX(map, i, j); 00133 cell->occ_state = occ; 00134 } 00135 } 00136 00137 fclose(file); 00138 00139 return 0; 00140 } 00141 00142 00143 //////////////////////////////////////////////////////////////////////////// 00144 // Load a wifi signal strength map 00145 /* 00146 int map_load_wifi(map_t *map, const char *filename, int index) 00147 { 00148 FILE *file; 00149 char magic[3]; 00150 int i, j; 00151 int ch, level; 00152 int width, height, depth; 00153 map_cell_t *cell; 00154 00155 // Open file 00156 file = fopen(filename, "r"); 00157 if (file == NULL) 00158 { 00159 fprintf(stderr, "%s: %s\n", strerror(errno), filename); 00160 return -1; 00161 } 00162 00163 // Read ppm header 00164 fscanf(file, "%10s \n", magic); 00165 if (strcmp(magic, "P5") != 0) 00166 { 00167 fprintf(stderr, "incorrect image format; must be PGM/binary"); 00168 return -1; 00169 } 00170 00171 // Ignore comments 00172 while ((ch = fgetc(file)) == '#') 00173 while (fgetc(file) != '\n'); 00174 ungetc(ch, file); 00175 00176 // Read image dimensions 00177 fscanf(file, " %d %d \n %d \n", &width, &height, &depth); 00178 00179 // Allocate space in the map 00180 if (map->cells == NULL) 00181 { 00182 map->size_x = width; 00183 map->size_y = height; 00184 map->cells = calloc(width * height, sizeof(map->cells[0])); 00185 } 00186 else 00187 { 00188 if (width != map->size_x || height != map->size_y) 00189 { 00190 //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); 00191 return -1; 00192 } 00193 } 00194 00195 // Read in the image 00196 for (j = height - 1; j >= 0; j--) 00197 { 00198 for (i = 0; i < width; i++) 00199 { 00200 ch = fgetc(file); 00201 00202 if (!MAP_VALID(map, i, j)) 00203 continue; 00204 00205 if (ch == 0) 00206 level = 0; 00207 else 00208 level = ch * 100 / 255 - 100; 00209 00210 cell = map->cells + MAP_INDEX(map, i, j); 00211 cell->wifi_levels[index] = level; 00212 } 00213 } 00214 00215 fclose(file); 00216 00217 return 0; 00218 } 00219 */ 00220 00221 /// @endcond