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
47int 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/*
149int 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