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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:02