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, "%10s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
67  {
68  fprintf(stderr, "incorrect image format; must be PGM/binary");
69  return -1;
70  }
71 
72  // Ignore comments
73  while ((ch = fgetc(file)) == '#')
74  while (fgetc(file) != '\n');
75  ungetc(ch, file);
76 
77  // Read image dimensions
78  if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
79  {
80  fprintf(stderr, "Failed ot read image dimensions");
81  return -1;
82  }
83 
84  // Allocate space in the map
85  if (map->cells == NULL)
86  {
87  map->scale = scale;
88  map->size_x = width;
89  map->size_y = height;
90  map->cells = calloc(width * height, sizeof(map->cells[0]));
91  }
92  else
93  {
94  if (width != map->size_x || height != map->size_y)
95  {
96  //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
97  return -1;
98  }
99  }
100 
101  // Read in the image
102  for (j = height - 1; j >= 0; j--)
103  {
104  for (i = 0; i < width; i++)
105  {
106  ch = fgetc(file);
107 
108  // Black-on-white images
109  if (!negate)
110  {
111  if (ch < depth / 4)
112  occ = +1;
113  else if (ch > 3 * depth / 4)
114  occ = -1;
115  else
116  occ = 0;
117  }
118 
119  // White-on-black images
120  else
121  {
122  if (ch < depth / 4)
123  occ = -1;
124  else if (ch > 3 * depth / 4)
125  occ = +1;
126  else
127  occ = 0;
128  }
129 
130  if (!MAP_VALID(map, i, j))
131  continue;
132  cell = map->cells + MAP_INDEX(map, i, j);
133  cell->occ_state = occ;
134  }
135  }
136 
137  fclose(file);
138 
139  return 0;
140 }
141 
142 
143 ////////////////////////////////////////////////////////////////////////////
144 // Load a wifi signal strength map
145 /*
146 int map_load_wifi(map_t *map, const char *filename, int index)
147 {
148  FILE *file;
149  char magic[3];
150  int i, j;
151  int ch, level;
152  int width, height, depth;
153  map_cell_t *cell;
154 
155  // Open file
156  file = fopen(filename, "r");
157  if (file == NULL)
158  {
159  fprintf(stderr, "%s: %s\n", strerror(errno), filename);
160  return -1;
161  }
162 
163  // Read ppm header
164  fscanf(file, "%10s \n", magic);
165  if (strcmp(magic, "P5") != 0)
166  {
167  fprintf(stderr, "incorrect image format; must be PGM/binary");
168  return -1;
169  }
170 
171  // Ignore comments
172  while ((ch = fgetc(file)) == '#')
173  while (fgetc(file) != '\n');
174  ungetc(ch, file);
175 
176  // Read image dimensions
177  fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
178 
179  // Allocate space in the map
180  if (map->cells == NULL)
181  {
182  map->size_x = width;
183  map->size_y = height;
184  map->cells = calloc(width * height, sizeof(map->cells[0]));
185  }
186  else
187  {
188  if (width != map->size_x || height != map->size_y)
189  {
190  //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
191  return -1;
192  }
193  }
194 
195  // Read in the image
196  for (j = height - 1; j >= 0; j--)
197  {
198  for (i = 0; i < width; i++)
199  {
200  ch = fgetc(file);
201 
202  if (!MAP_VALID(map, i, j))
203  continue;
204 
205  if (ch == 0)
206  level = 0;
207  else
208  level = ch * 100 / 255 - 100;
209 
210  cell = map->cells + MAP_INDEX(map, i, j);
211  cell->wifi_levels[index] = level;
212  }
213  }
214 
215  fclose(file);
216 
217  return 0;
218 }
219 */
220 
221 /// @endcond