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


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Tue Nov 7 2017 06:02:33