Fawkes API  Fawkes Development Version
map_store.c
1 
2 /***************************************************************************
3  * map_store.c: Global map storage functions
4  *
5  * Created: Thu May 24 18:48:43 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: Global map storage functions
31  * Author: Andrew Howard
32  * Date: 6 Feb 2003
33 **************************************************************************/
34 
35 #include <errno.h>
36 #include <math.h>
37 #include <stdio.h>
38 #include <stdlib.h>
39 #include <string.h>
40 
41 #include "map.h"
42 
43 /// @cond EXTERNAL
44 
45 ////////////////////////////////////////////////////////////////////////////
46 // Load an occupancy grid
47 int map_load_occ(map_t *map, const char *filename, double scale, int negate)
48 {
49  FILE *file;
50  char magic[3];
51  int i, j;
52  int ch, occ;
53  int width, height, depth;
54  map_cell_t *cell;
55 
56  // Open file
57  file = fopen(filename, "r");
58  if (file == NULL)
59  {
60  fprintf(stderr, "%s: %s\n", strerror(errno), filename);
61  return -1;
62  }
63 
64  // Read ppm header
65 
66  if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
67  {
68  fprintf(stderr, "incorrect image format; must be PGM/binary");
69  fclose(file);
70  return -1;
71  }
72 
73  // Ignore comments
74  while ((ch = fgetc(file)) == '#')
75  while (fgetc(file) != '\n');
76  ungetc(ch, file);
77 
78  // Read image dimensions
79  if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
80  {
81  fprintf(stderr, "Failed ot read image dimensions");
82  fclose(file);
83  return -1;
84  }
85 
86  // Allocate space in the map
87  if (map->cells == NULL)
88  {
89  map->scale = scale;
90  map->size_x = width;
91  map->size_y = height;
92  map->cells = calloc((size_t)width * height, sizeof(map->cells[0]));
93  }
94  else
95  {
96  if (width != map->size_x || height != map->size_y)
97  {
98  //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
99  fclose(file);
100  return -1;
101  }
102  }
103 
104  // Read in the image
105  for (j = height - 1; j >= 0; j--)
106  {
107  for (i = 0; i < width; i++)
108  {
109  ch = fgetc(file);
110 
111  // Black-on-white images
112  if (!negate)
113  {
114  if (ch < depth / 4)
115  occ = +1;
116  else if (ch > 3 * depth / 4)
117  occ = -1;
118  else
119  occ = 0;
120  }
121 
122  // White-on-black images
123  else
124  {
125  if (ch < depth / 4)
126  occ = -1;
127  else if (ch > 3 * depth / 4)
128  occ = +1;
129  else
130  occ = 0;
131  }
132 
133  if (!MAP_VALID(map, i, j))
134  continue;
135  cell = map->cells + MAP_INDEX(map, i, j);
136  cell->occ_state = occ;
137  }
138  }
139 
140  fclose(file);
141 
142  return 0;
143 }
144 
145 
146 ////////////////////////////////////////////////////////////////////////////
147 // Load a wifi signal strength map
148 /*
149 int map_load_wifi(map_t *map, const char *filename, int index)
150 {
151  FILE *file;
152  char magic[3];
153  int i, j;
154  int ch, level;
155  int width, height, depth;
156  map_cell_t *cell;
157 
158  // Open file
159  file = fopen(filename, "r");
160  if (file == NULL)
161  {
162  fprintf(stderr, "%s: %s\n", strerror(errno), filename);
163  return -1;
164  }
165 
166  // Read ppm header
167  fscanf(file, "%10s \n", magic);
168  if (strcmp(magic, "P5") != 0)
169  {
170  fprintf(stderr, "incorrect image format; must be PGM/binary");
171  return -1;
172  }
173 
174  // Ignore comments
175  while ((ch = fgetc(file)) == '#')
176  while (fgetc(file) != '\n');
177  ungetc(ch, file);
178 
179  // Read image dimensions
180  fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
181 
182  // Allocate space in the map
183  if (map->cells == NULL)
184  {
185  map->size_x = width;
186  map->size_y = height;
187  map->cells = calloc(width * height, sizeof(map->cells[0]));
188  }
189  else
190  {
191  if (width != map->size_x || height != map->size_y)
192  {
193  //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
194  return -1;
195  }
196  }
197 
198  // Read in the image
199  for (j = height - 1; j >= 0; j--)
200  {
201  for (i = 0; i < width; i++)
202  {
203  ch = fgetc(file);
204 
205  if (!MAP_VALID(map, i, j))
206  continue;
207 
208  if (ch == 0)
209  level = 0;
210  else
211  level = ch * 100 / 255 - 100;
212 
213  cell = map->cells + MAP_INDEX(map, i, j);
214  cell->wifi_levels[index] = level;
215  }
216  }
217 
218  fclose(file);
219 
220  return 0;
221 }
222 */
223 
224 /// @endcond