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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36