Fawkes API  Fawkes Development Version
map_store.c
00001 
00002 /***************************************************************************
00003  *  map_store.c: Global map storage functions
00004  *
00005  *  Created: Thu May 24 18:48:43 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 /**************************************************************************
00030  * Desc: Global map storage functions
00031  * Author: Andrew Howard
00032  * Date: 6 Feb 2003
00033 **************************************************************************/
00034 
00035 #include <errno.h>
00036 #include <math.h>
00037 #include <stdio.h>
00038 #include <stdlib.h>
00039 #include <string.h>
00040 
00041 #include "map.h"
00042 
00043 /// @cond EXTERNAL
00044 
00045 ////////////////////////////////////////////////////////////////////////////
00046 // Load an occupancy grid
00047 int map_load_occ(map_t *map, const char *filename, double scale, int negate)
00048 {
00049   FILE *file;
00050   char magic[3];
00051   int i, j;
00052   int ch, occ;
00053   int width, height, depth;
00054   map_cell_t *cell;
00055 
00056   // Open file
00057   file = fopen(filename, "r");
00058   if (file == NULL)
00059   {
00060     fprintf(stderr, "%s: %s\n", strerror(errno), filename);
00061     return -1;
00062   }
00063 
00064   // Read ppm header
00065   
00066   if ((fscanf(file, "%10s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
00067   {
00068     fprintf(stderr, "incorrect image format; must be PGM/binary");
00069     return -1;
00070   }
00071 
00072   // Ignore comments
00073   while ((ch = fgetc(file)) == '#')
00074     while (fgetc(file) != '\n');
00075   ungetc(ch, file);
00076 
00077   // Read image dimensions
00078   if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
00079   {
00080     fprintf(stderr, "Failed ot read image dimensions");
00081     return -1;
00082   }
00083 
00084   // Allocate space in the map
00085   if (map->cells == NULL)
00086   {
00087     map->scale = scale;
00088     map->size_x = width;
00089     map->size_y = height;
00090     map->cells = calloc(width * height, sizeof(map->cells[0]));
00091   }
00092   else
00093   {
00094     if (width != map->size_x || height != map->size_y)
00095     {
00096       //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
00097       return -1;
00098     }
00099   }
00100 
00101   // Read in the image
00102   for (j = height - 1; j >= 0; j--)
00103   {
00104     for (i = 0; i < width; i++)
00105     {
00106       ch = fgetc(file);
00107 
00108       // Black-on-white images
00109       if (!negate)
00110       {
00111         if (ch < depth / 4)
00112           occ = +1;
00113         else if (ch > 3 * depth / 4)
00114           occ = -1;
00115         else
00116           occ = 0;
00117       }
00118 
00119       // White-on-black images
00120       else
00121       {
00122         if (ch < depth / 4)
00123           occ = -1;
00124         else if (ch > 3 * depth / 4)
00125           occ = +1;
00126         else
00127           occ = 0;
00128       }
00129 
00130       if (!MAP_VALID(map, i, j))
00131         continue;
00132       cell = map->cells + MAP_INDEX(map, i, j);
00133       cell->occ_state = occ;
00134     }
00135   }
00136   
00137   fclose(file);
00138   
00139   return 0;
00140 }
00141 
00142 
00143 ////////////////////////////////////////////////////////////////////////////
00144 // Load a wifi signal strength map
00145 /*
00146 int map_load_wifi(map_t *map, const char *filename, int index)
00147 {
00148   FILE *file;
00149   char magic[3];
00150   int i, j;
00151   int ch, level;
00152   int width, height, depth;
00153   map_cell_t *cell;
00154 
00155   // Open file
00156   file = fopen(filename, "r");
00157   if (file == NULL)
00158   {
00159     fprintf(stderr, "%s: %s\n", strerror(errno), filename);
00160     return -1;
00161   }
00162 
00163   // Read ppm header
00164   fscanf(file, "%10s \n", magic);
00165   if (strcmp(magic, "P5") != 0)
00166   {
00167     fprintf(stderr, "incorrect image format; must be PGM/binary");
00168     return -1;
00169   }
00170 
00171   // Ignore comments
00172   while ((ch = fgetc(file)) == '#')
00173     while (fgetc(file) != '\n');
00174   ungetc(ch, file);
00175 
00176   // Read image dimensions
00177   fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
00178 
00179   // Allocate space in the map
00180   if (map->cells == NULL)
00181   {
00182     map->size_x = width;
00183     map->size_y = height;
00184     map->cells = calloc(width * height, sizeof(map->cells[0]));
00185   }
00186   else
00187   {
00188     if (width != map->size_x || height != map->size_y)
00189     {
00190       //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
00191       return -1;
00192     }
00193   }
00194 
00195   // Read in the image
00196   for (j = height - 1; j >= 0; j--)
00197   {
00198     for (i = 0; i < width; i++)
00199     {
00200       ch = fgetc(file);
00201 
00202       if (!MAP_VALID(map, i, j))
00203         continue;
00204 
00205       if (ch == 0)
00206         level = 0;
00207       else
00208         level = ch * 100 / 255 - 100;
00209 
00210       cell = map->cells + MAP_INDEX(map, i, j);
00211       cell->wifi_levels[index] = level;
00212     }
00213   }
00214   
00215   fclose(file);
00216 
00217   return 0;
00218 }
00219 */
00220 
00221 /// @endcond